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 @@
+
+
+
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 @@
+
+
+