mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-22 14:19:42 +01:00
Pathplanner: removed pathPlannersettings and preprogrammed (hardcoded) paths
This commit is contained in:
Corvus Corax
parent
cc8e56f170
commit
e8232ba825
@ -37,7 +37,6 @@
|
||||
#include "pathaction.h"
|
||||
#include "pathdesired.h"
|
||||
#include "pathstatus.h"
|
||||
#include "pathplannersettings.h"
|
||||
#include "positionactual.h"
|
||||
#include "velocityactual.h"
|
||||
#include "waypoint.h"
|
||||
@ -55,12 +54,8 @@
|
||||
|
||||
// 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();
|
||||
@ -77,7 +72,6 @@ static uint8_t conditionImmediate();
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static PathPlannerSettingsData pathPlannerSettings;
|
||||
static WaypointActiveData waypointActive;
|
||||
static WaypointData waypoint;
|
||||
static PathActionData pathAction;
|
||||
@ -105,7 +99,6 @@ int32_t PathPlannerInitialize()
|
||||
{
|
||||
taskHandle = NULL;
|
||||
|
||||
PathPlannerSettingsInitialize();
|
||||
PathActionInitialize();
|
||||
PathStatusInitialize();
|
||||
PathDesiredInitialize();
|
||||
@ -125,10 +118,6 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
|
||||
*/
|
||||
static void pathPlannerTask(void *parameters)
|
||||
{
|
||||
// update settings on change
|
||||
PathPlannerSettingsConnectCallback(settingsUpdated);
|
||||
settingsUpdated(PathPlannerSettingsHandle());
|
||||
|
||||
// when the active waypoint changes, update pathDesired
|
||||
WaypointConnectCallback(updatePathDesired);
|
||||
WaypointActiveConnectCallback(updatePathDesired);
|
||||
@ -218,25 +207,6 @@ static void pathPlannerTask(void *parameters)
|
||||
}
|
||||
}
|
||||
|
||||
// standard settings updated callback
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// callback function when waypoints changed in any way, update pathDesired
|
||||
void updatePathDesired(UAVObjEvent * ev) {
|
||||
|
||||
@ -300,17 +270,6 @@ static void setWaypoint(uint16_t num) {
|
||||
|
||||
}
|
||||
|
||||
// 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
|
||||
@ -541,150 +500,6 @@ static uint8_t conditionImmediate() {
|
||||
return true;
|
||||
}
|
||||
|
||||
// demo path - box
|
||||
static void createPathBox()
|
||||
{
|
||||
|
||||
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.Action = 0;
|
||||
waypoint.Velocity = 2;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// 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 = 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] = 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;
|
||||
WaypointInstSet(i, &waypoint);
|
||||
}
|
||||
|
||||
// Draw P
|
||||
for(uint32_t i = 20; i < 35; i++) {
|
||||
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;
|
||||
WaypointInstSet(i, &waypoint);
|
||||
}
|
||||
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -50;
|
||||
waypoint.Position[2] = -50;
|
||||
WaypointInstSet(35, &waypoint);
|
||||
|
||||
// Draw Box
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(36, &waypoint);
|
||||
|
||||
waypoint.Position[1] = SIZE * 85;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(37, &waypoint);
|
||||
|
||||
waypoint.Position[1] = SIZE * 85;
|
||||
waypoint.Position[0] = SIZE * 60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(38, &waypoint);
|
||||
|
||||
waypoint.Position[1] = SIZE * -40;
|
||||
waypoint.Position[0] = SIZE * 60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(39, &waypoint);
|
||||
|
||||
waypoint.Position[1] = SIZE * -40;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(40, &waypoint);
|
||||
|
||||
waypoint.Position[1] = SIZE * 35;
|
||||
waypoint.Position[0] = SIZE * -60;
|
||||
waypoint.Position[2] = -30;
|
||||
WaypointInstSet(41, &waypoint);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -1,10 +0,0 @@
|
||||
<xml>
|
||||
<object name="PathPlannerSettings" singleinstance="true" settings="true">
|
||||
<description>Settings for the @ref PathPlanner Module</description>
|
||||
<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