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

Get navigation working and program hardcoded flight path

Browse Source
This commit is contained in:
James Cotton 2012-03-15 03:19:32 -05:00
parent cd5fad2ae9
commit ba1ade33cb
2 changed files with 63 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/Modules/Guidance/guidance.c
View File

@ -201,13 +201,15 @@ static void guidanceTask(void *parameters)
SystemSettingsGet(&systemSettings); SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings); GuidanceSettingsGet(&guidanceSettings);
if ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && if ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ||
flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) || ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||
(systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) )) (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))
{ {
if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ) if( flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ||
flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
updateVtolDesiredVelocity(); updateVtolDesiredVelocity();
else else
manualSetDesiredVelocity(); manualSetDesiredVelocity();

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

@ -37,7 +37,7 @@
#include "waypointactive.h" #include "waypointactive.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 1500 #define STACK_SIZE_BYTES 2500
#define TASK_PRIORITY (tskIDLE_PRIORITY+1) #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
@ -85,13 +85,42 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/** /**
* Module task * Module task
*/ */
int32_t bad_inits;
int32_t bad_reads;
static void pathPlannerTask(void *parameters) static void pathPlannerTask(void *parameters)
{ {
FlightStatusData flightStatus; FlightStatusData flightStatus;
PositionActualData positionActual; PositionActualData positionActual;
PositionDesiredData positionDesired;
WaypointActiveData waypointActive; WaypointActiveData waypointActive;
WaypointData waypoint; WaypointData waypoint;
for(uint32_t i = 0; i < 20; i++) {
waypoint.Position[1] = 30 * cos(i / 10.0 * M_PI);
waypoint.Position[0] = 50 * sin(i / 10.0 * M_PI);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
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[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
waypoint.Position[1] = 35;
waypoint.Position[0] = -50;
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_RTH;
WaypointCreateInstance();
WaypointInstSet(35, &waypoint);
const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m) const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m)
@ -100,12 +129,14 @@ static void pathPlannerTask(void *parameters)
{ {
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
vTaskDelay(100);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER)
continue; continue;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
WaypointActiveGet(&waypointActive); WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint); bad_reads += (WaypointInstGet(waypointActive.Index, &waypoint) != 0);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) + float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) + powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
@ -113,32 +144,34 @@ static void pathPlannerTask(void *parameters)
// We hit this waypoint // We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS)) { if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
if (waypoint.Action == WAYPOINT_ACTION_NEXT) { switch(waypoint.Action) {
waypointActive.Index++; case WAYPOINT_ACTION_NEXT:
WaypointActiveSet(&waypointActive); waypointActive.Index++;
WaypointActiveSet(&waypointActive);
if(WaypointInstGet(waypointActive.Index, &waypoint) != 0) {
// Oh shit, tried to go to non-existant waypoint if(WaypointInstGet(waypointActive.Index, &waypoint) != 0) {
continue; // Oh shit, tried to go to non-existant waypoint
} continue;
}
PositionDesiredData positionDesired; break;
PositionDesiredGet(&positionDesired); case WAYPOINT_ACTION_RTH:
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH]; // Fly back to the home location but 20 m above it
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST]; PositionDesiredGet(&positionDesired);
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN]; positionDesired.North = 0;
PositionDesiredSet(&positionDesired); positionDesired.East = 0;
} else if (waypointAction == WAYPOINT_ACTION_RTH) { positionDesired.Down = -20;
// Fly back to the home location but 20 m above it PositionDesiredSet(&positionDesired);
PositionDesiredData positionDesired; break;
PositionDesiredGet(&positionDesired); default:
positionDesired.North = 0; PIOS_DEBUG_Assert(0);
positionDesired.East = 0;
positionDesired.Down = -20;
PositionDesiredSet(&positionDesired);
} }
} }
vTaskDelay(10);
PositionDesiredGet(&positionDesired);
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
PositionDesiredSet(&positionDesired);
} }
} }