rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

VTOL path fixes

Browse Source 
Flight mode fixes
POI mode
AutoLand Tests
Simple stabilization Shortest way yaw fix
GCS map and pathplanner fixes
This commit is contained in:
sambas 2013-03-28 18:29:26 +02:00
parent 77cae9ffc3
commit 5ded8c6dfc
19 changed files with 315 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/Libraries/CoordinateConversions.c
View File

@ -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);

29
flight/Libraries/paths.c
View File

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

53
flight/Modules/ManualControl/manualcontrol.c
View File

@ -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);

15
flight/Modules/PathPlanner/pathplanner.c
View File

@ -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);

2
flight/Modules/Stabilization/inc/relay_tuning.h
View File

@ -36,4 +36,4 @@
int stabilization_relay_rate(float err, float *output, int axis, bool reinit);
#endif
#endif

9
flight/Modules/Stabilization/relay_tuning.c
View File

@ -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;
}

15
flight/Modules/Stabilization/stabilization.c
View File

@ -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);

157
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -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);
}
}
}
}

2
flight/targets/RevoMini/UAVObjects.inc
View File

@ -89,6 +89,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
UAVOBJSRCFILENAMES += altitudeholddesired
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += poilocation
UAVOBJSRCFILENAMES += poilearnsettings
UAVOBJSRCFILENAMES += mpu6000settings
UAVOBJSRCFILENAMES += txpidsettings

2
flight/targets/Revolution/UAVObjects.inc
View File

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

14
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/gpsitem.cpp
View File

@ -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()));
}

2
ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp
View File

@ -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);
}
}

23
ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp
View File

@ -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);

8
ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp
View File

@ -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();

2
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp
View File

@ -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);

5
ground/openpilotgcs/src/plugins/opmap/widgetdelegates.h
View File

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

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

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

10
shared/uavobjectdefinition/poilearnsettings.xml Normal file
View File

@ -0,0 +1,10 @@
<xml>
<object name="PoiLearnSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Point of Interest feature</description>
<field name="Input" units="channel" type="enum" elements="1" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

12
shared/uavobjectdefinition/poilocation.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="PoiLocation" singleinstance="true" settings="false">
<description>Contains the current Point of interest relative to @ref HomeLocation</description>
<field name="North" units="m" type="float" elements="1"/>
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>