From 5ded8c6dfc678e9f32e666a5bb0560b42cb76635 Mon Sep 17 00:00:00 2001 From: sambas Date: Thu, 28 Mar 2013 18:29:26 +0200 Subject: [PATCH] VTOL path fixes Flight mode fixes POI mode AutoLand Tests Simple stabilization Shortest way yaw fix GCS map and pathplanner fixes --- flight/Libraries/CoordinateConversions.c | 6 +- flight/Libraries/paths.c | 29 +++- flight/Modules/ManualControl/manualcontrol.c | 53 +++++- flight/Modules/PathPlanner/pathplanner.c | 15 +- .../Modules/Stabilization/inc/relay_tuning.h | 2 +- flight/Modules/Stabilization/relay_tuning.c | 9 +- flight/Modules/Stabilization/stabilization.c | 15 +- .../VtolPathFollower/vtolpathfollower.c | 157 ++++++++++++++++-- flight/targets/RevoMini/UAVObjects.inc | 2 + flight/targets/Revolution/UAVObjects.inc | 2 + .../opmapcontrol/src/mapwidget/gpsitem.cpp | 14 +- .../src/mapwidget/opmapwidget.cpp | 2 + .../opmap/opmap_edit_waypoint_dialog.cpp | 23 +++ .../src/plugins/opmap/opmapgadgetwidget.cpp | 8 +- .../src/plugins/opmap/widgetdelegates.cpp | 2 + .../src/plugins/opmap/widgetdelegates.h | 5 +- .../src/plugins/uavobjects/uavobjects.pro | 4 + .../uavobjectdefinition/poilearnsettings.xml | 10 ++ shared/uavobjectdefinition/poilocation.xml | 12 ++ 19 files changed, 315 insertions(+), 55 deletions(-) create mode 100644 shared/uavobjectdefinition/poilearnsettings.xml create mode 100644 shared/uavobjectdefinition/poilocation.xml diff --git a/flight/Libraries/CoordinateConversions.c b/flight/Libraries/CoordinateConversions.c index fc5032e4e..5347b6edb 100644 --- a/flight/Libraries/CoordinateConversions.c +++ b/flight/Libraries/CoordinateConversions.c @@ -148,9 +148,9 @@ void RPY2Quaternion(const float rpy[3], float q[4]) float phi, theta, psi; float cphi, sphi, ctheta, stheta, cpsi, spsi; - phi = DEG2RAD * rpy[0] / 2; - theta = DEG2RAD * rpy[1] / 2; - psi = DEG2RAD * rpy[2] / 2; + phi = DEG2RAD * rpy[0] / 2.0f; + theta = DEG2RAD * rpy[1] / 2.0f; + psi = DEG2RAD * rpy[2] / 2.0f; cphi = cosf(phi); sphi = sinf(phi); ctheta = cosf(theta); diff --git a/flight/Libraries/paths.c b/flight/Libraries/paths.c index 353ae9b10..d9b7fc389 100644 --- a/flight/Libraries/paths.c +++ b/flight/Libraries/paths.c @@ -176,6 +176,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin float radius_north, radius_east, diff_north, diff_east; float radius,cradius; float normal[2]; + float progress; + float a_diff, a_radius; // Radius radius_north = end_point[0] - start_point[0]; @@ -185,8 +187,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin diff_north = cur_point[0] - end_point[0]; diff_east = cur_point[1] - end_point[1]; - radius = sqrtf( radius_north * radius_north + radius_east * radius_east ); - cradius = sqrtf( diff_north * diff_north + diff_east * diff_east ); + radius = sqrtf( powf(radius_north,2) + powf(radius_east,2) ); + cradius = sqrtf( powf(diff_north,2) + powf(diff_east,2) ); if (cradius < 1e-6) { // cradius is zero, just fly somewhere and make sure correction is still a normal @@ -209,7 +211,28 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin normal[1] = -diff_north / cradius; } - status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east); + + // normalize progress to 0..1 + a_diff = atan2f( diff_north, diff_east); + a_radius = atan2f( radius_north, radius_east); + + if(a_diff<0) + a_diff+=2*M_PI; + if(a_radius<0) + a_radius+=2*M_PI; + + progress = (a_diff - a_radius + M_PI) / (2 * M_PI); + + if(progress<0) + progress+=1.0; + else if(progress>=1) + progress-=1.0; + + if(clockwise) + { + progress=1-progress; + } + status->fractional_progress = progress; // error is current radius minus wanted radius - positive if too close status->error = radius - cradius; diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 4a4481566..44ca51a06 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -93,6 +93,7 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; // Private functions static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void updateLandDesired(ManualControlCommandData * cmd, bool changed); static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed); static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home); static void processFlightMode(ManualControlSettingsData * settings, float flightMode); @@ -455,11 +456,18 @@ static void manualControlTask(void *parameters) altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POI: updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); break; case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // No need to call anything. This just avoids errors. + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; default: AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); } @@ -728,12 +736,12 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10; + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.StartingVelocity=1; pathDesired.EndingVelocity=0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; @@ -751,6 +759,38 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool } } + +static void updateLandDesired(ManualControlCommandData * cmd, bool changed) +{ + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; + + thisSysTime = xTaskGetTickCount(); + dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; + lastSysTime = thisSysTime; + + PositionActualData positionActual; + PositionActualGet(&positionActual); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + if(changed) { + // After not being in this mode for a while init at current height + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.StartingVelocity=1; + pathDesired.EndingVelocity=0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + } + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; + PathDesiredSet(&pathDesired); +} + /** * @brief Update the altitude desired to current altitude when * enabled and enable altitude mode for stabilization @@ -804,6 +844,11 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); } +static void updateLandDesired(ManualControlCommandData * cmd, bool changed) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} + static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c index de2fe9184..f6ddbcd44 100644 --- a/flight/Modules/PathPlanner/pathplanner.c +++ b/flight/Modules/PathPlanner/pathplanner.c @@ -238,20 +238,25 @@ void updatePathDesired(UAVObjEvent * ev) { pathDesired.UID = waypointActive.Index; if(waypointActive.Index == 0) { + PositionActualData positionActual; + PositionActualGet(&positionActual); // First waypoint has itself as start point (used to be home position but that proved dangerous when looping) - pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; + /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ + pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.StartingVelocity = pathDesired.EndingVelocity; } else { // Get previous waypoint as start point WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; - pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; - pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; + pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; + pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; + pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); diff --git a/flight/Modules/Stabilization/inc/relay_tuning.h b/flight/Modules/Stabilization/inc/relay_tuning.h index 9044986c5..2fd69cafa 100644 --- a/flight/Modules/Stabilization/inc/relay_tuning.h +++ b/flight/Modules/Stabilization/inc/relay_tuning.h @@ -36,4 +36,4 @@ int stabilization_relay_rate(float err, float *output, int axis, bool reinit); -#endif \ No newline at end of file +#endif diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index 5d53aa7d3..02081f427 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -32,15 +32,11 @@ */ #include "openpilot.h" +#include "stabilization.h" #include "relaytuning.h" #include "relaytuningsettings.h" #include "sin_lookup.h" -//! Private variables -static const int SIN_RESOLUTION = 180; - -#define MAX_AXES 3 - /** * Apply a step function for the stabilization controller and monitor the * result @@ -108,7 +104,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) if(phase >= 360) phase = 0; accum_sin += sin_lookup_deg(phase) * error; - accum_cos += sin_lookup_deg(phase + 90) * error; + accum_cos += cos_lookup_deg(phase) * error; accumulated ++; // Make sure we've had enough time since last transition then check for a change in the output @@ -148,4 +144,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) return 0; } - diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 52ccc8f29..6d24eaa9a 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -53,6 +53,9 @@ #include "relay_tuning.h" #include "virtualflybar.h" +// Includes for various stabilization algorithms +#include "relay_tuning.h" + // Private constants #define MAX_QUEUE_SIZE 1 @@ -123,7 +126,6 @@ int32_t StabilizationInitialize() #ifdef DIAG_RATEDESIRED RateDesiredInitialize(); #endif - // Code required for relay tuning sin_lookup_initalize(); RelayTuningSettingsInitialize(); @@ -211,7 +213,12 @@ static void stabilizationTask(void* parameters) float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, stabDesired.Pitch - attitudeActual.Pitch, stabDesired.Yaw - attitudeActual.Yaw}; - local_error[2] = fmodf(local_error[2] + 180, 360) - 180; + // find shortest way + float modulo = fmodf(local_error[2] + 180.0f, 360.0f); + if(modulo<0) + local_error[2] = modulo + 180.0f; + else + local_error[2] = modulo - 180.0f; #endif float gyro_filtered[3]; @@ -277,6 +284,7 @@ static void stabilizationTask(void* parameters) stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &settings); break; + case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { if (reinit) @@ -292,6 +300,7 @@ static void stabilizationTask(void* parameters) break; } + case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) pids[PID_RATE_ROLL + i].iAccumulator = 0; @@ -307,7 +316,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); } - rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); + rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f); diff --git a/flight/Modules/VtolPathFollower/vtolpathfollower.c b/flight/Modules/VtolPathFollower/vtolpathfollower.c index 3781f492f..7a6182640 100644 --- a/flight/Modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/Modules/VtolPathFollower/vtolpathfollower.c @@ -70,30 +70,40 @@ #include "velocityactual.h" #include "CoordinateConversions.h" +#include "cameradesired.h" +#include "poilearnsettings.h" +#include "poilocation.h" +#include "accessorydesired.h" + + // Private constants #define MAX_QUEUE_SIZE 4 #define STACK_SIZE_BYTES 1548 #define TASK_PRIORITY (tskIDLE_PRIORITY+2) #define F_PI 3.14159265358979323846f #define DEG2RAD (F_PI/180.0f) +#define RAD2DEG(rad) ((rad)*(180.0f/F_PI)) // Private types // Private variables static xTaskHandle pathfollowerTaskHandle; static PathDesiredData pathDesired; +static PathStatusData pathStatus; static VtolPathFollowerSettingsData vtolpathfollowerSettings; // Private functions static void vtolPathFollowerTask(void *parameters); static void SettingsUpdatedCb(UAVObjEvent * ev); static void updateNedAccel(); +static void updatePOIBearing(); static void updatePathVelocity(); static void updateEndpointVelocity(); static void updateFixedAttitude(float* attitude); -static void updateVtolDesiredAttitude(); +static void updateVtolDesiredAttitude(bool yaw_attitude); static float bound(float val, float min, float max); static bool vtolpathfollower_enabled; +static void accessoryUpdated(UAVObjEvent* ev); /** * Initialise the module, called on startup @@ -126,6 +136,10 @@ int32_t VtolPathFollowerInitialize() PathDesiredInitialize(); PathStatusInitialize(); VelocityDesiredInitialize(); + CameraDesiredInitialize(); + AccessoryDesiredInitialize(); + PoiLearnSettingsInitialize(); + PoiLocationInitialize(); vtolpathfollower_enabled = true; } else { vtolpathfollower_enabled = false; @@ -152,12 +166,12 @@ static void vtolPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; - PathStatusData pathStatus; portTickType lastUpdateTime; VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); + AccessoryDesiredConnectCallback(accessoryUpdated); VtolPathFollowerSettingsGet(&vtolpathfollowerSettings); PathDesiredGet(&pathDesired); @@ -200,11 +214,12 @@ static void vtolPathFollowerTask(void *parameters) // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { updateEndpointVelocity(); - updateVtolDesiredAttitude(); + updateVtolDesiredAttitude(false); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); } else { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); @@ -216,15 +231,11 @@ static void vtolPathFollowerTask(void *parameters) switch(pathDesired.Mode) { // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly case PATHDESIRED_MODE_FLYENDPOINT: - updateEndpointVelocity(); - updateVtolDesiredAttitude(); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); - break; case PATHDESIRED_MODE_FLYVECTOR: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: updatePathVelocity(); - updateVtolDesiredAttitude(); + updateVtolDesiredAttitude(false); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK); break; case PATHDESIRED_MODE_FIXEDATTITUDE: @@ -239,6 +250,16 @@ static void vtolPathFollowerTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); break; } + PathStatusSet(&pathStatus); + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(true); + updatePOIBearing(); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR); + } break; default: // Be cleaner and get rid of global variables @@ -262,6 +283,54 @@ static void vtolPathFollowerTask(void *parameters) } } +/** + * Compute bearing and elevation between current position and POI + */ +static void updatePOIBearing() +{ + PositionActualData positionActual; + PositionActualGet(&positionActual); + CameraDesiredData cameraDesired; + CameraDesiredGet(&cameraDesired); + StabilizationDesiredData stabDesired; + StabilizationDesiredGet(&stabDesired); + PoiLocationData poi; + PoiLocationGet(&poi); + //use poi here + //HomeLocationData poi; + //HomeLocationGet (&poi); + + float dLoc[3]; + float yaw=0; + float elevation=0; + + dLoc[0]=positionActual.North-poi.North; + dLoc[1]=positionActual.East-poi.East; + dLoc[2]=positionActual.Down-poi.Down; + + if(dLoc[1]<0) + yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))+180; + else + yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))-180; + + // distance + float distance = sqrtf(powf(dLoc[0],2)+powf(dLoc[1],2)); + + //not above + if(distance!=0) { + //You can feed this into camerastabilization + elevation=RAD2DEG(atan2f(dLoc[2],distance)); + } + stabDesired.Yaw=yaw; + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + cameraDesired.Yaw=yaw; + cameraDesired.PitchOrServo2=elevation; + + CameraDesiredSet(&cameraDesired); + StabilizationDesiredSet(&stabDesired); +} + + /** * Compute desired velocity from the current position and path * @@ -280,11 +349,31 @@ static void updatePathVelocity() struct path_status progress; path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); - - float groundspeed = pathDesired.StartingVelocity + - (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1); - if(progress.fractional_progress > 1) - groundspeed = 0; + + float groundspeed; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_DRIVECIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + case PATHDESIRED_MODE_DRIVECIRCLELEFT: + groundspeed = pathDesired.EndingVelocity; + break; + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_DRIVEENDPOINT: + groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * + bound(progress.fractional_progress,0,1); + if(progress.fractional_progress > 1) + groundspeed = 0; + break; + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_DRIVEVECTOR: + default: + groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * + bound(progress.fractional_progress,0,1); + if(progress.fractional_progress > 1) + groundspeed = 0; + break; + } VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; @@ -314,6 +403,10 @@ static void updatePathVelocity() -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + VelocityDesiredSet(&velocityDesired); } @@ -332,7 +425,7 @@ void updateEndpointVelocity() PositionActualGet(&positionActual); VelocityDesiredGet(&velocityDesired); - + float northError; float eastError; float downError; @@ -435,7 +528,7 @@ static void updateFixedAttitude(float* attitude) * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ -static void updateVtolDesiredAttitude() +static void updateVtolDesiredAttitude(bool yaw_attitude) { float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f; @@ -501,7 +594,6 @@ static void updateVtolDesiredAttitude() // Testing code - refactor into manual control command ManualControlCommandData manualControlData; ManualControlCommandGet(&manualControlData); - stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; // Compute desired north command northError = velocityDesired.North - northVel; @@ -554,8 +646,12 @@ static void updateVtolDesiredAttitude() stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - + if(yaw_attitude) { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; + } else { + stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; + } StabilizationDesiredSet(&stabDesired); } @@ -618,3 +714,28 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) PathDesiredGet(&pathDesired); } +static void accessoryUpdated(UAVObjEvent* ev) +{ + if (ev->obj != AccessoryDesiredHandle()) + return; + + PositionActualData positionActual; + PositionActualGet(&positionActual); + AccessoryDesiredData accessory; + PoiLearnSettingsData poiLearn; + PoiLearnSettingsGet(&poiLearn); + PoiLocationData poi; + PoiLocationGet(&poi); + if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) { + if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { + if(accessory.AccessoryVal<-0.5f) + { + poi.North=positionActual.North; + poi.East=positionActual.East; + poi.Down=positionActual.Down; + PoiLocationSet(&poi); + } + } + } +} + diff --git a/flight/targets/RevoMini/UAVObjects.inc b/flight/targets/RevoMini/UAVObjects.inc index 56f708ebe..f750288c0 100644 --- a/flight/targets/RevoMini/UAVObjects.inc +++ b/flight/targets/RevoMini/UAVObjects.inc @@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRCFILENAMES += txpidsettings diff --git a/flight/targets/Revolution/UAVObjects.inc b/flight/targets/Revolution/UAVObjects.inc index 1d7b592f8..f025cd9ea 100644 --- a/flight/targets/Revolution/UAVObjects.inc +++ b/flight/targets/Revolution/UAVObjects.inc @@ -94,6 +94,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += poilocation +UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpu6000settings UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c ) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp index 07d75da2a..358ba5ed7 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp @@ -28,7 +28,7 @@ #include "gpsitem.h" namespace mapcontrol { - GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(5),traildistance(50),autosetreached(true) + GPSItem::GPSItem(MapGraphicItem* map,OPMapWidget* parent,QString uavPic):map(map),mapwidget(parent),showtrail(true),showtrailline(true),trailtime(1),traildistance(2),autosetreached(true) ,autosetdistance(100) { pic.load(uavPic); @@ -42,8 +42,10 @@ namespace mapcontrol trailLine=new QGraphicsItemGroup(this); trailLine->setParentItem(map); this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); + setCacheMode(QGraphicsItem::ItemCoordinateCache); mapfollowtype=UAVMapFollowType::None; - trailtype=UAVTrailType::ByDistance; + //trailtype=UAVTrailType::ByDistance; + trailtype=UAVTrailType::ByTimeElapsed; timer.start(); connect(map,SIGNAL(childRefreshPosition()),this,SLOT(RefreshPos())); connect(map,SIGNAL(childSetOpacity(qreal)),this,SLOT(setOpacitySlot(qreal))); @@ -74,12 +76,12 @@ namespace mapcontrol { if(timer.elapsed()>trailtime*1000) { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); + TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); trail->addToGroup(ob); connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); if(!lasttrailline.IsEmpty()) { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); + TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); trailLine->addToGroup(obj); connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); } @@ -92,12 +94,12 @@ namespace mapcontrol { if(qAbs(internals::PureProjection::DistanceBetweenLatLng(lastcoord,position)*1000)>traildistance) { - TrailItem * ob=new TrailItem(position,altitude,Qt::green,map); + TrailItem * ob=new TrailItem(position,altitude,Qt::red,map); trail->addToGroup(ob); connect(this,SIGNAL(setChildPosition()),ob,SLOT(setPosSLOT())); if(!lasttrailline.IsEmpty()) { - TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::red,map); + TrailLineItem * obj=new TrailLineItem(lasttrailline,position,Qt::green,map); trailLine->addToGroup(obj); connect(this,SIGNAL(setChildLine()),obj,SLOT(setLineSlot())); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index bdc60225d..ea477adde 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -80,6 +80,7 @@ namespace mapcontrol if(GPS!=0) { + GPS->DeleteTrail(); delete GPS; GPS=0; } @@ -93,6 +94,7 @@ namespace mapcontrol { GPS=new GPSItem(map,this); GPS->setParentItem(map); + GPS->setOpacity(overlayOpacity); setOverlayOpacity(overlayOpacity); } } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index 2aef32311..57dbaa550 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -200,6 +200,17 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets() ui->dsb_condParam4->setVisible(false); ui->condParam1->setText("Relative Distance(0=complete,1=just starting)"); break; + case MapDataDelegate::ENDCONDITION_BELOWERROR: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(false); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(false); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("error margin (in m)"); + break; case MapDataDelegate::ENDCONDITION_ABOVEALTITUDE: ui->condParam1->setVisible(true); ui->condParam2->setVisible(false); @@ -211,6 +222,18 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets() ui->dsb_condParam4->setVisible(false); ui->condParam1->setText("Altitude in meters (negative)"); break; + case MapDataDelegate::ENDCONDITION_ABOVESPEED: + ui->condParam1->setVisible(true); + ui->condParam2->setVisible(true); + ui->condParam3->setVisible(false); + ui->condParam4->setVisible(false); + ui->dsb_condParam1->setVisible(true); + ui->dsb_condParam2->setVisible(true); + ui->dsb_condParam3->setVisible(false); + ui->dsb_condParam4->setVisible(false); + ui->condParam1->setText("Speed in meters/second"); + ui->condParam2->setText("flag: 0=groundspeed 1=airspeed"); + break; case MapDataDelegate::ENDCONDITION_POINTINGTOWARDSNEXT: ui->condParam1->setVisible(true); ui->condParam2->setVisible(false); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 0d9639e38..223343ec4 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -24,6 +24,7 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#define USE_PATHPLANNER #include "opmapgadgetwidget.h" #include "ui_opmap_widget.h" @@ -546,8 +547,8 @@ void OPMapGadgetWidget::closeEvent(QCloseEvent *event) */ void OPMapGadgetWidget::updatePosition() { - double uav_latitude, uav_longitude, uav_altitude, uav_yaw; - double gps_latitude, gps_longitude, gps_altitude, gps_heading; + double uav_latitude, uav_longitude, uav_altitude, uav_yaw; + double gps_latitude, gps_longitude, gps_altitude, gps_heading; internals::PointLatLng uav_pos; internals::PointLatLng gps_pos; @@ -581,7 +582,7 @@ void OPMapGadgetWidget::updatePosition() gps_longitude = gpsPositionData.Longitude; gps_altitude = gpsPositionData.Altitude; - gps_pos = internals::PointLatLng(gps_latitude, gps_longitude); + gps_pos = internals::PointLatLng(gps_latitude*1e-7, gps_longitude*1e-7); //********************** // get the current position and heading estimates @@ -643,6 +644,7 @@ void OPMapGadgetWidget::updatePosition() { m_map->GPS->SetUAVPos(gps_pos, gps_altitude); // set the maps GPS position m_map->GPS->SetUAVHeading(gps_heading); // set the maps GPS heading + m_map->GPS->update(); } m_map->UAV->updateTextOverlay(); m_map->UAV->update(); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp index 7bd385842..fd44550a6 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp @@ -121,7 +121,9 @@ void MapDataDelegate::loadComboBox(QComboBox *combo, flightDataModel::pathPlanDa combo->addItem("Timeout",ENDCONDITION_TIMEOUT); combo->addItem("Distance to tgt",ENDCONDITION_DISTANCETOTARGET); combo->addItem("Leg remaining",ENDCONDITION_LEGREMAINING); + combo->addItem("Below Error",ENDCONDITION_BELOWERROR); combo->addItem("Above Altitude",ENDCONDITION_ABOVEALTITUDE); + combo->addItem("Above Speed",ENDCONDITION_ABOVESPEED); combo->addItem("Pointing towards next",ENDCONDITION_POINTINGTOWARDSNEXT); combo->addItem("Python script",ENDCONDITION_PYTHONSCRIPT); combo->addItem("Immediate",ENDCONDITION_IMMEDIATE); diff --git a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h index ae29bc975..53aac4887 100644 --- a/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h +++ b/ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h @@ -41,8 +41,9 @@ class MapDataDelegate : public QItemDelegate MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, MODE_DRIVECIRCLERIGHT=7, MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; typedef enum { ENDCONDITION_NONE=0, ENDCONDITION_TIMEOUT=1, ENDCONDITION_DISTANCETOTARGET=2, - ENDCONDITION_LEGREMAINING=3, ENDCONDITION_ABOVEALTITUDE=4, ENDCONDITION_POINTINGTOWARDSNEXT=5, - ENDCONDITION_PYTHONSCRIPT=6, ENDCONDITION_IMMEDIATE=7 } EndConditionOptions; + ENDCONDITION_LEGREMAINING=3, ENDCONDITION_BELOWERROR=4, ENDCONDITION_ABOVEALTITUDE=5, + ENDCONDITION_ABOVESPEED=6, ENDCONDITION_POINTINGTOWARDSNEXT=7, ENDCONDITION_PYTHONSCRIPT=8, + ENDCONDITION_IMMEDIATE=9 } EndConditionOptions; typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYPOINT=1, COMMAND_ONCONDITIONJUMPWAYPOINT=2, COMMAND_ONNOTCONDITIONJUMPWAYPOINT=3, COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT=4 } CommandOptions; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 5b61364fc..42536c390 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -95,6 +95,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/txpidsettings.h \ $$UAVOBJECT_SYNTHETICS/cameradesired.h \ $$UAVOBJECT_SYNTHETICS/faultsettings.h \ + $$UAVOBJECT_SYNTHETICS/poilearnsettings.h \ + $$UAVOBJECT_SYNTHETICS/poilocation.h \ $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ @@ -175,6 +177,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \ $$UAVOBJECT_SYNTHETICS/cameradesired.cpp \ $$UAVOBJECT_SYNTHETICS/faultsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/poilearnsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/poilocation.cpp \ $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ diff --git a/shared/uavobjectdefinition/poilearnsettings.xml b/shared/uavobjectdefinition/poilearnsettings.xml new file mode 100644 index 000000000..281d064e3 --- /dev/null +++ b/shared/uavobjectdefinition/poilearnsettings.xml @@ -0,0 +1,10 @@ + + + Settings for the @ref Point of Interest feature + + + + + + + diff --git a/shared/uavobjectdefinition/poilocation.xml b/shared/uavobjectdefinition/poilocation.xml new file mode 100644 index 000000000..70022a86a --- /dev/null +++ b/shared/uavobjectdefinition/poilocation.xml @@ -0,0 +1,12 @@ + + + Contains the current Point of interest relative to @ref HomeLocation + + + + + + + + +