mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-19 09:54:15 +01:00
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.
This commit is contained in:
James Cotton
parent
af63f94da0
commit
46fc5fa4fb
@ -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;
|
||||
|
||||
@ -59,6 +59,7 @@ UAVOBJSRCFILENAMES += nedaccel
|
||||
UAVOBJSRCFILENAMES += nedposition
|
||||
UAVOBJSRCFILENAMES += objectpersistence
|
||||
UAVOBJSRCFILENAMES += overosyncstats
|
||||
UAVOBJSRCFILENAMES += pathplannersettings
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += positionactual
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
|
||||
@ -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 \
|
||||
|
||||
@ -2,7 +2,6 @@
|
||||
<object name="GuidanceSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref GuidanceModule</description>
|
||||
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
|
||||
<field name="PathMode" units="" type="enum" elements="1" options="ENDPOINT,PATH" defaultvalue="ENDPOINT"/>
|
||||
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="10"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
|
||||
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="1,0,0"/>
|
||||
|
||||
11
shared/uavobjectdefinition/pathplannersettings.xml
Normal file
11
shared/uavobjectdefinition/pathplannersettings.xml
Normal file
@ -0,0 +1,11 @@
|
||||
<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"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
Loading…
x
Reference in New Issue
Block a user