From 46fc5fa4fb54a59d5cd5642e09c589a80042744f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 9 May 2012 19:36:44 -0500 Subject: [PATCH] Create a PathPlannerSettings object and move the PathMode setting to there. Also add a (temporary) field to allow storing some custom paths to select on the fly. Later will be replaced with some code to fetch from the flash chip. --- flight/Modules/PathPlanner/pathplanner.c | 92 +++++++++++++++---- flight/Revolution/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../uavobjectdefinition/guidancesettings.xml | 1 - .../pathplannersettings.xml | 11 +++ 5 files changed, 89 insertions(+), 18 deletions(-) create mode 100644 shared/uavobjectdefinition/pathplannersettings.xml diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c index b1ad1b434..57f617560 100644 --- a/flight/Modules/PathPlanner/pathplanner.c +++ b/flight/Modules/PathPlanner/pathplanner.c @@ -33,8 +33,8 @@ #include "paths.h" #include "flightstatus.h" -#include "guidancesettings.h" #include "pathdesired.h" +#include "pathplannersettings.h" #include "positionactual.h" #include "waypoint.h" #include "waypointactive.h" @@ -49,14 +49,17 @@ // Private variables static xTaskHandle taskHandle; static xQueueHandle queue; +static PathPlannerSettingsData pathPlannerSettings; static WaypointActiveData waypointActive; static WaypointData waypoint; -static GuidanceSettingsData guidanceSettings; // Private functions static void pathPlannerTask(void *parameters); +static void settingsUpdated(UAVObjEvent * ev); static void waypointsUpdated(UAVObjEvent * ev); -static void createPath(); +static void createPathBox(); +static void createPathLogo(); + /** * Module initialization */ @@ -77,7 +80,8 @@ int32_t PathPlannerStart() int32_t PathPlannerInitialize() { taskHandle = NULL; - + + PathPlannerSettingsInitialize(); WaypointInitialize(); WaypointActiveInitialize(); @@ -96,6 +100,9 @@ int32_t bad_inits; int32_t bad_reads; static void pathPlannerTask(void *parameters) { + PathPlannerSettingsConnectCallback(settingsUpdated); + settingsUpdated(PathPlannerSettingsHandle()); + WaypointConnectCallback(waypointsUpdated); WaypointActiveConnectCallback(waypointsUpdated); @@ -103,8 +110,6 @@ static void pathPlannerTask(void *parameters) PathDesiredData pathDesired; PositionActualData positionActual; - createPath(); - const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m) // Main thread loop @@ -129,11 +134,9 @@ static void pathPlannerTask(void *parameters) pathplanner_active = true; continue; } - - GuidanceSettingsGet(&guidanceSettings); - switch(guidanceSettings.PathMode) { - case GUIDANCESETTINGS_PATHMODE_ENDPOINT: + switch(pathPlannerSettings.PathMode) { + case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: PositionActualGet(&positionActual); float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) + @@ -164,7 +167,7 @@ static void pathPlannerTask(void *parameters) break; - case GUIDANCESETTINGS_PATHMODE_PATH: + case PATHPLANNERSETTINGS_PATHMODE_PATH: PathDesiredGet(&pathDesired); PositionActualGet(&positionActual); @@ -213,13 +216,11 @@ static void waypointsUpdated(UAVObjEvent * ev) WaypointActiveGet(&waypointActive); WaypointInstGet(waypointActive.Index, &waypoint); - - GuidanceSettingsGet(&guidanceSettings); PathDesiredData pathDesired; - switch(guidanceSettings.PathMode) { - case GUIDANCESETTINGS_PATHMODE_ENDPOINT: + switch(pathPlannerSettings.PathMode) { + case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: { PathDesiredGet(&pathDesired); pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; @@ -231,7 +232,7 @@ static void waypointsUpdated(UAVObjEvent * ev) } break; - case GUIDANCESETTINGS_PATHMODE_PATH: + case PATHPLANNERSETTINGS_PATHMODE_PATH: { PathDesiredData pathDesired; @@ -270,7 +271,64 @@ static void waypointsUpdated(UAVObjEvent * ev) } } -static void createPath() +void settingsUpdated(UAVObjEvent * ev) { + uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath; + PathPlannerSettingsGet(&pathPlannerSettings); + if (pathPlannerSettings.PreprogrammedPath != preprogrammedPath) { + switch(pathPlannerSettings.PreprogrammedPath) { + case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_NONE: + break; + case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_10M_BOX: + createPathBox(); + break; + case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_LOGO: + createPathLogo(); + break; + + } + } +} + +static void createPathBox() +{ + WaypointCreateInstance(); + WaypointCreateInstance(); + WaypointCreateInstance(); + WaypointCreateInstance(); + WaypointCreateInstance(); + + // 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.Position[0] = 5; + waypoint.Position[1] = 5; + waypoint.Position[2] = -10; + WaypointInstSet(0, &waypoint); + + waypoint.Position[0] = -5; + waypoint.Position[1] = 5; + WaypointInstSet(1, &waypoint); + + waypoint.Position[0] = -5; + waypoint.Position[1] = -5; + WaypointInstSet(2, &waypoint); + + waypoint.Position[0] = 5; + waypoint.Position[1] = -5; + WaypointInstSet(3, &waypoint); + + waypoint.Position[0] = 5; + waypoint.Position[1] = 5; + WaypointInstSet(4, &waypoint); + + waypoint.Position[0] = 0; + waypoint.Position[1] = 0; + WaypointInstSet(5, &waypoint); +} + +static void createPathLogo() { // Draw O WaypointData waypoint; diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 9dda7de58..b8dfcb546 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -59,6 +59,7 @@ UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += nedposition UAVOBJSRCFILENAMES += objectpersistence UAVOBJSRCFILENAMES += overosyncstats +UAVOBJSRCFILENAMES += pathplannersettings UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += positionactual UAVOBJSRCFILENAMES += ratedesired diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 1f439cfe0..05cb287cf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -55,6 +55,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ + $$UAVOBJECT_SYNTHETICS/pathplannersettings.h \ $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ $$UAVOBJECT_SYNTHETICS/positionactual.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ @@ -119,6 +120,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ + $$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \ $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ diff --git a/shared/uavobjectdefinition/guidancesettings.xml b/shared/uavobjectdefinition/guidancesettings.xml index 4e581fdc3..220bc9eba 100644 --- a/shared/uavobjectdefinition/guidancesettings.xml +++ b/shared/uavobjectdefinition/guidancesettings.xml @@ -2,7 +2,6 @@ Settings for the @ref GuidanceModule - diff --git a/shared/uavobjectdefinition/pathplannersettings.xml b/shared/uavobjectdefinition/pathplannersettings.xml new file mode 100644 index 000000000..c1f1aab20 --- /dev/null +++ b/shared/uavobjectdefinition/pathplannersettings.xml @@ -0,0 +1,11 @@ + + + Settings for the @ref PathPlanner Module + + + + + + + +