mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
Merge branch 'corvuscorax/OP699' into revo-fixes
This commit is contained in:
commit
34210ef9ae
@ -556,6 +556,95 @@ static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const Mix
|
|||||||
ActuatorCommandChannelSet(Channel);
|
ActuatorCommandChannelSet(Channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* determine buzzer or blink sequence
|
||||||
|
**/
|
||||||
|
|
||||||
|
typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype;
|
||||||
|
|
||||||
|
static inline bool buzzerState(buzzertype type)
|
||||||
|
{
|
||||||
|
// This is for buzzers that take a PWM input
|
||||||
|
|
||||||
|
static uint32_t tune[BUZZ_MAX]={0};
|
||||||
|
static uint32_t tunestate[BUZZ_MAX]={0};
|
||||||
|
|
||||||
|
|
||||||
|
uint32_t newTune = 0;
|
||||||
|
if(type==BUZZ_BUZZER)
|
||||||
|
{
|
||||||
|
// Decide what tune to play
|
||||||
|
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
|
||||||
|
newTune = 0b11110110110000; // pause, short, short, short, long
|
||||||
|
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
|
||||||
|
newTune = 0x80000000; // pause, short
|
||||||
|
} else {
|
||||||
|
newTune = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else { // BUZZ_ARMING || BUZZ_INFO
|
||||||
|
uint8_t arming;
|
||||||
|
FlightStatusArmedGet(&arming);
|
||||||
|
//base idle tune
|
||||||
|
newTune = 0x80000000; // 0b1000...
|
||||||
|
|
||||||
|
// Merge the error pattern for InfoLed
|
||||||
|
if(type==BUZZ_INFO)
|
||||||
|
{
|
||||||
|
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
|
||||||
|
{
|
||||||
|
newTune |= 0b00000000001111111011111110000000;
|
||||||
|
}
|
||||||
|
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
|
||||||
|
{
|
||||||
|
newTune |= 0b00000000000000110110110000000000;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// fast double blink pattern if armed
|
||||||
|
if (arming == FLIGHTSTATUS_ARMED_ARMED)
|
||||||
|
newTune |= 0xA0000000; // 0b101000...
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Do we need to change tune?
|
||||||
|
if (newTune != tune[type]) {
|
||||||
|
tune[type] = newTune;
|
||||||
|
// resynchronize all tunes on change, so they stay in sync
|
||||||
|
for (int i=0;i<BUZZ_MAX;i++) {
|
||||||
|
tunestate[i] = tune[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Play tune
|
||||||
|
bool buzzOn = false;
|
||||||
|
static portTickType lastSysTime = 0;
|
||||||
|
portTickType thisSysTime = xTaskGetTickCount();
|
||||||
|
portTickType dT = 0;
|
||||||
|
|
||||||
|
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
|
||||||
|
if (tune[type]) {
|
||||||
|
if(thisSysTime > lastSysTime) {
|
||||||
|
dT = thisSysTime - lastSysTime;
|
||||||
|
} else {
|
||||||
|
lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
|
||||||
|
}
|
||||||
|
|
||||||
|
buzzOn = (tunestate[type]&1);
|
||||||
|
|
||||||
|
if (dT > 80) {
|
||||||
|
// Go to next bit in alarm_seq_state
|
||||||
|
for (int i=0;i<BUZZ_MAX;i++) {
|
||||||
|
tunestate[i] >>=1;
|
||||||
|
if (tunestate[i]==0) // All done, re-start the tune
|
||||||
|
tunestate[i]=tune[i];
|
||||||
|
}
|
||||||
|
lastSysTime = thisSysTime;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return buzzOn;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
|
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
|
||||||
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
|
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
|
||||||
{
|
{
|
||||||
@ -566,118 +655,17 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
|
|||||||
{
|
{
|
||||||
switch(actuatorSettings->ChannelType[mixer_channel]) {
|
switch(actuatorSettings->ChannelType[mixer_channel]) {
|
||||||
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
|
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
|
||||||
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
|
|
||||||
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
|
|
||||||
{
|
|
||||||
// This is for buzzers that take a PWM input
|
|
||||||
|
|
||||||
static uint32_t currBuzzTune = 0;
|
|
||||||
static uint32_t currBuzzTuneState;
|
|
||||||
|
|
||||||
static uint32_t currArmingTune = 0;
|
|
||||||
static uint32_t currArmingTuneState;
|
|
||||||
|
|
||||||
static uint32_t currInfoTune = 0;
|
|
||||||
static uint32_t currInfoTuneState;
|
|
||||||
|
|
||||||
uint32_t newTune = 0;
|
|
||||||
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
|
|
||||||
{
|
|
||||||
|
|
||||||
// Decide what tune to play
|
|
||||||
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
|
|
||||||
newTune = 0b11110110110000; // pause, short, short, short, long
|
|
||||||
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
|
|
||||||
newTune = 0x80000000; // pause, short
|
|
||||||
} else {
|
|
||||||
newTune = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Do we need to change tune?
|
|
||||||
if (newTune != currBuzzTune) {
|
|
||||||
currBuzzTune = newTune;
|
|
||||||
currBuzzTuneState = currBuzzTune;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else // ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED || ACTUATORSETTINGS_CHANNELTYPE_INFOLED
|
|
||||||
{
|
|
||||||
uint8_t arming;
|
|
||||||
FlightStatusArmedGet(&arming);
|
|
||||||
//base idle tune
|
|
||||||
newTune = 0x80000000; // 0b1000...
|
|
||||||
|
|
||||||
// Merge the error pattern for InfoLed
|
|
||||||
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
|
|
||||||
{
|
|
||||||
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
|
|
||||||
{
|
|
||||||
newTune |= 0b00000000001111111011111110000000;
|
|
||||||
}
|
|
||||||
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
|
|
||||||
{
|
|
||||||
newTune |= 0b00000000000000110110110000000000;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// fast double blink pattern if armed
|
|
||||||
if (arming == FLIGHTSTATUS_ARMED_ARMED)
|
|
||||||
newTune |= 0xA0000000; // 0b101000...
|
|
||||||
|
|
||||||
// Do we need to change tune?
|
|
||||||
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
|
|
||||||
{
|
|
||||||
if (newTune != currArmingTune) {
|
|
||||||
currArmingTune = newTune;
|
|
||||||
// note: those are both updated so that Info and Arming are in sync if used simultaneously
|
|
||||||
currArmingTuneState = currArmingTune;
|
|
||||||
currInfoTuneState = currInfoTune;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if (newTune != currInfoTune) {
|
|
||||||
currInfoTune = newTune;
|
|
||||||
currArmingTuneState = currArmingTune;
|
|
||||||
currInfoTuneState = currInfoTune;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Play tune
|
|
||||||
bool buzzOn = false;
|
|
||||||
static portTickType lastSysTime = 0;
|
|
||||||
portTickType thisSysTime = xTaskGetTickCount();
|
|
||||||
portTickType dT = 0;
|
|
||||||
|
|
||||||
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
|
|
||||||
if (currBuzzTune||currArmingTune||currInfoTune) {
|
|
||||||
if(thisSysTime > lastSysTime)
|
|
||||||
dT = thisSysTime - lastSysTime;
|
|
||||||
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
|
|
||||||
buzzOn = (currBuzzTuneState&1); // Buzz when the LS bit is 1
|
|
||||||
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
|
|
||||||
buzzOn = (currArmingTuneState&1);
|
|
||||||
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
|
|
||||||
buzzOn = (currInfoTuneState&1);
|
|
||||||
|
|
||||||
if (dT > 80) {
|
|
||||||
// Go to next bit in alarm_seq_state
|
|
||||||
currArmingTuneState >>=1;
|
|
||||||
currInfoTuneState >>= 1;
|
|
||||||
currBuzzTuneState >>= 1;
|
|
||||||
|
|
||||||
if (currBuzzTuneState == 0)
|
|
||||||
currBuzzTuneState = currBuzzTune; // All done, re-start the tune
|
|
||||||
if (currArmingTuneState == 0)
|
|
||||||
currArmingTuneState = currArmingTune;
|
|
||||||
if (currInfoTuneState == 0)
|
|
||||||
currInfoTuneState = currInfoTune;
|
|
||||||
lastSysTime = thisSysTime;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
|
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
|
||||||
buzzOn?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
|
buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
|
||||||
|
return true;
|
||||||
|
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
|
||||||
|
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
|
||||||
|
buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
|
||||||
|
return true;
|
||||||
|
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
|
||||||
|
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
|
||||||
|
buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
|
||||||
return true;
|
return true;
|
||||||
}
|
|
||||||
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
|
case ACTUATORSETTINGS_CHANNELTYPE_PWM:
|
||||||
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
|
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
|
||||||
return true;
|
return true;
|
||||||
@ -686,7 +674,6 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
|
|||||||
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value);
|
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value);
|
||||||
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
|
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
|
||||||
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value);
|
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value);
|
||||||
break;
|
|
||||||
#endif
|
#endif
|
||||||
default:
|
default:
|
||||||
return false;
|
return false;
|
||||||
|
@ -384,7 +384,6 @@ static uint8_t updateFixedDesiredAttitude()
|
|||||||
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
|
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
|
||||||
|
|
||||||
VelocityActualGet(&velocityActual);
|
VelocityActualGet(&velocityActual);
|
||||||
// VelocityDesiredGet(&velocityDesired);
|
|
||||||
StabilizationDesiredGet(&stabDesired);
|
StabilizationDesiredGet(&stabDesired);
|
||||||
VelocityDesiredGet(&velocityDesired);
|
VelocityDesiredGet(&velocityDesired);
|
||||||
AttitudeActualGet(&attitudeActual);
|
AttitudeActualGet(&attitudeActual);
|
||||||
|
@ -37,7 +37,6 @@
|
|||||||
#include "pathaction.h"
|
#include "pathaction.h"
|
||||||
#include "pathdesired.h"
|
#include "pathdesired.h"
|
||||||
#include "pathstatus.h"
|
#include "pathstatus.h"
|
||||||
#include "pathplannersettings.h"
|
|
||||||
#include "positionactual.h"
|
#include "positionactual.h"
|
||||||
#include "velocityactual.h"
|
#include "velocityactual.h"
|
||||||
#include "waypoint.h"
|
#include "waypoint.h"
|
||||||
@ -49,17 +48,14 @@
|
|||||||
#define MAX_QUEUE_SIZE 2
|
#define MAX_QUEUE_SIZE 2
|
||||||
#define F_PI 3.141526535897932f
|
#define F_PI 3.141526535897932f
|
||||||
#define RAD2DEG (180.0f/F_PI)
|
#define RAD2DEG (180.0f/F_PI)
|
||||||
|
#define PATH_PLANNER_UPDATE_RATE_MS 20
|
||||||
|
|
||||||
// Private types
|
// Private types
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void pathPlannerTask(void *parameters);
|
static void pathPlannerTask(void *parameters);
|
||||||
static void settingsUpdated(UAVObjEvent * ev);
|
|
||||||
static void updatePathDesired(UAVObjEvent * ev);
|
static void updatePathDesired(UAVObjEvent * ev);
|
||||||
static void setWaypoint(uint16_t num);
|
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 pathConditionCheck();
|
||||||
static uint8_t conditionNone();
|
static uint8_t conditionNone();
|
||||||
@ -76,8 +72,6 @@ static uint8_t conditionImmediate();
|
|||||||
|
|
||||||
// Private variables
|
// Private variables
|
||||||
static xTaskHandle taskHandle;
|
static xTaskHandle taskHandle;
|
||||||
static xQueueHandle queue;
|
|
||||||
static PathPlannerSettingsData pathPlannerSettings;
|
|
||||||
static WaypointActiveData waypointActive;
|
static WaypointActiveData waypointActive;
|
||||||
static WaypointData waypoint;
|
static WaypointData waypoint;
|
||||||
static PathActionData pathAction;
|
static PathActionData pathAction;
|
||||||
@ -105,7 +99,6 @@ int32_t PathPlannerInitialize()
|
|||||||
{
|
{
|
||||||
taskHandle = NULL;
|
taskHandle = NULL;
|
||||||
|
|
||||||
PathPlannerSettingsInitialize();
|
|
||||||
PathActionInitialize();
|
PathActionInitialize();
|
||||||
PathStatusInitialize();
|
PathStatusInitialize();
|
||||||
PathDesiredInitialize();
|
PathDesiredInitialize();
|
||||||
@ -115,9 +108,6 @@ int32_t PathPlannerInitialize()
|
|||||||
WaypointInitialize();
|
WaypointInitialize();
|
||||||
WaypointActiveInitialize();
|
WaypointActiveInitialize();
|
||||||
|
|
||||||
// Create object queue
|
|
||||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -128,10 +118,6 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
|
|||||||
*/
|
*/
|
||||||
static void pathPlannerTask(void *parameters)
|
static void pathPlannerTask(void *parameters)
|
||||||
{
|
{
|
||||||
// update settings on change
|
|
||||||
PathPlannerSettingsConnectCallback(settingsUpdated);
|
|
||||||
settingsUpdated(PathPlannerSettingsHandle());
|
|
||||||
|
|
||||||
// when the active waypoint changes, update pathDesired
|
// when the active waypoint changes, update pathDesired
|
||||||
WaypointConnectCallback(updatePathDesired);
|
WaypointConnectCallback(updatePathDesired);
|
||||||
WaypointActiveConnectCallback(updatePathDesired);
|
WaypointActiveConnectCallback(updatePathDesired);
|
||||||
@ -146,7 +132,7 @@ static void pathPlannerTask(void *parameters)
|
|||||||
while (1)
|
while (1)
|
||||||
{
|
{
|
||||||
|
|
||||||
vTaskDelay(20);
|
vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
|
||||||
|
|
||||||
FlightStatusGet(&flightStatus);
|
FlightStatusGet(&flightStatus);
|
||||||
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
|
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
|
||||||
@ -221,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
|
// callback function when waypoints changed in any way, update pathDesired
|
||||||
void updatePathDesired(UAVObjEvent * ev) {
|
void updatePathDesired(UAVObjEvent * ev) {
|
||||||
|
|
||||||
@ -303,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
|
// execute the apropriate condition and report result
|
||||||
static uint8_t pathConditionCheck() {
|
static uint8_t pathConditionCheck() {
|
||||||
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
|
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
|
||||||
@ -544,150 +500,6 @@ static uint8_t conditionImmediate() {
|
|||||||
return true;
|
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);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @}
|
* @}
|
||||||
* @}
|
* @}
|
||||||
|
@ -69,7 +69,6 @@ UAVOBJSRCFILENAMES += overosyncstats
|
|||||||
UAVOBJSRCFILENAMES += overosyncsettings
|
UAVOBJSRCFILENAMES += overosyncsettings
|
||||||
UAVOBJSRCFILENAMES += pathaction
|
UAVOBJSRCFILENAMES += pathaction
|
||||||
UAVOBJSRCFILENAMES += pathdesired
|
UAVOBJSRCFILENAMES += pathdesired
|
||||||
UAVOBJSRCFILENAMES += pathplannersettings
|
|
||||||
UAVOBJSRCFILENAMES += pathstatus
|
UAVOBJSRCFILENAMES += pathstatus
|
||||||
UAVOBJSRCFILENAMES += positionactual
|
UAVOBJSRCFILENAMES += positionactual
|
||||||
UAVOBJSRCFILENAMES += ratedesired
|
UAVOBJSRCFILENAMES += ratedesired
|
||||||
|
@ -69,7 +69,6 @@ UAVOBJSRCFILENAMES += overosyncstats
|
|||||||
UAVOBJSRCFILENAMES += overosyncsettings
|
UAVOBJSRCFILENAMES += overosyncsettings
|
||||||
UAVOBJSRCFILENAMES += pathaction
|
UAVOBJSRCFILENAMES += pathaction
|
||||||
UAVOBJSRCFILENAMES += pathdesired
|
UAVOBJSRCFILENAMES += pathdesired
|
||||||
UAVOBJSRCFILENAMES += pathplannersettings
|
|
||||||
UAVOBJSRCFILENAMES += pathstatus
|
UAVOBJSRCFILENAMES += pathstatus
|
||||||
UAVOBJSRCFILENAMES += positionactual
|
UAVOBJSRCFILENAMES += positionactual
|
||||||
UAVOBJSRCFILENAMES += ratedesired
|
UAVOBJSRCFILENAMES += ratedesired
|
||||||
|
@ -66,7 +66,6 @@ UAVOBJSRCFILENAMES += objectpersistence
|
|||||||
UAVOBJSRCFILENAMES += overosyncstats
|
UAVOBJSRCFILENAMES += overosyncstats
|
||||||
UAVOBJSRCFILENAMES += pathaction
|
UAVOBJSRCFILENAMES += pathaction
|
||||||
UAVOBJSRCFILENAMES += pathdesired
|
UAVOBJSRCFILENAMES += pathdesired
|
||||||
UAVOBJSRCFILENAMES += pathplannersettings
|
|
||||||
UAVOBJSRCFILENAMES += pathstatus
|
UAVOBJSRCFILENAMES += pathstatus
|
||||||
UAVOBJSRCFILENAMES += positionactual
|
UAVOBJSRCFILENAMES += positionactual
|
||||||
UAVOBJSRCFILENAMES += ratedesired
|
UAVOBJSRCFILENAMES += ratedesired
|
||||||
|
@ -61,7 +61,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
|
|||||||
$$UAVOBJECT_SYNTHETICS/gpssatellites.h \
|
$$UAVOBJECT_SYNTHETICS/gpssatellites.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathaction.h \
|
$$UAVOBJECT_SYNTHETICS/pathaction.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
|
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathplannersettings.h \
|
|
||||||
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
|
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
|
$$UAVOBJECT_SYNTHETICS/gpsvelocity.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/positionactual.h \
|
$$UAVOBJECT_SYNTHETICS/positionactual.h \
|
||||||
@ -141,7 +140,6 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
|
|||||||
$$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
|
$$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
|
$$UAVOBJECT_SYNTHETICS/pathaction.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
|
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \
|
|
||||||
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
|
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
|
$$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/positionactual.cpp \
|
$$UAVOBJECT_SYNTHETICS/positionactual.cpp \
|
||||||
|
@ -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…
Reference in New Issue
Block a user