rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/new_navigation' into revo-next

Browse Source
This commit is contained in:
Corvus Corax 2012-06-02 22:30:44 +02:00
commit ab28686976
47 changed files with 3564 additions and 322 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/Libraries/inc/paths.h
View File

@ -34,6 +34,6 @@ struct path_status {
float path_direction[2];
};
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status);
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode);
#endif

169
flight/Libraries/paths.c
View File

@ -27,6 +27,89 @@
#include "pios.h"
#include "paths.h"
#include "uavobjectmanager.h" // <--.
#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
// no direct UAVObject usage allowed in this file
// private functions
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] cur_point Current location
* @param[in] mode Path following mode
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
{
switch(mode) {
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
break;
}
}
/**
* @brief Compute progress towards endpoint. Deviation equals distance
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path, dist_diff;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_diff < 1e-6 ) {
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->error = dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
}
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
@ -34,10 +117,10 @@
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status)
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path2;
float dist_path;
float dot;
float normal[2];
@ -50,21 +133,22 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
diff_east = cur_point[1] - start_point[1];
dot = path_north * diff_north + path_east * diff_east;
dist_path2 = path_north * path_north + path_east * path_east;
dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_path2 < 1e-3) {
if(dist_path < 1e-6) {
// if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint( start_point, end_point, cur_point, status );
status->fractional_progress = 1;
status->error = 0;
status->correction_direction[0] = status->correction_direction[1] = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
// Compute the normal to the path
normal[0] = -path_east / sqrtf(dist_path2);
normal[1] = path_north / sqrtf(dist_path2);
normal[0] = -path_east / dist_path;
normal[1] = path_north / dist_path;
status->fractional_progress = dot / dist_path2;
status->fractional_progress = dot / (dist_path * dist_path);
status->error = normal[0] * diff_north + normal[1] * diff_east;
// Compute direction to correct error
@ -75,9 +159,68 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
status->error = fabs(status->error);
// Compute direction to travel
status->path_direction[0] = path_north / sqrtf(dist_path2);
status->path_direction[1] = path_east / sqrtf(dist_path2);
status->path_direction[0] = path_north / dist_path;
status->path_direction[1] = path_east / dist_path;
}
/**
* @brief Compute progress along circular path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Center point
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
{
float radius_north, radius_east, diff_north, diff_east;
float radius,cradius;
float normal[2];
// Radius
radius_north = end_point[0] - start_point[0];
radius_east = end_point[1] - start_point[1];
// Current location relative to center
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 );
if (cradius < 1e-6) {
// cradius is zero, just fly somewhere and make sure correction is still a normal
status->fractional_progress = 1;
status->error = radius;
status->correction_direction[0] = 0;
status->correction_direction[1] = 1;
status->path_direction[0] = 1;
status->path_direction[1] = 0;
return;
}
if (clockwise) {
// Compute the normal to the radius clockwise
normal[0] = -diff_east / cradius;
normal[1] = diff_north / cradius;
} else {
// Compute the normal to the radius counter clockwise
normal[0] = diff_east / cradius;
normal[1] = -diff_north / cradius;
}
status->fractional_progress = (clockwise?1:-1) * atan2f( diff_north, diff_east) - atan2f( radius_north, radius_east);
// error is current radius minus wanted radius - positive if too close
status->error = radius - cradius;
// Compute direction to correct error
status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
// Compute direction to travel
status->path_direction[0] = normal[0];
status->path_direction[1] = normal[1];
status->error = fabs(status->error);
}

643
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c Normal file
View File

@ -0,0 +1,643 @@
/**
******************************************************************************
*
* @file fixedwingpathfollower.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input object: ActiveWaypoint
* Input object: PositionActual
* Input object: ManualControlCommand
* Output object: AttitudeDesired
*
* This module will periodically update the value of the AttitudeDesired object.
*
* The module executes in its own thread in this example.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "openpilot.h"
#include "paths.h"
#include "accels.h"
#include "hwsettings.h"
#include "attitudeactual.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "baroairspeed.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "fixedwingpathfollowersettings.h"
#include "fixedwingpathfollowerstatus.h"
#include "homelocation.h"
#include "nedposition.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "systemsettings.h"
#include "velocitydesired.h"
#include "velocityactual.h"
#include "CoordinateConversions.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 RAD2DEG (180.0f/F_PI)
#define DEG2RAD (F_PI/180.0f)
#define GEE 9.81f
// Private types
// Private variables
static bool followerEnabled = false;
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static PathStatusData pathStatus;
static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
// Private functions
static void pathfollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void baroAirspeedUpdatedCb(UAVObjEvent * ev);
static float bound(float val, float min, float max);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerStart()
{
if (followerEnabled) {
// Start main task
xTaskCreate(pathfollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t FixedWingPathFollowerInitialize()
{
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
followerEnabled = true;
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
BaroAirspeedInitialize();
} else {
followerEnabled = false;
}
return 0;
}
MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart)
static float northVelIntegral = 0;
static float eastVelIntegral = 0;
static float downVelIntegral = 0;
static float courseIntegral = 0;
static float speedIntegral = 0;
static float accelIntegral = 0;
static float powerIntegral = 0;
// correct speed by measured airspeed
static float baroAirspeedBias = 0;
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
BaroAirspeedConnectCallback(baroAirspeedUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
{
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch(pathDesired.Mode) {
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
courseIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
break;
}
PathStatusSet(&pathStatus);
}
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
// look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
float cur[3] = {positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward),
positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward),
positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward)
};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed;
float altitudeSetpoint;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End[2];
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
break;
}
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * bound(groundspeed,1e-6,groundspeed);
velocityDesired.East = progress.path_direction[1] * bound(groundspeed,1e-6,groundspeed);
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
// calculate correction - can also be zero if correction vector is 0 or no error present
velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed;
float downError = altitudeSetpoint - positionActual.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float* attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
AccelsData accels;
FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
StabilizationSettingsData stabSettings;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
float groundspeedActual;
float groundspeedDesired;
float airspeedActual;
float airspeedDesired;
float speedError;
float accelActual;
float accelDesired;
float accelError;
float accelCommand;
float pitchCommand;
float climbspeedDesired;
float climbspeedError;
float powerError;
float powerCommand;
float courseError;
float courseCommand;
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
AccelsGet(&accels);
StabilizationSettingsGet(&stabSettings);
/**
* Compute speed error (required for throttle and pitch)
*/
// Current ground speed
groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
airspeedActual = groundspeedActual + baroAirspeedBias;
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
airspeedDesired = bound( groundspeedDesired + baroAirspeedBias,
fixedwingpathfollowerSettings.AirSpeedMin,
fixedwingpathfollowerSettings.AirSpeedMax);
// Airspeed error
speedError = airspeedDesired - ( airspeedActual );
// Vertical error
climbspeedDesired = bound (
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
climbspeedError = climbspeedDesired - velocityActual.Down;
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - baroAirspeedBias <= 0 ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0;
}
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0;
}
if (airspeedActual<1e-6) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
return 0;
}
/**
* Compute desired throttle command
*/
// compute desired power
powerError = -climbspeedError +
bound (
(speedError/airspeedActual) * fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP],
-fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX]
);
powerIntegral = bound(powerIntegral + powerError * dT * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
powerIntegral) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL];
// prevent integral running out of bounds
if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
powerIntegral = bound(
powerIntegral -
( powerCommand
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]),
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
}
if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
powerIntegral = bound(
powerIntegral -
( powerCommand
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]),
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN];
}
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_POWER] = powerError;
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_POWER] = powerIntegral;
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_POWER] = powerCommand;
// set throttle
stabDesired.Throttle = powerCommand;
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
&& velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up
&& speedError > 0 // we are too slow already
) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
&& velocityActual.Down < 0 // we ARE going up
&& climbspeedDesired > 0 // we WANT to go down
&& speedError < 0 // we are too fast already
) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// compute desired acceleration
accelDesired = bound( (speedError/airspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_SPEED] = (speedError/airspeedActual);
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_SPEED] = 0.0f;
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_SPEED] = accelDesired;
// exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster
accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE);
accelError = accelDesired - accelActual;
accelIntegral = bound(accelIntegral + accelError * dT * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT],
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]);
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] +
accelIntegral);
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_ACCEL] = accelError;
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_ACCEL] = accelIntegral;
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_ACCEL] = accelCommand;
// compute desired pitch
pitchCommand= -accelCommand + bound ( (-climbspeedError/airspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
pitchCommand,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
&& velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up
&& speedError < 0 // we are too fast already
) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0;
}
/**
* Compute desired roll command
*/
if (groundspeedDesired> 1e-6) {
courseError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
} else {
// if we are not supposed to move, keep going wherever we are now. Don't make things worse by changing direction.
courseError = 0;
}
if (courseError<-180.0f) courseError+=360.0f;
if (courseError>180.0f) courseError-=360.0f;
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI],
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
courseIntegral);
fixedwingpathfollowerStatus.E[FIXEDWINGPATHFOLLOWERSTATUS_E_COURSE] = courseError;
fixedwingpathfollowerStatus.A[FIXEDWINGPATHFOLLOWERSTATUS_A_COURSE] = courseIntegral;
fixedwingpathfollowerStatus.C[FIXEDWINGPATHFOLLOWERSTATUS_C_COURSE] = courseCommand;
stabDesired.Roll = bound( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
courseCommand,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX] );
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.X
stabDesired.Yaw = 0;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
return result;
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
PathDesiredGet(&pathDesired);
}
static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
{
BaroAirspeedData baroAirspeed;
VelocityActualData velocityActual;
BaroAirspeedGet(&baroAirspeed);
if (baroAirspeed.Connected != BAROAIRSPEED_CONNECTED_TRUE) {
baroAirspeedBias = 0;
} else {
VelocityActualGet(&velocityActual);
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
baroAirspeedBias = baroAirspeed.Airspeed - groundspeed;
}
}

1
flight/Modules/ManualControl/inc/manualcontrol.h
View File

@ -42,6 +42,7 @@ typedef enum {FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABIL
(x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \
(x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : FLIGHTMODE_UNDEFINED \
)

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

@ -84,7 +84,7 @@ static portTickType lastSysTime;
static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed);
static void updatePathDesired(ManualControlCommandData * cmd, bool changed);
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
static void setArmedIfChanged(uint8_t val);
@ -395,7 +395,10 @@ static void manualControlTask(void *parameters)
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -619,7 +622,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
* @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter
*/
static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home)
{
static portTickType lastSysTime;
portTickType thisSysTime;
@ -629,24 +632,46 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
if(changed) {
if (home && changed) {
// Simple Return To Base mode - keep altitude the same, fly to home position
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if(changed) {
// After not being in this mode for a while init at current height
PositionActualData positionActual;
PositionActualGet(&positionActual);
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.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else {
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
}
@ -699,7 +724,7 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
// TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening
// in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed)
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home)
{
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
}
@ -771,6 +796,21 @@ static bool okToArm(void)
return true;
}
/**
* @brief Determine if the aircraft is forced to disarm by an explicit alarm
* @returns True if safe to arm, false otherwise
*/
static bool forcedDisArm(void)
{
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
@ -796,6 +836,12 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
bool lowThrottle = cmd->Throttle <= 0;
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
return;
}
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);

12
flight/Modules/PathPlanner/inc/pathplanner.h
View File

@ -2,11 +2,11 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup FlightPlan Flight Plan Module
* @addtogroup PathPlanner Flight Plan Module
* @brief Executes flight plan scripts in Python
* @{
*
* @file flightplan.c
* @file pathplanner.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Executes flight plan scripts in Python
*
@ -28,9 +28,9 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FLIGHTPLAN_H
#define FLIGHTPLAN_H
#ifndef PATHPLANNER_H
#define PATHPLANNER_H
int32_t FlightPlanInitialize();
// public initialisations go here
#endif // FLIGHTPLAN_H
#endif // PATHPLANNER_H

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

@ -33,32 +33,56 @@
#include "paths.h"
#include "flightstatus.h"
#include "baroairspeed.h" // TODO: make baroairspeed optional
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
#include "pathplannersettings.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "waypoint.h"
#include "waypointactive.h"
// Private constants
#define STACK_SIZE_BYTES 1024
#define STACK_SIZE_BYTES 1024 // TODO profile and reduce!
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define MAX_QUEUE_SIZE 2
#define F_PI 3.141526535897932f
#define RAD2DEG (180.0f/F_PI)
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void settingsUpdated(UAVObjEvent * ev);
static void updatePathDesired(UAVObjEvent * ev);
static void setWaypoint(uint16_t num);
static void WaypointCreateInstances(uint16_t num);
static void createPathBox();
static void createPathLogo();
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
static uint8_t conditionTimeOut();
static uint8_t conditionDistanceToTarget();
static uint8_t conditionLegRemaining();
static uint8_t conditionBelowError();
static uint8_t conditionAboveAltitude();
static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
static xQueueHandle queue;
static PathPlannerSettingsData pathPlannerSettings;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
static bool pathplanner_active = false;
// Private functions
static void pathPlannerTask(void *parameters);
static void settingsUpdated(UAVObjEvent * ev);
static void waypointsUpdated(UAVObjEvent * ev);
static void createPathBox();
static void createPathLogo();
/**
* Module initialization
@ -82,6 +106,12 @@ int32_t PathPlannerInitialize()
taskHandle = NULL;
PathPlannerSettingsInitialize();
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
PositionActualInitialize();
BaroAirspeedInitialize();
VelocityActualInitialize();
WaypointInitialize();
WaypointActiveInitialize();
@ -96,24 +126,23 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
int32_t bad_inits;
int32_t bad_reads;
static void pathPlannerTask(void *parameters)
{
// update settings on change
PathPlannerSettingsConnectCallback(settingsUpdated);
settingsUpdated(PathPlannerSettingsHandle());
WaypointConnectCallback(waypointsUpdated);
WaypointActiveConnectCallback(waypointsUpdated);
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
WaypointActiveConnectCallback(updatePathDesired);
PathActionConnectCallback(updatePathDesired);
FlightStatusData flightStatus;
PathDesiredData pathDesired;
PositionActualData positionActual;
PathStatusData pathStatus;
const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m)
// Main thread loop
bool pathplanner_active = false;
bool endCondition = false;
while (1)
{
@ -125,152 +154,74 @@ static void pathPlannerTask(void *parameters)
continue;
}
WaypointActiveGet(&waypointActive);
if(pathplanner_active == false) {
pathplanner_active = true;
// This triggers callback to update variable
WaypointActiveGet(&waypointActive);
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
pathplanner_active = true;
continue;
}
switch(pathPlannerSettings.PathMode) {
case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
PositionActualGet(&positionActual);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
// We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT:
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
break;
case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -20;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
WaypointInstGet(waypointActive.Index,&waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusGet(&pathStatus);
PathDesiredGet(&pathDesired);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
continue;
}
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) setWaypoint(waypointActive.Index+1);
break;
case PATHPLANNERSETTINGS_PATHMODE_PATH:
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
if (progress.fractional_progress >= 1) {
switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT:
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
break;
case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = -20;
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break;
default:
PIOS_DEBUG_Assert(0);
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
} else {
setWaypoint(waypointActive.Index+1);
}
break;
}
}
}
/**
* On changed waypoints or active waypoint update position desired
* if we are in charge
*/
static void waypointsUpdated(UAVObjEvent * ev)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
return;
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
PathDesiredData pathDesired;
switch(pathPlannerSettings.PathMode) {
case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT:
{
PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = -waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
}
break;
case PATHPLANNERSETTINGS_PATHMODE_PATH:
{
PathDesiredData pathDesired;
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Mode = PATHDESIRED_MODE_PATH;
pathDesired.EndingVelocity = sqrtf(powf(waypoint.Velocity[WAYPOINT_VELOCITY_NORTH],2) +
powf(waypoint.Velocity[WAYPOINT_VELOCITY_EAST],2));
if(waypointActive.Index == 0) {
// Get current position as start point
PositionActualData positionActual;
PositionActualGet(&positionActual);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1;
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.StartingVelocity = sqrtf(powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_NORTH],2) +
powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_EAST],2));
}
PathDesiredSet(&pathDesired);
}
break;
}
}
// standard settings updated callback
void settingsUpdated(UAVObjEvent * ev) {
uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath;
PathPlannerSettingsGet(&pathPlannerSettings);
@ -289,18 +240,343 @@ void settingsUpdated(UAVObjEvent * ev) {
}
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(UAVObjEvent * ev) {
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) return;
// use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActive;
static PathActionData pathAction;
static WaypointData waypoint;
static PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index,&waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
if(waypointActive.Index == 0) {
// 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_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_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.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
}
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num) {
// path plans wrap around
if (num>=UAVObjGetNumInstances(WaypointHandle())) {
num = 0;
}
waypointActive.Index = num;
WaypointActiveSet(&waypointActive);
}
// helper function to make sure there are enough waypoint instances
static void WaypointCreateInstances(uint16_t num) {
uint16_t t;
for (t=UAVObjGetNumInstances(WaypointHandle());t<num;t++) {
WaypointCreateInstance();
}
}
// execute the apropriate condition and report result
static uint8_t pathConditionCheck() {
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
switch (pathAction.EndCondition) {
case PATHACTION_ENDCONDITION_NONE:
return conditionNone();
break;
case PATHACTION_ENDCONDITION_TIMEOUT:
return conditionTimeOut();
break;
case PATHACTION_ENDCONDITION_DISTANCETOTARGET:
return conditionDistanceToTarget();
break;
case PATHACTION_ENDCONDITION_LEGREMAINING:
return conditionLegRemaining();
break;
case PATHACTION_ENDCONDITION_BELOWERROR:
return conditionBelowError();
break;
case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
return conditionAboveAltitude();
break;
case PATHACTION_ENDCONDITION_ABOVESPEED:
return conditionAboveSpeed();
break;
case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
return conditionPointingTowardsNext();
break;
case PATHACTION_ENDCONDITION_PYTHONSCRIPT:
return conditionPythonScript();
break;
case PATHACTION_ENDCONDITION_IMMEDIATE:
return conditionImmediate();
break;
default:
// invalid endconditions are always true to prevent freezes
return true;
break;
}
}
/* the None condition is always false */
static uint8_t conditionNone() {
return false;
}
/**
* the Timeout condition measures time this waypoint is active
* Parameter 0: timeout in seconds
*/
static uint8_t conditionTimeOut() {
static uint16_t toWaypoint;
static uint32_t toStarttime;
// reset timer if waypoint changed
if (waypointActive.Index!=toWaypoint) {
toWaypoint = waypointActive.Index;
toStarttime = PIOS_DELAY_GetRaw();
}
if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) {
// make sure we reinitialize even if the same waypoint comes twice
toWaypoint = 0xFFFF;
return true;
}
return false;
}
/**
* the DistanceToTarget measures distance to a waypoint
* returns true if closer
* Parameter 0: distance in meters
* Parameter 1: flag: 0=2d 1=3d
*/
static uint8_t conditionDistanceToTarget() {
float distance;
PositionActualData positionActual;
PositionActualGet(&positionActual);
if (pathAction.ConditionParameters[1]>0.5f) {
distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
+powf( waypoint.Position[1]-positionActual.East ,2)
+powf( waypoint.Position[1]-positionActual.Down ,2));
} else {
distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
+powf( waypoint.Position[1]-positionActual.East ,2));
}
if (distance>=pathAction.ConditionParameters[0]) {
return true;
}
return false;
}
/**
* the LegRemaining measures how far the pathfollower got on a linear path segment
* returns true if closer to destination (path more complete)
* Parameter 0: relative distance (0= complete, 1= just starting)
*/
static uint8_t conditionLegRemaining() {
PathDesiredData pathDesired;
PositionActualData positionActual;
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) {
return true;
}
return false;
}
/**
* the BelowError measures the error on a path segment
* returns true if error is below margin
* Parameter 0: error margin (in m)
*/
static uint8_t conditionBelowError() {
PathDesiredData pathDesired;
PositionActualData positionActual;
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
if ( progress.error <= pathAction.ConditionParameters[0] ) {
return true;
}
return false;
}
/**
* the AboveAltitude measures the flight altitude relative to home position
* returns true if above critical altitude
* WARNING! Altitudes are always negative (down coordinate)
* Parameter 0: altitude in meters (negative!)
*/
static uint8_t conditionAboveAltitude() {
PositionActualData positionActual;
PositionActualGet(&positionActual);
if (positionActual.Down <= pathAction.ConditionParameters[0]) {
return true;
}
return false;
}
/**
* the AboveSpeed measures the movement speed (3d)
* returns true if above critical speed
* Parameter 0: speed in m/s
* Parameter 1: flag: 0=groundspeed 1=airspeed
*/
static uint8_t conditionAboveSpeed() {
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down );
// use airspeed if requested and available
if (pathAction.ConditionParameters[1]>0.5f) {
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.Connected == BAROAIRSPEED_CONNECTED_TRUE) {
velocity = baroAirspeed.Airspeed;
}
}
if ( velocity >= pathAction.ConditionParameters[0]) {
return true;
}
return false;
}
/**
* the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
* regardless whether this waypoint will ever be active (Command could jump to another one on true)
* This is useful for curve segments where the craft should stop circling when facing a certain way (usually the next waypoint)
* returns true if within a certain angular margin
* Parameter 0: degrees variation allowed
*/
static uint8_t conditionPointingTowardsNext() {
uint16_t nextWaypointId = waypointActive.Index+1;
if (nextWaypointId>=UAVObjGetNumInstances(WaypointHandle())) {
nextWaypointId = 0;
}
WaypointData nextWaypoint;
WaypointInstGet(nextWaypointId,&nextWaypoint);
float angle1 = atan2f((nextWaypoint.Position[0]-waypoint.Position[0]),(nextWaypoint.Position[1]-waypoint.Position[1]));
VelocityActualData velocity;
VelocityActualGet (&velocity);
float angle2 = atan2f(velocity.North,velocity.East);
// calculate the absolute angular difference
angle1 = fabsf(RAD2DEG * (angle1 - angle2));
while (angle1>360) angle1-=360;
if (angle1 <= pathAction.ConditionParameters[0]) {
return true;
}
return false;
}
/**
* the PythonScript is supposed to read the output of a PyMite program running at the same time
* and return based on its output, likely read out through some to be defined UAVObject
* TODO XXX NOT YET IMPLEMENTED
* returns always true until implemented
* Parameter 0-3: defined by user script
*/
static uint8_t conditionPythonScript() {
return true;
}
/* the immediate condition is always true */
static uint8_t conditionImmediate() {
return true;
}
// demo path - box
static void createPathBox()
{
WaypointCreateInstance();
WaypointCreateInstance();
WaypointCreateInstance();
WaypointCreateInstance();
WaypointCreateInstance();
uint16_t t;
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
PathActionCreateInstance();
}
PathActionData action;
PathActionInstGet(0,&action);
action.Mode = PATHACTION_MODE_FLYVECTOR;
action.ModeParameters[0]=0;
action.ModeParameters[1]=0;
action.ModeParameters[2]=0;
action.ModeParameters[3]=0;
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
action.ConditionParameters[0] = 0;
action.ConditionParameters[1] = 0;
action.ConditionParameters[2] = 0;
action.ConditionParameters[3] = 0;
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
action.JumpDestination = 0;
action.ErrorDestination = 0;
PathActionInstSet(0,&action);
PathActionInstSet(1,&action);
PathActionInstSet(2,&action);
WaypointCreateInstances(6);
// Draw O
WaypointData waypoint;
waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
waypoint.Action = WAYPOINT_ACTION_NEXT;
waypoint.Action = 0;
waypoint.Velocity = 2;
waypoint.Position[0] = 5;
waypoint.Position[1] = 5;
@ -328,78 +604,86 @@ static void createPathBox()
WaypointInstSet(5, &waypoint);
}
// demo path - logo
static void createPathLogo()
{
#define SIZE 10.0f
PathActionData action;
PathActionInstGet(0,&action);
action.Mode = PATHACTION_MODE_FLYVECTOR;
action.ModeParameters[0]=0;
action.ModeParameters[1]=0;
action.ModeParameters[2]=0;
action.ModeParameters[3]=0;
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
action.ConditionParameters[0] = 0;
action.ConditionParameters[1] = 0;
action.ConditionParameters[2] = 0;
action.ConditionParameters[3] = 0;
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
action.JumpDestination = 0;
action.ErrorDestination = 0;
PathActionInstSet(0,&action);
uint16_t t;
for (t=UAVObjGetNumInstances(PathActionHandle());t<10;t++) {
PathActionCreateInstance();
}
WaypointCreateInstances(42);
// Draw O
WaypointData waypoint;
waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
waypoint.Velocity = 2; // Since for now this isn't directional just set a mag
waypoint.Action = 0;
for(uint32_t i = 0; i < 20; i++) {
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
WaypointInstSet(i, &waypoint);
}
// Draw P
for(uint32_t i = 20; i < 35; i++) {
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
WaypointInstSet(i, &waypoint);
}
waypoint.Position[1] = 35;
waypoint.Position[0] = -50;
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -50;
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(35, &waypoint);
// Draw Box
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(36, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = -60;
waypoint.Position[1] = SIZE * 85;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(37, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = 60;
waypoint.Position[1] = SIZE * 85;
waypoint.Position[0] = SIZE * 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(38, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = 60;
waypoint.Position[1] = SIZE * -40;
waypoint.Position[0] = SIZE * 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(39, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = -60;
waypoint.Position[1] = SIZE * -40;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(40, &waypoint);
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[1] = SIZE * 35;
waypoint.Position[0] = SIZE * -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(41, &waypoint);
}

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

@ -48,14 +48,16 @@
#include "vtolpathfollower.h"
#include "accels.h"
#include "hwsettings.h"
#include "attitudeactual.h"
#include "pathdesired.h" // object that will be updated by the module
#include "positionactual.h"
#include "manualcontrol.h"
#include "flightstatus.h"
#include "pathstatus.h"
#include "gpsvelocity.h"
#include "gpsposition.h"
#include "guidancesettings.h"
#include "vtolpathfollowersettings.h"
#include "nedaccel.h"
#include "nedposition.h"
#include "stabilizationdesired.h"
@ -74,9 +76,10 @@
// Private types
// Private variables
static bool followerEnabled = false;
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static GuidanceSettingsData guidanceSettings;
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
// Private functions
static void vtolPathFollowerTask(void *parameters);
@ -84,6 +87,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updateNedAccel();
static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float* attitude);
static void updateVtolDesiredAttitude();
static float bound(float val, float min, float max);
@ -93,9 +97,11 @@ static float bound(float val, float min, float max);
*/
int32_t VtolPathFollowerStart()
{
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
if (followerEnabled) {
// Start main task
xTaskCreate(vtolPathFollowerTask, (signed char *)"PathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
}
return 0;
}
@ -106,10 +112,19 @@ int32_t VtolPathFollowerStart()
*/
int32_t VtolPathFollowerInitialize()
{
GuidanceSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
VelocityDesiredInitialize();
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
followerEnabled = true;
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
PathStatusInitialize();
VelocityDesiredInitialize();
} else {
followerEnabled = false;
}
return 0;
}
@ -132,13 +147,14 @@ static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
PathStatusData pathStatus;
portTickType lastUpdateTime;
GuidanceSettingsConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
// Main task loop
@ -169,33 +185,54 @@ static void vtolPathFollowerTask(void *parameters)
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, guidanceSettings.UpdatePeriod / portTICK_RATE_MS);
vTaskDelayUntil(&lastUpdateTime, vtolpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS);
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_ENDPOINT) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_PATH) {
updatePathVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
break;
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
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();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FIXEDATTITUDE:
updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_DISARMALARM:
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
break;
}
break;
default:
@ -225,7 +262,7 @@ static void vtolPathFollowerTask(void *parameters)
*/
static void updatePathVelocity()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
float downCommand;
PositionActualData positionActual;
@ -234,10 +271,10 @@ static void updatePathVelocity()
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
if(progress.fractional_progress > 1)
groundspeed = 0;
@ -245,14 +282,14 @@ static void updatePathVelocity()
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP];
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
@ -261,13 +298,13 @@ static void updatePathVelocity()
bound(progress.fractional_progress,0,1);
float downError = altitudeSetpoint - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
@ -280,7 +317,7 @@ static void updatePathVelocity()
*/
void updateEndpointVelocity()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
PositionActualData positionActual;
VelocityDesiredData velocityDesired;
@ -296,13 +333,13 @@ void updateEndpointVelocity()
float downCommand;
float northPos = 0, eastPos = 0, downPos = 0;
switch (guidanceSettings.PositionSource) {
case GUIDANCESETTINGS_POSITIONSOURCE_EKF:
switch (vtolpathfollowerSettings.PositionSource) {
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case GUIDANCESETTINGS_POSITIONSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_POSITIONSOURCE_GPSPOS:
{
NEDPositionData nedPosition;
NEDPositionGet(&nedPosition);
@ -318,40 +355,58 @@ void updateEndpointVelocity()
// Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
-vtolpathfollowerSettings.VerticalVelMax,
vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from a fixed preset
*
*/
static void updateFixedAttitude(float* attitude)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
stabDesired.Roll = attitude[0];
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
*
@ -361,14 +416,14 @@ void updateEndpointVelocity()
*/
static void updateVtolDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
VtolPathFollowerSettingsData vtolpathfollowerSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
@ -382,7 +437,7 @@ static void updateVtolDesiredAttitude()
float downCommand;
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
@ -393,13 +448,13 @@ static void updateVtolDesiredAttitude()
NedAccelGet(&nedAccel);
float northVel = 0, eastVel = 0, downVel = 0;
switch (guidanceSettings.VelocitySource) {
case GUIDANCESETTINGS_VELOCITYSOURCE_EKF:
switch (vtolpathfollowerSettings.VelocitySource) {
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_EKF:
northVel = velocityActual.North;
eastVel = velocityActual.East;
downVel = velocityActual.Down;
break;
case GUIDANCESETTINGS_VELOCITYSOURCE_NEDVEL:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_NEDVEL:
{
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
@ -408,7 +463,7 @@ static void updateVtolDesiredAttitude()
downVel = gpsVelocity.Down;
}
break;
case GUIDANCESETTINGS_VELOCITYSOURCE_GPSPOS:
case VTOLPATHFOLLOWERSETTINGS_VELOCITYSOURCE_GPSPOS:
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
@ -429,34 +484,34 @@ static void updateVtolDesiredAttitude()
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
northVelIntegral -
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.North * guidanceSettings.VelocityFeedforward);
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI],
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
eastVelIntegral -
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.East * guidanceSettings.VelocityFeedforward);
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI],
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
downVelIntegral -
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
@ -464,12 +519,12 @@ static void updateVtolDesiredAttitude()
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
@ -538,7 +593,7 @@ static float bound(float val, float min, float max)
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
GuidanceSettingsGet(&guidanceSettings);
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
PathDesiredGet(&pathDesired);
}

2
flight/Revolution/Makefile
View File

@ -53,7 +53,7 @@ MODULES = Sensors Attitude/revolution ManualControl Stabilization Actuator
MODULES += Battery
MODULES += Altitude/revolution GPS FirmwareIAP
MODULES += Airspeed/revolution
MODULES += AltitudeHold VtolPathFollower PathPlanner
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += CameraStab
MODULES += OveroSync
MODULES += Telemetry

8
flight/Revolution/UAVObjects.inc
View File

@ -49,7 +49,9 @@ UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += guidancesettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
@ -60,8 +62,10 @@ UAVOBJSRCFILENAMES += nedaccel
UAVOBJSRCFILENAMES += nedposition
UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += revocalibration

4
flight/SimPosix/Makefile
View File

@ -53,6 +53,9 @@ FLASH_TOOL = OPENOCD
# List of modules to include
MODULES = ManualControl Stabilization GPS
MODULES += PathPlanner
MODULES += FixedWingPathFollower
MODULES += VtolPathFollower
MODULES += CameraStab
MODULES += Telemetry
#MODULES += OveroSync
@ -131,6 +134,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(FLIGHTLIB)/paths.c
## PIOS Hardware (STM32F4xx)
include $(PIOS)/posix/library.mk

14
flight/SimPosix/UAVObjects.inc
View File

@ -36,6 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate
@ -47,7 +48,10 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += guidancesettings
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += homelocation
UAVOBJSRCFILENAMES += i2cstats
UAVOBJSRCFILENAMES += manualcontrolcommand
@ -55,10 +59,14 @@ UAVOBJSRCFILENAMES += manualcontrolsettings
UAVOBJSRCFILENAMES += mixersettings
UAVOBJSRCFILENAMES += mixerstatus
UAVOBJSRCFILENAMES += nedaccel
UAVOBJSRCFILENAMES += nedposition
UAVOBJSRCFILENAMES += objectpersistence
UAVOBJSRCFILENAMES += overosyncstats
UAVOBJSRCFILENAMES += pathaction
UAVOBJSRCFILENAMES += pathdesired
UAVOBJSRCFILENAMES += pathplannersettings
UAVOBJSRCFILENAMES += pathstatus
UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += positiondesired
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += sonaraltitude
@ -71,6 +79,8 @@ UAVOBJSRCFILENAMES += taskinfo
UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += waypoint
UAVOBJSRCFILENAMES += waypointactive
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += hwsettings
UAVOBJSRCFILENAMES += receiveractivity

2
ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
View File

@ -133,7 +133,7 @@ void MagicWaypointGadgetWidget::positionSelected(double north, double east) {
PathDesired::DataFields pathDesired = getPathDesired()->getData();
pathDesired.End[PathDesired::END_NORTH] = north * scale;
pathDesired.End[PathDesired::END_EAST] = east * scale;
pathDesired.Mode = PathDesired::MODE_ENDPOINT;
pathDesired.Mode = PathDesired::MODE_FLYENDPOINT;
getPathDesired()->setData(pathDesired);
}

11
ground/openpilotgcs/src/plugins/pathactioneditor/PathActionEditor.pluginspec Normal file
View File

@ -0,0 +1,11 @@
<plugin name="PathActionEditor" version="0.0.1" compatVersion="1.0.0">
<vendor>The OpenPilot Project</vendor>
<copyright>(C) 2012 OpenPilot Project</copyright>
<license>The GNU Public License (GPL) Version 3</license>
<description>Allows editing a list of pathactions</description>
<url>http://www.openpilot.org</url>
<dependencyList>
<dependency name="Core" version="1.0.0"/>
</dependencyList>
</plugin>

75
ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.cpp Normal file
View File

@ -0,0 +1,75 @@
/**
******************************************************************************
*
* @file browseritemdelegate.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "browseritemdelegate.h"
#include "fieldtreeitem.h"
BrowserItemDelegate::BrowserItemDelegate(QObject *parent) :
QStyledItemDelegate(parent)
{
}
QWidget *BrowserItemDelegate::createEditor(QWidget *parent,
const QStyleOptionViewItem & option ,
const QModelIndex & index ) const
{
Q_UNUSED(option)
FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
QWidget *editor = item->createEditor(parent);
Q_ASSERT(editor);
return editor;
}
void BrowserItemDelegate::setEditorData(QWidget *editor,
const QModelIndex &index) const
{
FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
QVariant value = index.model()->data(index, Qt::EditRole);
item->setEditorValue(editor, value);
}
void BrowserItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model,
const QModelIndex &index) const
{
FieldTreeItem *item = static_cast<FieldTreeItem*>(index.internalPointer());
QVariant value = item->getEditorValue(editor);
model->setData(index, value, Qt::EditRole);
}
void BrowserItemDelegate::updateEditorGeometry(QWidget *editor,
const QStyleOptionViewItem &option, const QModelIndex &/* index */) const
{
editor->setGeometry(option.rect);
}
QSize BrowserItemDelegate::sizeHint(const QStyleOptionViewItem & option, const QModelIndex &index) const
{
Q_UNUSED(option);
Q_UNUSED(index);
return QSpinBox().sizeHint();
}

58
ground/openpilotgcs/src/plugins/pathactioneditor/browseritemdelegate.h Normal file
View File

@ -0,0 +1,58 @@
/**
******************************************************************************
*
* @file browseritemdelegate.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef BROWSERITEMDELEGATE_H
#define BROWSERITEMDELEGATE_H
#include <QStyledItemDelegate>
class BrowserItemDelegate : public QStyledItemDelegate
{
Q_OBJECT
public:
explicit BrowserItemDelegate(QObject *parent = 0);
QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &option,
const QModelIndex &index) const;
void setEditorData(QWidget *editor, const QModelIndex &index) const;
void setModelData(QWidget *editor, QAbstractItemModel *model,
const QModelIndex &index) const;
void updateEditorGeometry(QWidget *editor,
const QStyleOptionViewItem &option, const QModelIndex &index) const;
QSize sizeHint(const QStyleOptionViewItem & option,
const QModelIndex &index) const;
signals:
public slots:
};
#endif // BROWSERITEMDELEGATE_H

29
ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.cpp Normal file
View File

@ -0,0 +1,29 @@
/**
******************************************************************************
*
* @file fieldtreeitem.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "fieldtreeitem.h"

327
ground/openpilotgcs/src/plugins/pathactioneditor/fieldtreeitem.h Normal file
View File

@ -0,0 +1,327 @@
/**
******************************************************************************
*
* @file fieldtreeitem.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef FIELDTREEITEM_H
#define FIELDTREEITEM_H
#include "treeitem.h"
#include <QtCore/QStringList>
#include <QtGui/QWidget>
#include <QtGui/QSpinBox>
#include <QtGui/QDoubleSpinBox>
#include <qscispinbox/QScienceSpinBox.h>
#include <QtGui/QComboBox>
#include <limits>
#define QINT8MIN std::numeric_limits<qint8>::min()
#define QINT8MAX std::numeric_limits<qint8>::max()
#define QUINTMIN std::numeric_limits<quint8>::min()
#define QUINT8MAX std::numeric_limits<quint8>::max()
#define QINT16MIN std::numeric_limits<qint16>::min()
#define QINT16MAX std::numeric_limits<qint16>::max()
#define QUINT16MAX std::numeric_limits<quint16>::max()
#define QINT32MIN std::numeric_limits<qint32>::min()
#define QINT32MAX std::numeric_limits<qint32>::max()
#define QUINT32MAX std::numeric_limits<qint32>::max()
//#define USE_SCIENTIFIC_NOTATION
class FieldTreeItem : public TreeItem
{
Q_OBJECT
public:
FieldTreeItem(int index, const QList<QVariant> &data, TreeItem *parent = 0) :
TreeItem(data, parent), m_index(index) { }
FieldTreeItem(int index, const QVariant &data, TreeItem *parent = 0) :
TreeItem(data, parent), m_index(index) { }
bool isEditable() { return true; }
virtual QWidget *createEditor(QWidget *parent) = 0;
virtual QVariant getEditorValue(QWidget *editor) = 0;
virtual void setEditorValue(QWidget *editor, QVariant value) = 0;
virtual void apply() { }
protected:
int m_index;
};
class EnumFieldTreeItem : public FieldTreeItem
{
Q_OBJECT
public:
EnumFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data,
TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { }
EnumFieldTreeItem(UAVObjectField *field, int index, const QVariant &data,
TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_enumOptions(field->getOptions()), m_field(field) { }
void setData(QVariant value, int column) {
QStringList options = m_field->getOptions();
QVariant tmpValue = m_field->getValue(m_index);
int tmpValIndex = options.indexOf(tmpValue.toString());
TreeItem::setData(value, column);
setChanged(tmpValIndex != value);
}
QString enumOptions(int index) {
if((index < 0) || (index >= m_enumOptions.length())) {
return QString("Invalid Value (") + QString().setNum(index) + QString(")");
}
return m_enumOptions.at(index);
}
void apply() {
int value = data(dataColumn).toInt();
QStringList options = m_field->getOptions();
m_field->setValue(options[value], m_index);
setChanged(false);
}
void update() {
QStringList options = m_field->getOptions();
QVariant value = m_field->getValue(m_index);
int valIndex = options.indexOf(value.toString());
if (data() != valIndex || changed()) {
TreeItem::setData(valIndex);
setHighlight(true);
}
}
QWidget *createEditor(QWidget *parent) {
QComboBox *editor = new QComboBox(parent);
foreach (QString option, m_enumOptions)
editor->addItem(option);
return editor;
}
QVariant getEditorValue(QWidget *editor) {
QComboBox *comboBox = static_cast<QComboBox*>(editor);
return comboBox->currentIndex();
}
void setEditorValue(QWidget *editor, QVariant value) {
QComboBox *comboBox = static_cast<QComboBox*>(editor);
comboBox->setCurrentIndex(value.toInt());
}
private:
QStringList m_enumOptions;
UAVObjectField *m_field;
};
class IntFieldTreeItem : public FieldTreeItem
{
Q_OBJECT
public:
IntFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) {
setMinMaxValues();
}
IntFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) {
setMinMaxValues();
}
void setMinMaxValues() {
switch (m_field->getType()) {
case UAVObjectField::INT8:
m_minValue = QINT8MIN;
m_maxValue = QINT8MAX;
break;
case UAVObjectField::INT16:
m_minValue = QINT16MIN;
m_maxValue = QINT16MAX;
break;
case UAVObjectField::INT32:
m_minValue = QINT32MIN;
m_maxValue = QINT32MAX;
break;
case UAVObjectField::UINT8:
m_minValue = QUINTMIN;
m_maxValue = QUINT8MAX;
break;
case UAVObjectField::UINT16:
m_minValue = QUINTMIN;
m_maxValue = QUINT16MAX;
break;
case UAVObjectField::UINT32:
m_minValue = QUINTMIN;
m_maxValue = QUINT32MAX;
break;
default:
Q_ASSERT(false);
break;
}
}
QWidget *createEditor(QWidget *parent) {
QSpinBox *editor = new QSpinBox(parent);
editor->setMinimum(m_minValue);
editor->setMaximum(m_maxValue);
return editor;
}
QVariant getEditorValue(QWidget *editor) {
QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
spinBox->interpretText();
return spinBox->value();
}
void setEditorValue(QWidget *editor, QVariant value) {
QSpinBox *spinBox = static_cast<QSpinBox*>(editor);
spinBox->setValue(value.toInt());
}
void setData(QVariant value, int column) {
QVariant old=m_field->getValue(m_index);
TreeItem::setData(value, column);
setChanged(old != value);
}
void apply() {
m_field->setValue(data(dataColumn).toInt(), m_index);
setChanged(false);
}
void update() {
int value = m_field->getValue(m_index).toInt();
if (data() != value || changed()) {
TreeItem::setData(value);
setHighlight(true);
}
}
private:
UAVObjectField *m_field;
int m_minValue;
int m_maxValue;
};
class FloatFieldTreeItem : public FieldTreeItem
{
Q_OBJECT
public:
FloatFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) { }
FloatFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) { }
void setData(QVariant value, int column) {
QVariant old=m_field->getValue(m_index);
TreeItem::setData(value, column);
setChanged(old != value);
}
void apply() {
m_field->setValue(data(dataColumn).toDouble(), m_index);
setChanged(false);
}
void update() {
double value = m_field->getValue(m_index).toDouble();
if (data() != value || changed()) {
TreeItem::setData(value);
setHighlight(true);
}
}
QWidget *createEditor(QWidget *parent) {
#ifdef USE_SCIENTIFIC_NOTATION
QScienceSpinBox *editor = new QScienceSpinBox(parent);
editor->setDecimals(6);
#else
QDoubleSpinBox *editor = new QDoubleSpinBox(parent);
editor->setDecimals(8);
#endif
editor->setMinimum(-std::numeric_limits<float>::max());
editor->setMaximum(std::numeric_limits<float>::max());
return editor;
}
QVariant getEditorValue(QWidget *editor) {
#ifdef USE_SCIENTIFIC_NOTATION
QScienceSpinBox *spinBox = static_cast<QScienceSpinBox*>(editor);
#else
QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
#endif
spinBox->interpretText();
return spinBox->value();
}
void setEditorValue(QWidget *editor, QVariant value) {
#ifdef USE_SCIENTIFIC_NOTATION
QScienceSpinBox *spinBox = static_cast<QScienceSpinBox*>(editor);
#else
QDoubleSpinBox *spinBox = static_cast<QDoubleSpinBox*>(editor);
#endif
spinBox->setValue(value.toDouble());
}
private:
UAVObjectField *m_field;
};
class ActionFieldTreeItem : public FieldTreeItem
{
Q_OBJECT
public:
ActionFieldTreeItem(UAVObjectField *field, int index, const QList<QVariant> &data, QStringList *actions,
TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; }
ActionFieldTreeItem(UAVObjectField *field, int index, const QVariant &data, QStringList *actions,
TreeItem *parent = 0) :
FieldTreeItem(index, data, parent), m_field(field) { m_enumOptions=actions; }
void setData(QVariant value, int column) {
int tmpValIndex = m_field->getValue(m_index).toInt();
TreeItem::setData(value, column);
setChanged(tmpValIndex != value);
}
QString enumOptions(int index) {
if((index < 0) || (index >= m_enumOptions->length())) {
return QString("Invalid Value (") + QString().setNum(index) + QString(")");
}
return m_enumOptions->at(index);
}
void apply() {
int value = data(dataColumn).toInt();
m_field->setValue(value, m_index);
setChanged(false);
}
void update() {
int valIndex = m_field->getValue(m_index).toInt();
if (data() != valIndex || changed()) {
TreeItem::setData(valIndex);
setHighlight(true);
}
}
QWidget *createEditor(QWidget *parent) {
QComboBox *editor = new QComboBox(parent);
foreach (QString option, *m_enumOptions)
editor->addItem(option);
return editor;
}
QVariant getEditorValue(QWidget *editor) {
QComboBox *comboBox = static_cast<QComboBox*>(editor);
return comboBox->currentIndex();
}
void setEditorValue(QWidget *editor, QVariant value) {
QComboBox *comboBox = static_cast<QComboBox*>(editor);
comboBox->setCurrentIndex(value.toInt());
}
private:
QStringList *m_enumOptions;
UAVObjectField *m_field;
};
#endif // FIELDTREEITEM_H

33
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.pro Normal file
View File

@ -0,0 +1,33 @@
TEMPLATE = lib
TARGET = PathActionEditor
include(../../openpilotgcsplugin.pri)
include(../../plugins/coreplugin/coreplugin.pri)
include(../../plugins/uavobjects/uavobjects.pri)
HEADERS += pathactioneditorgadget.h
HEADERS += pathactioneditorgadgetwidget.h
HEADERS += pathactioneditorgadgetfactory.h
HEADERS += pathactioneditorplugin.h
HEADERS += pathactioneditortreemodel.h
HEADERS += treeitem.h
HEADERS += fieldtreeitem.h
HEADERS += browseritemdelegate.h
SOURCES += pathactioneditorgadget.cpp
SOURCES += pathactioneditorgadgetwidget.cpp
SOURCES += pathactioneditorgadgetfactory.cpp
SOURCES += pathactioneditorplugin.cpp
SOURCES += pathactioneditortreemodel.cpp
SOURCES += treeitem.cpp
SOURCES += fieldtreeitem.cpp
SOURCES += browseritemdelegate.cpp
OTHER_FILES += pathactioneditor.pluginspec
FORMS += pathactioneditor.ui
RESOURCES += pathactioneditor.qrc

3
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.qrc Normal file
View File

@ -0,0 +1,3 @@
<RCC>
<qresource prefix="/pathactioneditor"/>
</RCC>

59
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditor.ui Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>PathActionEditor</class>
<widget class="QWidget" name="PathActionEditor">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>653</width>
<height>296</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QTreeView" name="pathactions">
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonNewPathAction">
<property name="text">
<string>New PathAction</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonNewWaypoint">
<property name="text">
<string>New Waypoint</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonSaveFile">
<property name="text">
<string>Save to File</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="buttonLoadFile">
<property name="text">
<string>Load from File</string>
</property>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>

48
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.cpp Normal file
View File

@ -0,0 +1,48 @@
/**
******************************************************************************
* @file pathactioneditorgadget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pathactioneditorgadget.h"
#include "pathactioneditorgadgetwidget.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include <QDebug>
PathActionEditorGadget::PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent) :
IUAVGadget(classId, parent),
m_widget(widget)
{
}
PathActionEditorGadget::~PathActionEditorGadget()
{
delete m_widget;
}
void PathActionEditorGadget::loadConfiguration(IUAVGadgetConfiguration* config)
{
Q_UNUSED(config);
}

59
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadget.h Normal file
View File

@ -0,0 +1,59 @@
/**
******************************************************************************
* @file pathactioneditorgadget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PathActionEditorGADGET_H_
#define PathActionEditorGADGET_H_
#include <coreplugin/iuavgadget.h>
namespace Core {
class IUAVGadget;
}
//class QWidget;
//class QString;
class PathActionEditorGadgetWidget;
using namespace Core;
class PathActionEditorGadget : public Core::IUAVGadget
{
Q_OBJECT
public:
PathActionEditorGadget(QString classId, PathActionEditorGadgetWidget *widget, QWidget *parent = 0);
~PathActionEditorGadget();
QList<int> context() const { return m_context; }
QWidget *widget() { return m_widget; }
QString contextHelpId() const { return QString(); }
void loadConfiguration(IUAVGadgetConfiguration* config);
private:
QWidget *m_widget;
QList<int> m_context;
};
#endif // PathActionEditorGADGET_H_

47
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.cpp Normal file
View File

@ -0,0 +1,47 @@
/**
******************************************************************************
* @file pathactioneditorgadgetfactor.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pathactioneditorgadgetfactory.h"
#include "pathactioneditorgadgetwidget.h"
#include "pathactioneditorgadget.h"
#include <coreplugin/iuavgadget.h>
#include <QDebug>
PathActionEditorGadgetFactory::PathActionEditorGadgetFactory(QObject *parent) :
IUAVGadgetFactory(QString("PathActionEditorGadget"),
tr("PathAction Editor"),
parent)
{
}
PathActionEditorGadgetFactory::~PathActionEditorGadgetFactory()
{
}
IUAVGadget* PathActionEditorGadgetFactory::createGadget(QWidget *parent) {
PathActionEditorGadgetWidget* gadgetWidget = new PathActionEditorGadgetWidget(parent);
return new PathActionEditorGadget(QString("PathActionEditorGadget"), gadgetWidget, parent);
}

49
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetfactory.h Normal file
View File

@ -0,0 +1,49 @@
/**
******************************************************************************
* @file pathactioneditorgadgetfactory.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PathActionEditorGADGETFACTORY_H_
#define PathActionEditorGADGETFACTORY_H_
#include <coreplugin/iuavgadgetfactory.h>
namespace Core {
class IUAVGadget;
class IUAVGadgetFactory;
}
using namespace Core;
class PathActionEditorGadgetFactory : public IUAVGadgetFactory
{
Q_OBJECT
public:
PathActionEditorGadgetFactory(QObject *parent = 0);
~PathActionEditorGadgetFactory();
IUAVGadget *createGadget(QWidget *parent);
};
#endif // PathActionEditorGADGETFACTORY_H_

113
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.cpp Normal file
View File

@ -0,0 +1,113 @@
/**
******************************************************************************
* @file pathactioneditorgadgetwidget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pathactioneditorgadgetwidget.h"
#include "ui_pathactioneditor.h"
#include <QDebug>
#include <QString>
#include <QStringList>
#include <QtGui/QWidget>
#include <QtGui/QTextEdit>
#include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton>
#include "browseritemdelegate.h"
#include "extensionsystem/pluginmanager.h"
PathActionEditorGadgetWidget::PathActionEditorGadgetWidget(QWidget *parent) : QLabel(parent)
{
m_pathactioneditor = new Ui_PathActionEditor();
m_pathactioneditor->setupUi(this);
m_model = new PathActionEditorTreeModel();
m_pathactioneditor->pathactions->setModel(m_model);
m_pathactioneditor->pathactions->setColumnWidth(0, 300);
m_pathactioneditor->pathactions->setColumnWidth(1, 500);
m_pathactioneditor->pathactions->expandAll();
BrowserItemDelegate *m_delegate = new BrowserItemDelegate();
m_pathactioneditor->pathactions->setItemDelegate(m_delegate);
m_pathactioneditor->pathactions->setEditTriggers(QAbstractItemView::AllEditTriggers);
m_pathactioneditor->pathactions->setSelectionBehavior(QAbstractItemView::SelectItems);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm != NULL);
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(objManager != NULL);
pathactionObj = PathAction::GetInstance(objManager);
Q_ASSERT(pathactionObj != NULL);
waypointObj = Waypoint::GetInstance(objManager);
Q_ASSERT(waypointObj != NULL);
// Connect the signals
connect(m_pathactioneditor->buttonNewPathAction, SIGNAL(clicked()),
this, SLOT(addPathActionInstance()));
connect(m_pathactioneditor->buttonNewWaypoint, SIGNAL(clicked()),
this, SLOT(addWaypointInstance()));
}
PathActionEditorGadgetWidget::~PathActionEditorGadgetWidget()
{
// Do nothing
}
void PathActionEditorGadgetWidget::pathactionChanged(UAVObject *)
{
}
void PathActionEditorGadgetWidget::addPathActionInstance()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm != NULL);
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(objManager != NULL);
qDebug() << "Instances before: " << objManager->getNumInstances(pathactionObj->getObjID());
PathAction *obj = new PathAction();
quint32 newInstId = objManager->getNumInstances(pathactionObj->getObjID());
obj->initialize(newInstId,obj->getMetaObject());
objManager->registerObject(obj);
qDebug() << "Instances after: " << objManager->getNumInstances(pathactionObj->getObjID());
}
void PathActionEditorGadgetWidget::addWaypointInstance()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm != NULL);
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(objManager != NULL);
qDebug() << "Instances before: " << objManager->getNumInstances(waypointObj->getObjID());
Waypoint *obj = new Waypoint();
quint32 newInstId = objManager->getNumInstances(waypointObj->getObjID());
obj->initialize(newInstId,obj->getMetaObject());
objManager->registerObject(obj);
qDebug() << "Instances after: " << objManager->getNumInstances(waypointObj->getObjID());
}
/**
* @}
* @}
*/

61
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorgadgetwidget.h Normal file
View File

@ -0,0 +1,61 @@
/**
******************************************************************************
* @file pathactioneditorgadgetwidget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PathActionEditorGADGETWIDGET_H_
#define PathActionEditorGADGETWIDGET_H_
#include <QtGui/QLabel>
#include <QtGui/QWidget>
#include <QtGui/QTreeView>
#include "pathaction.h"
#include "waypoint.h"
#include "pathactioneditortreemodel.h"
class Ui_PathActionEditor;
class PathActionEditorGadgetWidget : public QLabel
{
Q_OBJECT
public:
PathActionEditorGadgetWidget(QWidget *parent = 0);
~PathActionEditorGadgetWidget();
signals:
protected slots:
void pathactionChanged(UAVObject *);
void addPathActionInstance();
void addWaypointInstance();
private:
Ui_PathActionEditor * m_pathactioneditor;
PathActionEditorTreeModel *m_model;
PathAction *pathactionObj;
Waypoint *waypointObj;
};
#endif /* PathActionEditorGADGETWIDGET_H_ */

68
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.cpp Normal file
View File

@ -0,0 +1,68 @@
/**
******************************************************************************
* @file pathactioneditorplugin.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pathactioneditorplugin.h"
#include "pathactioneditorgadgetfactory.h"
#include <QDebug>
#include <QtPlugin>
#include <QStringList>
#include <extensionsystem/pluginmanager.h>
PathActionEditorPlugin::PathActionEditorPlugin()
{
// Do nothing
}
PathActionEditorPlugin::~PathActionEditorPlugin()
{
// Do nothing
}
bool PathActionEditorPlugin::initialize(const QStringList& args, QString *errMsg)
{
Q_UNUSED(args);
Q_UNUSED(errMsg);
mf = new PathActionEditorGadgetFactory(this);
addAutoReleasedObject(mf);
return true;
}
void PathActionEditorPlugin::extensionsInitialized()
{
// Do nothing
}
void PathActionEditorPlugin::shutdown()
{
// Do nothing
}
Q_EXPORT_PLUGIN(PathActionEditorPlugin)
/**
* @}
* @}
*/

46
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditorplugin.h Normal file
View File

@ -0,0 +1,46 @@
/**
******************************************************************************
* @file pathactioneditorplugin.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @addtogroup PathAction Editor GCS Plugins
* @{
* @addtogroup PathActionEditorGadgetPlugin PathAction Editor Gadget Plugin
* @{
* @brief A gadget to edit a list of pathactions
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PathActionEditorPLUGIN_H_
#define PathActionEditorPLUGIN_H_
#include <extensionsystem/iplugin.h>
class PathActionEditorGadgetFactory;
class PathActionEditorPlugin : public ExtensionSystem::IPlugin
{
public:
PathActionEditorPlugin();
~PathActionEditorPlugin();
void extensionsInitialized();
bool initialize(const QStringList & arguments, QString * errorString);
void shutdown();
private:
PathActionEditorGadgetFactory *mf;
};
#endif /* PathActionEditorPLUGIN_H_ */

423
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.cpp Normal file
View File

@ -0,0 +1,423 @@
/**
******************************************************************************
*
* @file pathactioneditortreemodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pathactioneditortreemodel.h"
#include "pathactioneditorgadget.h"
#include "fieldtreeitem.h"
#include "uavobjectmanager.h"
#include "uavdataobject.h"
#include "uavmetaobject.h"
#include "uavobjectfield.h"
#include "extensionsystem/pluginmanager.h"
#include <QtGui/QColor>
//#include <QtGui/QIcon>
#include <QMessageBox>
#include <QtCore/QSignalMapper>
#include <QtCore/QDebug>
#include "waypoint.h"
#include "waypointactive.h"
#include "pathaction.h"
PathActionEditorTreeModel::PathActionEditorTreeModel(QObject *parent) :
QAbstractItemModel(parent),
m_recentlyUpdatedColor(QColor(255, 230, 230)),
m_manuallyChangedColor(QColor(230, 230, 255))
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
m_objManager = pm->getObject<UAVObjectManager>();
connect(m_objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
connect(m_objManager->getObject("WaypointActive"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
connect(m_objManager->getObject("PathAction"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
connect(m_objManager->getObject("Waypoint"),SIGNAL(objectUpdated(UAVObject*)), this, SLOT(objUpdated(UAVObject*)));
setupModelData();
}
PathActionEditorTreeModel::~PathActionEditorTreeModel()
{
delete m_rootItem;
}
void PathActionEditorTreeModel::setupModelData()
{
m_actions = new QStringList();
updateActions();
// root
QList<QVariant> rootData;
rootData << tr("Property") << tr("Value") << tr("Unit");
m_rootItem = new TreeItem(rootData);
m_pathactionsTree = new TopTreeItem(tr("PathActions"), m_rootItem);
m_rootItem->appendChild(m_pathactionsTree);
m_waypointsTree = new TopTreeItem(tr("Waypoints"), m_rootItem);
m_rootItem->appendChild(m_waypointsTree);
connect(m_rootItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
connect(m_pathactionsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
connect(m_waypointsTree, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
{
QList<UAVObject*> list = m_objManager->getObjectInstances("PathAction");
foreach (UAVObject* obj, list) {
addInstance(obj,m_pathactionsTree);
}
}
{
QList<UAVObject*> list = m_objManager->getObjectInstances("Waypoint");
foreach (UAVObject* obj, list) {
addInstance(obj,m_waypointsTree);
}
}
}
void PathActionEditorTreeModel::updateActions() {
m_actions->clear();
QList<UAVObject*> list = m_objManager->getObjectInstances("PathAction");
foreach (UAVObject* obj, list) {
QString title;
title.append((QVariant(obj->getInstID()).toString()));
title.append(" ");
title.append((obj->getField("Mode")->getValue().toString()));
title.append(" ");
title.append((obj->getField("Command")->getValue().toString()));
title.append(":");
title.append((obj->getField("EndCondition")->getValue().toString()));
title.append(" ");
m_actions->append(title);
}
}
void PathActionEditorTreeModel::addInstance(UAVObject *obj, TreeItem *parent)
{
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*)));
TreeItem *item;
QString name = QString::number(obj->getInstID());
item = new InstanceTreeItem(obj, name);
connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
parent->appendChild(item);
foreach (UAVObjectField *field, obj->getFields()) {
if (field->getNumElements() > 1) {
addArrayField(field, item);
} else {
addSingleField(0, field, item);
}
}
}
void PathActionEditorTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent)
{
TreeItem *item = new ArrayFieldTreeItem(field->getName());
connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
for (uint i = 0; i < field->getNumElements(); ++i) {
addSingleField(i, field, item);
}
parent->appendChild(item);
}
void PathActionEditorTreeModel::addSingleField(int index, UAVObjectField *field, TreeItem *parent)
{
QList<QVariant> data;
if (field->getNumElements() == 1)
data.append(field->getName());
else
data.append( QString("[%1]").arg((field->getElementNames())[index]) );
FieldTreeItem *item;
UAVObjectField::FieldType type = field->getType();
// hack: list available actions in an enum
if (field->getName().compare("Action")==0 && type==UAVObjectField::UINT8) {
data.append( field->getValue(index).toInt());
data.append( field->getUnits());
item = new ActionFieldTreeItem(field, index, data, m_actions);
} else {
switch (type) {
case UAVObjectField::ENUM: {
QStringList options = field->getOptions();
QVariant value = field->getValue();
data.append( options.indexOf(value.toString()) );
data.append(field->getUnits());
item = new EnumFieldTreeItem(field, index, data);
break;
}
case UAVObjectField::INT8:
case UAVObjectField::INT16:
case UAVObjectField::INT32:
case UAVObjectField::UINT8:
case UAVObjectField::UINT16:
case UAVObjectField::UINT32:
data.append(field->getValue(index));
data.append(field->getUnits());
item = new IntFieldTreeItem(field, index, data);
break;
case UAVObjectField::FLOAT32:
data.append(field->getValue(index));
data.append(field->getUnits());
item = new FloatFieldTreeItem(field, index, data);
break;
default:
Q_ASSERT(false);
}
}
connect(item, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*)));
parent->appendChild(item);
}
QModelIndex PathActionEditorTreeModel::index(int row, int column, const QModelIndex &parent)
const
{
if (!hasIndex(row, column, parent))
return QModelIndex();
TreeItem *parentItem;
if (!parent.isValid())
parentItem = m_rootItem;
else
parentItem = static_cast<TreeItem*>(parent.internalPointer());
TreeItem *childItem = parentItem->child(row);
if (childItem)
return createIndex(row, column, childItem);
else
return QModelIndex();
}
QModelIndex PathActionEditorTreeModel::index(TreeItem *item)
{
if (item->parent() == 0)
return QModelIndex();
QModelIndex root = index(item->parent());
for (int i = 0; i < rowCount(root); ++i) {
QModelIndex childIndex = index(i, 0, root);
TreeItem *child = static_cast<TreeItem*>(childIndex.internalPointer());
if (child == item)
return childIndex;
}
Q_ASSERT(false);
return QModelIndex();
}
QModelIndex PathActionEditorTreeModel::parent(const QModelIndex &index) const
{
if (!index.isValid())
return QModelIndex();
TreeItem *childItem = static_cast<TreeItem*>(index.internalPointer());
TreeItem *parentItem = childItem->parent();
if (parentItem == m_rootItem)
return QModelIndex();
return createIndex(parentItem->row(), 0, parentItem);
}
int PathActionEditorTreeModel::rowCount(const QModelIndex &parent) const
{
TreeItem *parentItem;
if (parent.column() > 0)
return 0;
if (!parent.isValid())
parentItem = m_rootItem;
else
parentItem = static_cast<TreeItem*>(parent.internalPointer());
return parentItem->childCount();
}
int PathActionEditorTreeModel::columnCount(const QModelIndex &parent) const
{
if (parent.isValid())
return static_cast<TreeItem*>(parent.internalPointer())->columnCount();
else
return m_rootItem->columnCount();
}
QVariant PathActionEditorTreeModel::data(const QModelIndex &index, int role) const
{
if (!index.isValid())
return QVariant();
if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) {
TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
return item->data(index.column());
}
// if (role == Qt::DecorationRole)
// return QIcon(":/core/images/openpilot_logo_128.png");
if (role == Qt::ToolTipRole) {
TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
return item->description();
}
TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
if (index.column() == 0 && role == Qt::BackgroundRole) {
ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(item);
if (objItem && objItem->highlighted())
return QVariant(m_recentlyUpdatedColor);
}
if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) {
FieldTreeItem *fieldItem = dynamic_cast<FieldTreeItem*>(item);
if (fieldItem && fieldItem->highlighted())
return QVariant(m_recentlyUpdatedColor);
if (fieldItem && fieldItem->changed())
return QVariant(m_manuallyChangedColor);
}
if (role != Qt::DisplayRole)
return QVariant();
if (index.column() == TreeItem::dataColumn) {
EnumFieldTreeItem *fieldItem = dynamic_cast<EnumFieldTreeItem*>(item);
if (fieldItem) {
int enumIndex = fieldItem->data(index.column()).toInt();
return fieldItem->enumOptions(enumIndex);
} else {
ActionFieldTreeItem *afieldItem = dynamic_cast<ActionFieldTreeItem*>(item);
if (afieldItem) {
int enumIndex = afieldItem->data(index.column()).toInt();
return afieldItem->enumOptions(enumIndex);
}
}
}
return item->data(index.column());
}
bool PathActionEditorTreeModel::setData(const QModelIndex &index, const QVariant & value, int role)
{
Q_UNUSED(role)
TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
item->setData(value, index.column());
return true;
}
Qt::ItemFlags PathActionEditorTreeModel::flags(const QModelIndex &index) const
{
if (!index.isValid())
return 0;
if (index.column() == TreeItem::dataColumn) {
TreeItem *item = static_cast<TreeItem*>(index.internalPointer());
if (item->isEditable())
return Qt::ItemIsEnabled | Qt::ItemIsSelectable | Qt::ItemIsEditable;
}
return Qt::ItemIsEnabled | Qt::ItemIsSelectable;
}
QVariant PathActionEditorTreeModel::headerData(int section, Qt::Orientation orientation,
int role) const
{
if (orientation == Qt::Horizontal && role == Qt::DisplayRole)
return m_rootItem->data(section);
return QVariant();
}
void PathActionEditorTreeModel::updateHighlight(TreeItem *item)
{
QModelIndex itemIndex = index(item);
Q_ASSERT(itemIndex != QModelIndex());
emit dataChanged(itemIndex, itemIndex.sibling(itemIndex.row(), TreeItem::dataColumn));
// update uavobject if any - go up the tree until there is
ObjectTreeItem *objItem = 0;
TreeItem *searchItem = item;
while (searchItem) {
objItem = dynamic_cast<ObjectTreeItem*>(searchItem);
if (objItem)
break;
searchItem = searchItem->parent();
}
if (objItem) {
item->apply();
objItem->apply();
UAVObject *obj = objItem->object();
Q_ASSERT(obj);
obj->updated();
}
}
void PathActionEditorTreeModel::highlightUpdatedObject(UAVObject *obj)
{
Q_ASSERT(obj);
if (obj->getName().compare("Waypoint")==0) {
m_waypointsTree->update();
//emit dataChanged(index(m_waypointsTree), index(m_waypointsTree));
} else if (obj->getName().compare("PathAction")==0) {
m_pathactionsTree->update();
//emit dataChanged(index(m_pathactionsTree), index(m_pathactionsTree));
}
}
void PathActionEditorTreeModel::newInstance(UAVObject *obj)
{
if (obj->getName().compare("Waypoint")==0) {
addInstance(obj,m_waypointsTree);
m_waypointsTree->update();
} else if (obj->getName().compare("PathAction")==0) {
addInstance(obj,m_pathactionsTree);
m_pathactionsTree->update();
}
updateActions();
emit layoutChanged();
}
void PathActionEditorTreeModel::objUpdated(UAVObject *obj)
{
quint16 index = m_objManager->getObject("WaypointActive")->getField("Index")->getValue().toInt();
quint16 action;
foreach (TreeItem *child,m_waypointsTree->treeChildren()) {
ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(child);
if (index == objItem->object()->getInstID()) {
child->setActive(true);
action = objItem->object()->getField("Action")->getValue().toInt();
} else {
child->setActive(false);
}
}
foreach (TreeItem *child,m_pathactionsTree->treeChildren()) {
ObjectTreeItem *objItem = dynamic_cast<ObjectTreeItem*>(child);
if (action == objItem->object()->getInstID()) {
child->setActive(true);
} else {
child->setActive(false);
}
}
updateActions();
emit layoutChanged();
}

98
ground/openpilotgcs/src/plugins/pathactioneditor/pathactioneditortreemodel.h Normal file
View File

@ -0,0 +1,98 @@
/**
******************************************************************************
*
* @file pathactioneditortreemodel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PATHACTIONEDITORTREEMODEL_H
#define PATHACTIONEDITORTREEMODEL_H
#include "treeitem.h"
#include <QAbstractItemModel>
#include <QtCore/QMap>
#include <QtGui/QColor>
class TopTreeItem;
class ObjectTreeItem;
class DataObjectTreeItem;
class UAVObject;
class UAVDataObject;
class UAVMetaObject;
class UAVObjectField;
class UAVObjectManager;
class QSignalMapper;
class QTimer;
class PathActionEditorTreeModel : public QAbstractItemModel
{
Q_OBJECT
public:
explicit PathActionEditorTreeModel(QObject *parent = 0);
~PathActionEditorTreeModel();
QVariant data(const QModelIndex &index, int role) const;
bool setData(const QModelIndex &index, const QVariant & value, int role);
Qt::ItemFlags flags(const QModelIndex &index) const;
QVariant headerData(int section, Qt::Orientation orientation,
int role = Qt::DisplayRole) const;
QModelIndex index(int row, int column,
const QModelIndex &parent = QModelIndex()) const;
QModelIndex parent(const QModelIndex &index) const;
int rowCount(const QModelIndex &parent = QModelIndex()) const;
int columnCount(const QModelIndex &parent = QModelIndex()) const;
void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; }
void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; }
signals:
public slots:
void newInstance(UAVObject *obj);
void objUpdated(UAVObject *obj);
private slots:
void highlightUpdatedObject(UAVObject *obj);
void updateHighlight(TreeItem*);
private:
QModelIndex index(TreeItem *item);
void addArrayField(UAVObjectField *field, TreeItem *parent);
void addSingleField(int index, UAVObjectField *field, TreeItem *parent);
void addInstance(UAVObject *obj, TreeItem *parent);
//QString updateMode(quint8 updateMode);
void setupModelData();
void updateActions();
UAVObjectManager *m_objManager;
QStringList *m_actions;
TreeItem *m_rootItem;
TopTreeItem *m_pathactionsTree;
TopTreeItem *m_waypointsTree;
QColor m_recentlyUpdatedColor;
QColor m_manuallyChangedColor;
};
#endif // PATHACTIONEDITORTREEMODEL_H

122
ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.cpp Normal file
View File

@ -0,0 +1,122 @@
/**
******************************************************************************
*
* @file treeitem.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "treeitem.h"
TreeItem::TreeItem(const QList<QVariant> &data, TreeItem *parent) :
QObject(0),
m_data(data),
m_parent(parent),
m_highlight(false),
m_changed(false)
{
}
TreeItem::TreeItem(const QVariant &data, TreeItem *parent) :
QObject(0),
m_parent(parent),
m_highlight(false),
m_changed(false)
{
m_data << data << "" << "";
}
TreeItem::~TreeItem()
{
qDeleteAll(m_children);
}
void TreeItem::appendChild(TreeItem *child)
{
m_children.append(child);
child->setParentTree(this);
}
void TreeItem::insert(int index, TreeItem *child)
{
m_children.insert(index, child);
child->setParentTree(this);
}
TreeItem *TreeItem::child(int row)
{
return m_children.value(row);
}
int TreeItem::childCount() const
{
return m_children.count();
}
int TreeItem::row() const
{
if (m_parent)
return m_parent->m_children.indexOf(const_cast<TreeItem*>(this));
return 0;
}
int TreeItem::columnCount() const
{
return m_data.count();
}
QVariant TreeItem::data(int column) const
{
return m_data.value(column);
}
void TreeItem::setData(QVariant value, int column)
{
m_data.replace(column, value);
}
void TreeItem::update() {
foreach(TreeItem *child, treeChildren())
child->update();
}
void TreeItem::apply() {
foreach(TreeItem *child, treeChildren())
child->apply();
}
void TreeItem::setHighlight(bool highlight) {
//m_highlight = highlight;
m_changed = false;
}
void TreeItem::setActive(bool highlight) {
m_highlight = highlight;
foreach(TreeItem *child, treeChildren())
child->setActive(highlight);
}
void TreeItem::removeHighlight() {
m_highlight = false;
update();
}

188
ground/openpilotgcs/src/plugins/pathactioneditor/treeitem.h Normal file
View File

@ -0,0 +1,188 @@
/**
******************************************************************************
*
* @file treeitem.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup UAVObjectBrowserPlugin UAVObject Browser Plugin
* @{
* @brief The UAVObject Browser gadget plugin
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef TREEITEM_H
#define TREEITEM_H
#include "uavobject.h"
#include "uavmetaobject.h"
#include "uavobjectfield.h"
#include <QtCore/QList>
#include <QtCore/QVariant>
#include <QtCore/QObject>
class TreeItem : public QObject
{
Q_OBJECT
public:
TreeItem(const QList<QVariant> &data, TreeItem *parent = 0);
TreeItem(const QVariant &data, TreeItem *parent = 0);
virtual ~TreeItem();
void appendChild(TreeItem *child);
void insert(int index, TreeItem *child);
TreeItem *child(int row);
inline QList<TreeItem*> treeChildren() const { return m_children; }
int childCount() const;
int columnCount() const;
QVariant data(int column = 1) const;
QString description() { return m_description; }
void setDescription(QString d) { // Split around 40 characters
int idx = d.indexOf(" ",40);
d.insert(idx,QString("<br>"));
d.remove("@Ref", Qt::CaseInsensitive);
m_description = d;
}
// only column 1 (TreeItem::dataColumn) is changed with setData currently
// other columns are initialized in constructor
virtual void setData(QVariant value, int column = 1);
int row() const;
TreeItem *parent() { return m_parent; }
void setParentTree(TreeItem *parent) { m_parent = parent; }
inline virtual bool isEditable() { return false; }
virtual void update();
virtual void apply();
inline bool highlighted() { return m_highlight; }
void setHighlight(bool highlight);
void setActive(bool highlight);
inline bool changed() { return m_changed; }
inline void setChanged(bool changed) { m_changed = changed; if(changed) emit updateHighlight(this); }
signals:
void updateHighlight(TreeItem*);
private slots:
void removeHighlight();
private:
QList<TreeItem*> m_children;
// m_data contains: [0] property name, [1] value, [2] unit
QList<QVariant> m_data;
QString m_description;
TreeItem *m_parent;
bool m_highlight;
bool m_changed;
public:
static const int dataColumn = 1;
private:
};
class TopTreeItem : public TreeItem
{
Q_OBJECT
public:
TopTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
QList<quint32> objIds() { return m_objIds; }
void addObjId(quint32 objId) { m_objIds.append(objId); }
void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); }
int nameIndex(QString name) {
for (int i = 0; i < childCount(); ++i) {
if (name < child(i)->data(0).toString())
return i;
}
return childCount();
}
private:
QList<quint32> m_objIds;
};
class ObjectTreeItem : public TreeItem
{
Q_OBJECT
public:
ObjectTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) :
TreeItem(data, parent), m_obj(0) { }
ObjectTreeItem(const QVariant &data, TreeItem *parent = 0) :
TreeItem(data, parent), m_obj(0) { }
void setObject(UAVObject *obj) { m_obj = obj; setDescription(obj->getDescription()); }
inline UAVObject *object() { return m_obj; }
private:
UAVObject *m_obj;
};
class MetaObjectTreeItem : public ObjectTreeItem
{
Q_OBJECT
public:
MetaObjectTreeItem(UAVObject *obj, const QList<QVariant> &data, TreeItem *parent = 0) :
ObjectTreeItem(data, parent) { setObject(obj); }
MetaObjectTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) :
ObjectTreeItem(data, parent) { setObject(obj); }
};
class DataObjectTreeItem : public ObjectTreeItem
{
Q_OBJECT
public:
DataObjectTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) :
ObjectTreeItem(data, parent) { }
DataObjectTreeItem(const QVariant &data, TreeItem *parent = 0) :
ObjectTreeItem(data, parent) { }
virtual void apply() {
foreach(TreeItem *child, treeChildren()) {
MetaObjectTreeItem *metaChild = dynamic_cast<MetaObjectTreeItem*>(child);
if (!metaChild)
child->apply();
}
}
virtual void update() {
foreach(TreeItem *child, treeChildren()) {
MetaObjectTreeItem *metaChild = dynamic_cast<MetaObjectTreeItem*>(child);
if (!metaChild)
child->update();
}
}
};
class InstanceTreeItem : public DataObjectTreeItem
{
Q_OBJECT
public:
InstanceTreeItem(UAVObject *obj, const QList<QVariant> &data, TreeItem *parent = 0) :
DataObjectTreeItem(data, parent) { setObject(obj); }
InstanceTreeItem(UAVObject *obj, const QVariant &data, TreeItem *parent = 0) :
DataObjectTreeItem(data, parent) { setObject(obj); }
virtual void apply() { TreeItem::apply(); }
virtual void update() { TreeItem::update(); }
};
class ArrayFieldTreeItem : public TreeItem
{
Q_OBJECT
public:
ArrayFieldTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
ArrayFieldTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
};
#endif // TREEITEM_H

6
ground/openpilotgcs/src/plugins/plugins.pro
View File

@ -134,6 +134,12 @@ plugin_qmlview.depends = plugin_coreplugin
plugin_qmlview.depends += plugin_uavobjects
SUBDIRS += plugin_qmlview
# PathAction Editor gadget
plugin_pathactioneditor.subdir = pathactioneditor
plugin_pathactioneditor.depends = plugin_coreplugin
plugin_pathactioneditor.depends += plugin_uavobjects
SUBDIRS += plugin_pathactioneditor
# Waypoint Editor gadget
plugin_waypointeditor.subdir = waypointeditor
plugin_waypointeditor.depends = plugin_coreplugin

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

@ -55,8 +55,10 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/gpsposition.h \
$$UAVOBJECT_SYNTHETICS/gpstime.h \
$$UAVOBJECT_SYNTHETICS/gpssatellites.h \
$$UAVOBJECT_SYNTHETICS/pathaction.h \
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
$$UAVOBJECT_SYNTHETICS/pathplannersettings.h \
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
$$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
$$UAVOBJECT_SYNTHETICS/positionactual.h \
$$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
@ -65,7 +67,9 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/mixerstatus.h \
$$UAVOBJECT_SYNTHETICS/velocitydesired.h \
$$UAVOBJECT_SYNTHETICS/velocityactual.h \
$$UAVOBJECT_SYNTHETICS/guidancesettings.h \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.h \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.h \
$$UAVOBJECT_SYNTHETICS/ratedesired.h \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.h \
$$UAVOBJECT_SYNTHETICS/i2cstats.h \
@ -124,8 +128,10 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/gpsposition.cpp \
$$UAVOBJECT_SYNTHETICS/gpstime.cpp \
$$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
$$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
$$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
$$UAVOBJECT_SYNTHETICS/positionactual.cpp \
$$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
@ -134,7 +140,9 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/mixerstatus.cpp \
$$UAVOBJECT_SYNTHETICS/velocitydesired.cpp \
$$UAVOBJECT_SYNTHETICS/velocityactual.cpp \
$$UAVOBJECT_SYNTHETICS/guidancesettings.cpp \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/fixedwingpathfollowerstatus.cpp \
$$UAVOBJECT_SYNTHETICS/vtolpathfollowersettings.cpp \
$$UAVOBJECT_SYNTHETICS/ratedesired.cpp \
$$UAVOBJECT_SYNTHETICS/firmwareiapobj.cpp \
$$UAVOBJECT_SYNTHETICS/i2cstats.cpp \

55
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml Normal file
View File

@ -0,0 +1,55 @@
<xml>
<object name="FixedWingPathFollowerSettings" singleinstance="true" settings="true">
<description>Settings for the @ref FixedWingPathFollowerModule</description>
<!-- these coefficients control desired movement in airspace -->
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="15"/>
<!-- maximum speed the aircraft can reach in level flight at full throttle without loosing altitude - leave some safety margin -->
<field name="AirSpeedMin" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- stall speed - leave some safety margin -->
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- maximum allowed climb or sink rate in guided flight-->
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
<!-- how many seconds to plan the flight vector ahead when initiating necessary course changes - increase for planes with sluggish response -->
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
<!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
<!-- proportional coefficient for desired vertical speed in relation to altitude displacement-->
<!-- these coefficients control actual control outputs -->
<field name="CoursePI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
<!-- coefficients for desired bank angle in relation to ground heading error - this controls heading -->
<field name="SpeedP" units="(m/s^2) / ((m/s)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/>
<!-- coefficients for desired acceleration
in relation to relative airspeed error (IASerror/IASactual)-->
<field name="AccelPI" units="deg / (m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, 1.5, 20"/>
<!-- coefficients for desired pitch
in relation to acceleration error -->
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="50, 5"/>
<!-- coefficients for adjusting desired pitch
in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->
<field name="AirspeedToVerticalCrossFeed" units="(m/s) / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="100, 100"/>
<!-- proportional coefficient for adjusting vertical speed error for power calculation
in relation to relative airspeed error (IASerror/IASactual) -->
<field name="PowerPI" units="1/(m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.01,0.01,0.8"/>
<!-- proportional coefficient for desired throttle
in relation to vertical speed error (absolute but including crossfeed) -->
<!-- output limits -->
<field name="RollLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-35,0,35" />
<!-- maximum allowed bank angles in navigates flight -->
<field name="PitchLimit" units="deg" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="-10,5,20" />
<!-- maximum allowed pitch angles and setpoint for neutral pitch -->
<field name="ThrottleLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
<!-- minimum and maximum allowed throttle and setpoint for cruise speed -->
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
<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>

13
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml Normal file
View File

@ -0,0 +1,13 @@
<xml>
<object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
<description>Object Storing Debugging Information on PID internals</description>
<field name="E" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="A" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="C" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Lowspeed,Highspeed,Lowpower,Highpower,Pitchcontrol" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="500"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

2
shared/uavobjectdefinition/flightstatus.xml
View File

@ -4,7 +4,7 @@
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner"/>
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -18,7 +18,7 @@
<field name="USB_HIDPort" units="function" type="enum" elements="1" options="USBTelemetry,Disabled" defaultvalue="USBTelemetry"/>
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,VtolPathFollower,FixedWingPathFollower" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -23,7 +23,7 @@
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling" defaultvalue="Attitude,Attitude,Rate"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold,ReturnToBase,PathPlanner" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>

30
shared/uavobjectdefinition/pathaction.xml Normal file
View File

@ -0,0 +1,30 @@
<xml>
<object name="PathAction" singleinstance="false" settings="false">
<description>A waypoint command the pathplanner is to use at a certain waypoint</description>
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude,
SetAccessory,
DisarmAlarm" default="Endpoint" />
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,BelowError,
AboveAltitude,AboveSpeed,
PointingTowardsNext,
PythonScript,
Immediate" default="None" />
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>
<logging updatemode="periodic" period="1000"/>
</object>
</xml>

15
shared/uavobjectdefinition/pathdesired.xml
View File

@ -7,9 +7,18 @@
<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path" default="0"/>
<!-- Endpoint Mode - Fly to end location and attempt to stay there -->
<!-- Path Mode - Attempt to fly a line from Start to End using specifed velocity -->
<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
FixedAttitude,
SetAccessory,
DisarmAlarm" default="0"/>
<!-- Endpoint mode - move directly towards endpoint regardless of position -->
<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->
<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="UID" units="" type="int16" elements="1" default="0"/>
<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

1
shared/uavobjectdefinition/pathplannersettings.xml
View File

@ -1,7 +1,6 @@
<xml>
<object name="PathPlannerSettings" singleinstance="true" settings="true">
<description>Settings for the @ref PathPlanner Module</description>
<field name="PathMode" units="" type="enum" elements="1" options="ENDPOINT,PATH" defaultvalue="PATH"/>
<field name="PreprogrammedPath" units="" type="enum" elements="1" options="NONE,10M_BOX,LOGO" defaultvalue="NONE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

16
shared/uavobjectdefinition/pathstatus.xml Normal file
View File

@ -0,0 +1,16 @@
<xml>
<object name="PathStatus" singleinstance="true" settings="false">
<description>Status of the current path mode Can come from any @ref PathFollower module</description>
<field name="UID" units="" type="int16" elements="1" default="0"/>
<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
<field name="Status" units="" type="enum" elements="1" options="InProgress,Completed,Warning,Critical"/>
<field name="fractional_progress" units="" type="float" elements="1"/>
<field name="error" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="0"/>
<logging updatemode="onchange" period="0"/>
</object>
</xml>

4
shared/uavobjectdefinition/guidancesettings.xml → shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -1,6 +1,6 @@
<xml>
<object name="GuidanceSettings" singleinstance="true" settings="true">
<description>Settings for the @ref GuidanceModule</description>
<object name="VtolPathFollowerSettings" singleinstance="true" settings="true">
<description>Settings for the @ref VtolPathFollowerModule</description>
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>

5
shared/uavobjectdefinition/waypoint.xml
View File

@ -2,9 +2,8 @@
<object name="Waypoint" singleinstance="false" settings="false">
<description>A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module</description>
<field name="Position" units="m" type="float" elementnames="North, East, Down"/>
<field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/>
<field name="YawDesired" units="deg" type="float" elements="1"/>
<field name="Action" units="" type="enum" elements="1" options="Next,RTH,Loiter10s,Land"/>
<field name="Velocity" units="m/s" type="float" elements="1"/>
<field name="Action" units="" type="uint8" elements="1" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/>

2
shared/uavobjectdefinition/waypointactive.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="WaypointActive" singleinstance="true" settings="false">
<description>Indicates the currently active waypoint</description>
<field name="Index" units="" type="uint8" elements="1"/>
<field name="Index" units="" type="int16" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="onchange" period="0"/>