mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
gpsassist: One commit new branch
Collapsed into one branch for a new code review. Some unnecssary committed files removed.
This commit is contained in:
abeck70
parent
a7677843e6
commit
b1c94e83d0
@ -30,6 +30,7 @@
|
||||
|
||||
#ifndef PLANS_H_
|
||||
#define PLANS_H_
|
||||
#include <stdint.h>
|
||||
#include <pios_math.h>
|
||||
|
||||
/** \page standard Plans
|
||||
@ -67,6 +68,16 @@ void plan_setup_land();
|
||||
*/
|
||||
void plan_run_land();
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for braking
|
||||
*/
|
||||
void plan_setup_assistedcontrol(uint8_t timeout_occurred);
|
||||
|
||||
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0
|
||||
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1
|
||||
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN 2
|
||||
#define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT 3
|
||||
|
||||
/**
|
||||
* @brief setup pathfollower for positionvario
|
||||
*/
|
||||
|
||||
@ -53,6 +53,6 @@ typedef enum {
|
||||
|
||||
extern int32_t configuration_check();
|
||||
|
||||
FrameType_t GetCurrentFrameType();
|
||||
extern FrameType_t GetCurrentFrameType();
|
||||
|
||||
#endif /* SANITYCHECK_H */
|
||||
|
||||
@ -47,6 +47,7 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta
|
||||
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
|
||||
{
|
||||
switch (path->Mode) {
|
||||
case PATHDESIRED_MODE_BRAKE: // should never get here...
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
return path_vector(path, cur_point, status, true);
|
||||
|
||||
|
||||
@ -35,8 +35,11 @@
|
||||
#include <pathdesired.h>
|
||||
#include <positionstate.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <velocitystate.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <attitudestate.h>
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#include <sin_lookup.h>
|
||||
|
||||
#define UPDATE_EXPECTED 0.02f
|
||||
@ -53,8 +56,11 @@ void plan_initialize()
|
||||
PositionStateInitialize();
|
||||
PathDesiredInitialize();
|
||||
FlightModeSettingsInitialize();
|
||||
FlightStatusInitialize();
|
||||
AttitudeStateInitialize();
|
||||
ManualControlCommandInitialize();
|
||||
VelocityStateInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -84,6 +90,7 @@ void plan_setup_positionHold()
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for return to base
|
||||
*/
|
||||
@ -504,3 +511,89 @@ void plan_run_AutoCruise()
|
||||
pathDesired.Start.Down = pathDesired.End.Down;
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief setup pathplanner/pathfollower for braking in positionroam
|
||||
* timeout_occurred = false: Attempt to enter flyvector for braking
|
||||
* timeout_occurred = true: Revert to position hold
|
||||
*/
|
||||
#define ASSISTEDCONTROL_BRAKERATE_MINIMUM 0.2f // m/s2
|
||||
#define ASSISTEDCONTROL_TIMETOSTOP_MINIMUM 0.2f // seconds
|
||||
#define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds
|
||||
#define ASSISTEDCONTROL_DELAY_TO_BRAKE 1.0f // seconds
|
||||
#define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 2.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this
|
||||
void plan_setup_assistedcontrol(uint8_t timeout_occurred)
|
||||
{
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
|
||||
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
|
||||
|
||||
if (timeout_occurred) {
|
||||
pathDesired.End.North = positionState.North;
|
||||
pathDesired.End.East = positionState.East;
|
||||
pathDesired.End.Down = positionState.Down;
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
|
||||
} else {
|
||||
VelocityStateData velocityState;
|
||||
VelocityStateGet(&velocityState);
|
||||
float brakeRate;
|
||||
VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
|
||||
if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
|
||||
brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
|
||||
}
|
||||
// Calculate the velocity
|
||||
float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
|
||||
velocity = sqrtf(velocity);
|
||||
|
||||
// Calculate the desired time to zero velocity.
|
||||
float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
|
||||
time_to_stopped += velocity / brakeRate;
|
||||
|
||||
// Sanity check the brake rate by ensuring that the time to stop is within a range.
|
||||
if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
|
||||
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
|
||||
} else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
|
||||
time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
|
||||
}
|
||||
|
||||
// calculate the distance we will travel
|
||||
float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
|
||||
north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
|
||||
float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
|
||||
east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
|
||||
float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
|
||||
down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
|
||||
float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
|
||||
net_delta = sqrtf(net_delta);
|
||||
|
||||
pathDesired.Start.North = positionState.North;
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down;
|
||||
|
||||
pathDesired.End.North = positionState.North + north_delta;
|
||||
pathDesired.End.East = positionState.East + east_delta;
|
||||
pathDesired.End.Down = positionState.Down + down_delta;
|
||||
|
||||
pathDesired.StartingVelocity = velocity;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
|
||||
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
|
||||
assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
|
||||
}
|
||||
FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <systemsettings.h>
|
||||
#include <stabilizationsettings.h>
|
||||
#include <systemalarms.h>
|
||||
#include <revosettings.h>
|
||||
#include <positionstate.h>
|
||||
@ -46,7 +47,7 @@
|
||||
|
||||
|
||||
// ! Check a stabilization mode switch position for safety
|
||||
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
|
||||
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
|
||||
|
||||
/**
|
||||
* Run a preflight check over the hardware configuration
|
||||
@ -95,31 +96,41 @@ int32_t configuration_check()
|
||||
// modes
|
||||
uint8_t num_modes;
|
||||
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
||||
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||
ManualControlSettingsFlightModeNumberGet(&num_modes);
|
||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||
FlightModeSettingsFlightModePositionGet(modes);
|
||||
|
||||
for (uint32_t i = 0; i < num_modes; i++) {
|
||||
uint8_t gps_assisted = FlightModeAssistMap[i];
|
||||
if (gps_assisted) {
|
||||
ADDSEVERITY(!coptercontrol);
|
||||
ADDSEVERITY(multirotor);
|
||||
ADDSEVERITY(navCapableFusion);
|
||||
}
|
||||
|
||||
switch (modes[i]) {
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
|
||||
ADDSEVERITY(!gps_assisted);
|
||||
ADDSEVERITY(!multirotor);
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
|
||||
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
|
||||
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
|
||||
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
|
||||
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
|
||||
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
|
||||
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
|
||||
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted));
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
|
||||
{
|
||||
@ -129,9 +140,13 @@ int32_t configuration_check()
|
||||
SystemAlarmsAlarmData alarms;
|
||||
SystemAlarmsAlarmGet(&alarms);
|
||||
ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
|
||||
ADDSEVERITY(!gps_assisted);
|
||||
}
|
||||
// intentionally no break as this also needs pathfollower
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
|
||||
ADDSEVERITY(!coptercontrol);
|
||||
ADDSEVERITY(navCapableFusion);
|
||||
break;
|
||||
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
|
||||
@ -140,6 +155,7 @@ int32_t configuration_check()
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
|
||||
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
|
||||
ADDSEVERITY(!gps_assisted);
|
||||
ADDSEVERITY(!coptercontrol);
|
||||
ADDSEVERITY(navCapableFusion);
|
||||
break;
|
||||
@ -175,7 +191,7 @@ int32_t configuration_check()
|
||||
* @param[in] index Which stabilization mode to check
|
||||
* @returns true or false
|
||||
*/
|
||||
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
|
||||
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
|
||||
{
|
||||
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
|
||||
|
||||
@ -213,6 +229,17 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
|
||||
}
|
||||
}
|
||||
|
||||
if (gpsassisted) {
|
||||
// For multirotors verify that roll/pitch are either attitude or rattitude
|
||||
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
|
||||
if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
|
||||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// coptercontrol cannot do altitude holding
|
||||
if (coptercontrol) {
|
||||
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|
||||
|
||||
@ -278,9 +278,9 @@ static bool okToArm(void)
|
||||
|
||||
StabilizationDesiredStabilizationModeData stabDesired;
|
||||
|
||||
uint8_t flightMode;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
switch (flightMode) {
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
@ -293,9 +293,16 @@ static bool okToArm(void)
|
||||
if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD ||
|
||||
stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) {
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
// avoid assisted control with auto throttle. As it sits waiting to launch,
|
||||
// it will move to hold, and auto thrust will auto launch otherwise!
|
||||
if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
@ -37,12 +37,16 @@
|
||||
#include <sanitycheck.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <adjustments.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <systemsettings.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <callbackinfo.h>
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
#include <stabilizationsettings.h>
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#endif
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_MANUAL_STACK_SIZE)
|
||||
@ -93,6 +97,9 @@ static const controlHandler handler_PATHPLANNER = {
|
||||
},
|
||||
.handler = &pathPlannerHandler,
|
||||
};
|
||||
static float thrustAtBrakeStart = 0.0f;
|
||||
static float thrustLo = 0.0f;
|
||||
static float thrustHi = 0.0f;
|
||||
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
// Private variables
|
||||
@ -101,8 +108,10 @@ static DelayedCallbackInfo *callbackHandle;
|
||||
// Private functions
|
||||
static void configurationUpdatedCb(UAVObjEvent *ev);
|
||||
static void commandUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
static void manualControlTask(void);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
|
||||
#endif
|
||||
|
||||
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
|
||||
|
||||
@ -147,7 +156,11 @@ int32_t ManualControlInitialize()
|
||||
ManualControlSettingsInitialize();
|
||||
FlightModeSettingsInitialize();
|
||||
SystemSettingsInitialize();
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
AdjustmentsInitialize();
|
||||
StabilizationSettingsInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
#endif
|
||||
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
|
||||
|
||||
return 0;
|
||||
@ -170,16 +183,36 @@ static void manualControlTask(void)
|
||||
FlightStatusGet(&flightStatus);
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
VtolPathFollowerSettingsData vtolPathFollowerSettings;
|
||||
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
|
||||
#endif
|
||||
|
||||
FlightModeSettingsData modeSettings;
|
||||
FlightModeSettingsGet(&modeSettings);
|
||||
|
||||
uint8_t position = cmd.FlightModeSwitchPosition;
|
||||
uint8_t newMode = flightStatus.FlightMode;
|
||||
uint8_t newFlightModeAssist = flightStatus.FlightModeAssist;
|
||||
uint8_t newAssistedControlState = flightStatus.AssistedControlState;
|
||||
uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState;
|
||||
if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
|
||||
newMode = modeSettings.FlightModePosition[position];
|
||||
}
|
||||
|
||||
// if a mode change occurs we default the assist mode and states here
|
||||
// to avoid having to add it to all of the below modes that are
|
||||
// otherwise unrelated
|
||||
if (newMode != flightStatus.FlightMode) {
|
||||
// set assist mode to none to avoid an assisted flight mode position
|
||||
// carrying over and impacting a newly selected non-assisted flight mode pos
|
||||
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
// The following are eqivalent to none effectively. Code should always
|
||||
// check the flightmodeassist state.
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
}
|
||||
|
||||
// Depending on the mode update the Stabilization or Actuator objects
|
||||
const controlHandler *handler = &handler_MANUAL;
|
||||
switch (newMode) {
|
||||
@ -193,9 +226,171 @@ static void manualControlTask(void)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
handler = &handler_STABILIZED;
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
|
||||
if (newFlightModeAssist) {
|
||||
// assess roll/pitch state
|
||||
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
|
||||
|
||||
// assess throttle state
|
||||
bool throttleNeutral = false;
|
||||
bool throttleNeutralOrLow = false;
|
||||
float neutralThrustOffset = 0.0f;
|
||||
AdjustmentsNeutralThrustOffsetGet(&neutralThrustOffset);
|
||||
float throttleRangeDelta = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) * 0.2f;
|
||||
float throttleNeutralLow = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
|
||||
float throttleNeutralHi = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
|
||||
if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
|
||||
throttleNeutral = true;
|
||||
}
|
||||
if (cmd.Thrust < throttleNeutralHi) {
|
||||
throttleNeutralOrLow = true;
|
||||
}
|
||||
|
||||
// determine default thrust mode for hold/brake states
|
||||
uint8_t pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
|
||||
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
|
||||
}
|
||||
|
||||
switch (newAssistedControlState) {
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
|
||||
if (!flagRollPitchHasInput) {
|
||||
// no stick input on roll/pitch so enter brake state
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
|
||||
newAssistedThrottleState = pathfollowerthrustmode;
|
||||
handler = &handler_PATHFOLLOWER;
|
||||
// retain thrust cmd for later comparison with actual in braking
|
||||
thrustAtBrakeStart = cmd.Thrust;
|
||||
|
||||
// calculate hi and low value of +-8% as a mini-deadband
|
||||
// for use in auto-override in brake sequence
|
||||
thrustLo = 0.92f * thrustAtBrakeStart;
|
||||
thrustHi = 1.08f * thrustAtBrakeStart;
|
||||
|
||||
// The purpose for auto throttle assist is to go from a mid to high thrust range to a
|
||||
// neutral vertical-holding/maintaining ~50% thrust range. It is not designed/intended
|
||||
// to go from near zero to 50%...we don't want an auto-takeoff feature here!
|
||||
// Also for rapid decents a user might have a bit of forward stick and low throttle
|
||||
// then stick-off for auto-braking...but if below the vtol min of 20% it will not
|
||||
// kick in...the flyer needs to manually manage throttle to slow down decent,
|
||||
// and the next time they put in a bit of stick, revert to primary, and then
|
||||
// sticks-off it will brake and hold with auto-thrust
|
||||
if (thrustAtBrakeStart < vtolPathFollowerSettings.ThrustLimits.Min) {
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
}
|
||||
} else {
|
||||
// stick input so stay in primary mode control state
|
||||
// newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
}
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
|
||||
if (flagRollPitchHasInput) {
|
||||
// stick input during brake sequence allows immediate resumption of control
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
} else {
|
||||
// no stick input, stay in brake mode
|
||||
newAssistedThrottleState = pathfollowerthrustmode;
|
||||
handler = &handler_PATHFOLLOWER;
|
||||
|
||||
// if auto thrust and user adjusts thrust outside of a deadband in which case revert to manual
|
||||
if ((newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO) &&
|
||||
(cmd.Thrust < thrustLo || cmd.Thrust > thrustHi)) {
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
|
||||
|
||||
if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST ||
|
||||
(newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
|
||||
newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
|
||||
// for manual or primary throttle states/modes, stick control immediately reverts to primary mode control
|
||||
if (flagRollPitchHasInput) {
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
} else {
|
||||
// otherwise stay in the hold state
|
||||
// newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
handler = &handler_PATHFOLLOWER;
|
||||
}
|
||||
} else if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
|
||||
newAssistedThrottleState != FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
|
||||
// ok auto thrust mode applies in hold unless overridden
|
||||
|
||||
if (throttleNeutralOrLow && flagRollPitchHasInput) {
|
||||
// throttle is neutral approximately and stick input present so revert to primary mode control
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
} else {
|
||||
// otherwise hold, autothrust, no stick input...we watch throttle for need to change mode
|
||||
switch (newAssistedThrottleState) {
|
||||
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO:
|
||||
// whilst in auto, watch for neutral range, from which we allow override
|
||||
if (throttleNeutral) {
|
||||
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE;
|
||||
} else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO; }
|
||||
break;
|
||||
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE:
|
||||
// previously user has set throttle to neutral range, apply a deadband and revert to manual
|
||||
// if moving out of deadband. This allows for landing in hold state.
|
||||
if (!throttleNeutral) {
|
||||
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
} else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE; }
|
||||
break;
|
||||
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL:
|
||||
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
break;
|
||||
}
|
||||
|
||||
newAssistedThrottleState = pathfollowerthrustmode;
|
||||
handler = &handler_PATHFOLLOWER;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
break;
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
|
||||
if (newFlightModeAssist) {
|
||||
switch (newFlightModeAssist) {
|
||||
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST:
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODEASSIST_NONE:
|
||||
default:
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
break;
|
||||
}
|
||||
|
||||
switch (newAssistedControlState) {
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
|
||||
break;
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
|
||||
break;
|
||||
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
|
||||
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
|
||||
break;
|
||||
}
|
||||
}
|
||||
handler = &handler_PATHFOLLOWER;
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
|
||||
@ -209,7 +404,7 @@ static void manualControlTask(void)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
|
||||
handler = &handler_PATHPLANNER;
|
||||
break;
|
||||
#endif
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
// There is no default, so if a flightmode is forgotten the compiler can throw a warning!
|
||||
}
|
||||
|
||||
@ -218,10 +413,16 @@ static void manualControlTask(void)
|
||||
// FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
|
||||
static bool firstRun = true;
|
||||
|
||||
if (flightStatus.FlightMode != newMode || firstRun) {
|
||||
if (flightStatus.FlightMode != newMode || firstRun ||
|
||||
newFlightModeAssist != flightStatus.FlightModeAssist ||
|
||||
newAssistedControlState != flightStatus.AssistedControlState ||
|
||||
flightStatus.AssistedThrottleState != newAssistedThrottleState) {
|
||||
firstRun = false;
|
||||
flightStatus.ControlChain = handler->controlChain;
|
||||
flightStatus.FlightMode = newMode;
|
||||
flightStatus.ControlChain = handler->controlChain;
|
||||
flightStatus.FlightMode = newMode;
|
||||
flightStatus.FlightModeAssist = newFlightModeAssist;
|
||||
flightStatus.AssistedControlState = newAssistedControlState;
|
||||
flightStatus.AssistedThrottleState = newAssistedThrottleState;
|
||||
FlightStatusSet(&flightStatus);
|
||||
newinit = true;
|
||||
}
|
||||
@ -246,6 +447,81 @@ static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Check and set modes for gps assisted stablised flight modes
|
||||
*/
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
|
||||
{
|
||||
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
|
||||
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
|
||||
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||
|
||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
flightModeAssistOption = FlightModeAssistMap[position];
|
||||
}
|
||||
|
||||
switch (flightModeAssistOption) {
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
|
||||
break;
|
||||
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
|
||||
{
|
||||
// default to cruise control.
|
||||
FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
|
||||
|
||||
switch (flightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
thrustMode = modeSettings->Stabilization1Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
thrustMode = modeSettings->Stabilization2Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
||||
thrustMode = modeSettings->Stabilization3Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
thrustMode = modeSettings->Stabilization4Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
thrustMode = modeSettings->Stabilization5Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
thrustMode = modeSettings->Stabilization6Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
|
||||
// is a more appropriate throttle mode. "GPSAssist" adds braking
|
||||
// and a better throttle management to the standard Position Hold.
|
||||
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
|
||||
break;
|
||||
|
||||
// other modes will use cruisecontrol as default
|
||||
}
|
||||
|
||||
|
||||
switch (thrustMode) {
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
|
||||
// this is only for use with stabi mods with althold/vario.
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
|
||||
break;
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
|
||||
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
|
||||
default:
|
||||
// this is the default for non stabi modes also
|
||||
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return isAssistedFlag;
|
||||
}
|
||||
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -56,7 +56,9 @@ void pathFollowerHandler(bool newinit)
|
||||
}
|
||||
|
||||
uint8_t flightMode;
|
||||
uint8_t assistedControlFlightMode;
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
|
||||
|
||||
if (newinit) {
|
||||
// After not being in this mode for a while init at current height
|
||||
@ -64,9 +66,13 @@ void pathFollowerHandler(bool newinit)
|
||||
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
|
||||
plan_setup_returnToBase();
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
plan_setup_positionHold();
|
||||
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
|
||||
// Just initiated braking after returning from stabi control
|
||||
plan_setup_assistedcontrol(false);
|
||||
} else {
|
||||
plan_setup_positionHold();
|
||||
}
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
|
||||
plan_setup_CourseLock();
|
||||
@ -88,6 +94,18 @@ void pathFollowerHandler(bool newinit)
|
||||
plan_setup_AutoCruise();
|
||||
break;
|
||||
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
|
||||
// Just initiated braking after returning from stabi control
|
||||
plan_setup_assistedcontrol(false);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
plan_setup_positionHold();
|
||||
break;
|
||||
|
||||
@ -93,6 +93,7 @@ void stabilizedHandler(bool newinit)
|
||||
uint8_t *stab_settings;
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
|
||||
@ -163,8 +164,8 @@ void stabilizedHandler(bool newinit)
|
||||
0; // this is an invalid mode
|
||||
}
|
||||
|
||||
stabilization.Thrust = cmd.Thrust;
|
||||
stabilization.StabilizationMode.Thrust = stab_settings[3];
|
||||
stabilization.Thrust = cmd.Thrust;
|
||||
StabilizationDesiredSet(&stabilization);
|
||||
}
|
||||
|
||||
|
||||
@ -54,6 +54,7 @@
|
||||
#include <sin_lookup.h>
|
||||
#include <pathdesired.h>
|
||||
#include <paths.h>
|
||||
#include "plans.h"
|
||||
#include <sanitycheck.h>
|
||||
|
||||
|
||||
@ -61,6 +62,7 @@
|
||||
#include <fixedwingpathfollowerstatus.h>
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <pathstatus.h>
|
||||
#include <positionstate.h>
|
||||
#include <velocitystate.h>
|
||||
@ -73,25 +75,42 @@
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <systemsettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <adjustments.h>
|
||||
#include <pathsummary.h>
|
||||
|
||||
|
||||
// Private constants
|
||||
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
|
||||
|
||||
#define PF_IDLE_UPDATE_RATE_MS 100
|
||||
#define PF_IDLE_UPDATE_RATE_MS 100
|
||||
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
|
||||
#define DEADBAND_HIGH 0.10f
|
||||
#define DEADBAND_LOW -0.10f
|
||||
|
||||
#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
|
||||
#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f
|
||||
|
||||
#define BRAKE_RATE_MINIMUM 0.2f
|
||||
|
||||
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
|
||||
#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f
|
||||
#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f
|
||||
#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f
|
||||
|
||||
#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
|
||||
#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample
|
||||
|
||||
#define DEADBAND_HIGH 0.10f
|
||||
#define DEADBAND_LOW -0.10f
|
||||
// Private types
|
||||
|
||||
struct Globals {
|
||||
struct pid PIDposH[2];
|
||||
struct pid PIDposV;
|
||||
struct pid PIDvel[3];
|
||||
struct pid PIDvel[3]; // North, East, Down
|
||||
struct pid BrakePIDvel[2]; // North, East
|
||||
struct pid PIDcourse;
|
||||
struct pid PIDspeed;
|
||||
struct pid PIDpower;
|
||||
@ -100,6 +119,19 @@ struct Globals {
|
||||
bool vtolEmergencyFallbackSwitch;
|
||||
};
|
||||
|
||||
struct NeutralThrustEstimation {
|
||||
uint32_t count;
|
||||
float sum;
|
||||
float average;
|
||||
float correction;
|
||||
float algo_erro_check;
|
||||
float min;
|
||||
float max;
|
||||
bool start_sampling;
|
||||
bool have_correction;
|
||||
};
|
||||
static struct NeutralThrustEstimation neutralThrustEst;
|
||||
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *pathFollowerCBInfo;
|
||||
@ -109,6 +141,8 @@ static PathStatusData pathStatus;
|
||||
static PathDesiredData pathDesired;
|
||||
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
|
||||
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
|
||||
static FlightStatusData flightStatus;
|
||||
static PathSummaryData pathSummary;
|
||||
|
||||
// correct speed by measured airspeed
|
||||
static float indicatedAirspeedStateBias = 0.0f;
|
||||
@ -126,6 +160,7 @@ static float updateCourseBearing();
|
||||
static float updatePathBearing();
|
||||
static float updatePOIBearing();
|
||||
static void processPOI();
|
||||
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
|
||||
static void updatePathVelocity(float kFF, bool limited);
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
|
||||
@ -159,7 +194,9 @@ int32_t PathFollowerInitialize()
|
||||
FixedWingPathFollowerStatusInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
FlightStatusInitialize();
|
||||
FlightModeSettingsInitialize();
|
||||
PathStatusInitialize();
|
||||
PathSummaryInitialize();
|
||||
PathDesiredInitialize();
|
||||
PositionStateInitialize();
|
||||
VelocityStateInitialize();
|
||||
@ -172,6 +209,8 @@ int32_t PathFollowerInitialize()
|
||||
ManualControlCommandInitialize();
|
||||
SystemSettingsInitialize();
|
||||
StabilizationBankInitialize();
|
||||
AdjustmentsInitialize();
|
||||
|
||||
|
||||
// reset integrals
|
||||
resetGlobals();
|
||||
@ -193,8 +232,6 @@ MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
|
||||
*/
|
||||
static void pathFollowerTask(void)
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||
@ -208,11 +245,21 @@ static void pathFollowerTask(void)
|
||||
processPOI();
|
||||
}
|
||||
|
||||
int16_t old_uid = pathStatus.UID;
|
||||
pathStatus.UID = pathDesired.UID;
|
||||
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
|
||||
if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
|
||||
if (old_uid != pathStatus.UID) {
|
||||
pathStatus.path_time = 0.0f;
|
||||
} else {
|
||||
pathStatus.path_time += updatePeriod / 1000.0f;
|
||||
}
|
||||
}
|
||||
|
||||
switch (pathDesired.Mode) {
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
case PATHDESIRED_MODE_BRAKE:
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
{
|
||||
@ -239,6 +286,7 @@ static void pathFollowerTask(void)
|
||||
}
|
||||
PathStatusSet(&pathStatus);
|
||||
|
||||
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
|
||||
@ -258,6 +306,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
|
||||
pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
|
||||
|
||||
pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
|
||||
pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit);
|
||||
|
||||
PathDesiredGet(&pathDesired);
|
||||
}
|
||||
|
||||
@ -297,12 +348,27 @@ static void resetGlobals()
|
||||
pid_zero(&global.PIDvel[0]);
|
||||
pid_zero(&global.PIDvel[1]);
|
||||
pid_zero(&global.PIDvel[2]);
|
||||
pid_zero(&global.BrakePIDvel[0]);
|
||||
pid_zero(&global.BrakePIDvel[1]);
|
||||
pid_zero(&global.PIDcourse);
|
||||
pid_zero(&global.PIDspeed);
|
||||
pid_zero(&global.PIDpower);
|
||||
global.poiRadius = 0.0f;
|
||||
global.vtolEmergencyFallback = 0;
|
||||
global.vtolEmergencyFallbackSwitch = false;
|
||||
|
||||
// reset neutral thrust assessment. We restart this process
|
||||
// and do once for each position hold engagement
|
||||
neutralThrustEst.start_sampling = false;
|
||||
neutralThrustEst.count = 0;
|
||||
neutralThrustEst.sum = 0.0f;
|
||||
neutralThrustEst.have_correction = false;
|
||||
neutralThrustEst.average = 0.0f;
|
||||
neutralThrustEst.correction = 0.0f;
|
||||
neutralThrustEst.min = 0.0f;
|
||||
neutralThrustEst.max = 0.0f;
|
||||
|
||||
pathStatus.path_time = 0.0f;
|
||||
}
|
||||
|
||||
static uint8_t updateAutoPilotByFrameType()
|
||||
@ -389,28 +455,37 @@ static uint8_t updateAutoPilotVtol()
|
||||
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
|
||||
bool yaw_attitude = true;
|
||||
float yaw = 0.0f;
|
||||
switch (vtolPathFollowerSettings.YawControl) {
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
|
||||
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
|
||||
yaw_attitude = false;
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
|
||||
yaw = updateTailInBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
|
||||
yaw = updateCourseBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
|
||||
yaw = updatePathBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
|
||||
yaw = updatePOIBearing();
|
||||
break;
|
||||
} else {
|
||||
switch (vtolPathFollowerSettings.YawControl) {
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
|
||||
yaw_attitude = false;
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
|
||||
yaw = updateTailInBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
|
||||
yaw = updateCourseBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
|
||||
yaw = updatePathBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
|
||||
yaw = updatePOIBearing();
|
||||
break;
|
||||
}
|
||||
}
|
||||
result = updateVtolDesiredAttitude(yaw_attitude, yaw);
|
||||
|
||||
// switch to emergency follower if follower indicates problems
|
||||
if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED)) {
|
||||
global.vtolEmergencyFallbackSwitch = true;
|
||||
if (!result) {
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
|
||||
plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override
|
||||
} else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
|
||||
// switch to emergency follower if follower indicates problems
|
||||
global.vtolEmergencyFallbackSwitch = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -427,6 +502,48 @@ static uint8_t updateAutoPilotVtol()
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
// Brake mode end condition checks
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
|
||||
bool exit_brake = false;
|
||||
VelocityStateData velocityState;
|
||||
if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout
|
||||
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
|
||||
exit_brake = true;
|
||||
} else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
|
||||
VelocityStateGet(&velocityState);
|
||||
if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
|
||||
pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
|
||||
exit_brake = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (exit_brake) {
|
||||
// Calculate the distance error between the originally desired
|
||||
// stopping point and the actual brake-exit point.
|
||||
|
||||
PositionStateData p;
|
||||
PositionStateGet(&p);
|
||||
float north_offset = pathDesired.End.North - p.North;
|
||||
float east_offset = pathDesired.End.East - p.East;
|
||||
float down_offset = pathDesired.End.Down - p.Down;
|
||||
pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
|
||||
pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time;
|
||||
pathSummary.fractional_progress = pathStatus.fractional_progress;
|
||||
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
|
||||
cur_velocity = sqrtf(cur_velocity);
|
||||
pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time;
|
||||
pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate;
|
||||
pathSummary.velocityIntoHold = cur_velocity;
|
||||
pathSummary.UID = pathStatus.UID;
|
||||
PathSummarySet(&pathSummary);
|
||||
|
||||
plan_setup_assistedcontrol(true); // braking timeout true
|
||||
// having re-entered hold allow reassessment of neutral thrust
|
||||
neutralThrustEst.have_correction = false;
|
||||
}
|
||||
}
|
||||
|
||||
switch (returnmode) {
|
||||
case RETURN_RESULT:
|
||||
return result;
|
||||
@ -607,6 +724,28 @@ static void processPOI()
|
||||
}
|
||||
}
|
||||
|
||||
static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
|
||||
{
|
||||
if (startingVelocity >= 0.0f) {
|
||||
*updatedVelocity = startingVelocity - dT * brakeRate;
|
||||
if (*updatedVelocity > currentVelocity) {
|
||||
*updatedVelocity = currentVelocity;
|
||||
}
|
||||
if (*updatedVelocity < 0.0f) {
|
||||
*updatedVelocity = 0.0f;
|
||||
}
|
||||
} else {
|
||||
*updatedVelocity = startingVelocity + dT * brakeRate;
|
||||
if (*updatedVelocity < currentVelocity) {
|
||||
*updatedVelocity = currentVelocity;
|
||||
}
|
||||
if (*updatedVelocity > 0.0f) {
|
||||
*updatedVelocity = 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired velocity from the current position and path
|
||||
*/
|
||||
@ -617,57 +756,91 @@ static void updatePathVelocity(float kFF, bool limited)
|
||||
PositionStateGet(&positionState);
|
||||
VelocityStateData velocityState;
|
||||
VelocityStateGet(&velocityState);
|
||||
VelocityDesiredData velocityDesired;
|
||||
|
||||
const float dT = updatePeriod / 1000.0f;
|
||||
|
||||
// look ahead kFF seconds
|
||||
float cur[3] = { positionState.North + (velocityState.North * kFF),
|
||||
positionState.East + (velocityState.East * kFF),
|
||||
positionState.Down + (velocityState.Down * kFF) };
|
||||
struct path_status progress;
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
|
||||
float brakeRate = vtolPathFollowerSettings.BrakeRate;
|
||||
if (brakeRate < BRAKE_RATE_MINIMUM) {
|
||||
brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
|
||||
}
|
||||
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North);
|
||||
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East);
|
||||
updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down);
|
||||
|
||||
path_progress(&pathDesired, cur, &progress);
|
||||
float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
|
||||
cur_velocity = sqrtf(cur_velocity);
|
||||
float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
|
||||
desired_velocity = sqrtf(desired_velocity);
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
VelocityDesiredData velocityDesired;
|
||||
velocityDesired.North = progress.path_vector[0];
|
||||
velocityDesired.East = progress.path_vector[1];
|
||||
velocityDesired.Down = progress.path_vector[2];
|
||||
// update pathstatus
|
||||
pathStatus.error = cur_velocity - desired_velocity;
|
||||
pathStatus.fractional_progress = 1.0f;
|
||||
if (pathDesired.StartingVelocity > 0.0f) {
|
||||
pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity;
|
||||
|
||||
if (limited &&
|
||||
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
|
||||
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
|
||||
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
|
||||
// leading to an S-shape snake course the wrong way
|
||||
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
|
||||
// turn steep unless there is enough space complete the turn before crossing the flightpath
|
||||
// in this case the plane effectively needs to be turned around
|
||||
// indicators:
|
||||
// difference between correction_direction and velocitystate >90 degrees and
|
||||
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
|
||||
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
|
||||
// calculating angles < 90 degrees through dot products
|
||||
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
|
||||
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
|
||||
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
|
||||
;
|
||||
// sometimes current velocity can exceed starting velocity due to initial acceleration
|
||||
if (pathStatus.fractional_progress < 0.0f) {
|
||||
pathStatus.fractional_progress = 0.0f;
|
||||
}
|
||||
}
|
||||
pathStatus.path_direction_north = velocityDesired.North;
|
||||
pathStatus.path_direction_east = velocityDesired.East;
|
||||
pathStatus.path_direction_down = velocityDesired.Down;
|
||||
|
||||
pathStatus.correction_direction_north = velocityDesired.North - velocityState.North;
|
||||
pathStatus.correction_direction_east = velocityDesired.East - velocityState.East;
|
||||
pathStatus.correction_direction_down = velocityDesired.Down - velocityState.Down;
|
||||
} else {
|
||||
// calculate correction
|
||||
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
|
||||
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
|
||||
// look ahead kFF seconds
|
||||
float cur[3] = { positionState.North + (velocityState.North * kFF),
|
||||
positionState.East + (velocityState.East * kFF),
|
||||
positionState.Down + (velocityState.Down * kFF) };
|
||||
struct path_status progress;
|
||||
path_progress(&pathDesired, cur, &progress);
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
velocityDesired.North = progress.path_vector[0];
|
||||
velocityDesired.East = progress.path_vector[1];
|
||||
velocityDesired.Down = progress.path_vector[2];
|
||||
|
||||
if (limited &&
|
||||
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
|
||||
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
|
||||
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
|
||||
// leading to an S-shape snake course the wrong way
|
||||
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
|
||||
// turn steep unless there is enough space complete the turn before crossing the flightpath
|
||||
// in this case the plane effectively needs to be turned around
|
||||
// indicators:
|
||||
// difference between correction_direction and velocitystate >90 degrees and
|
||||
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
|
||||
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
|
||||
// calculating angles < 90 degrees through dot products
|
||||
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
|
||||
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
|
||||
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
|
||||
;
|
||||
} else {
|
||||
// calculate correction
|
||||
velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
|
||||
velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
|
||||
}
|
||||
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.fractional_progress = progress.fractional_progress;
|
||||
pathStatus.path_direction_north = progress.path_vector[0];
|
||||
pathStatus.path_direction_east = progress.path_vector[1];
|
||||
pathStatus.path_direction_down = progress.path_vector[2];
|
||||
|
||||
pathStatus.correction_direction_north = progress.correction_vector[0];
|
||||
pathStatus.correction_direction_east = progress.correction_vector[1];
|
||||
pathStatus.correction_direction_down = progress.correction_vector[2];
|
||||
}
|
||||
velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.fractional_progress = progress.fractional_progress;
|
||||
pathStatus.path_direction_north = progress.path_vector[0];
|
||||
pathStatus.path_direction_east = progress.path_vector[1];
|
||||
pathStatus.path_direction_down = progress.path_vector[2];
|
||||
|
||||
pathStatus.correction_direction_north = progress.correction_vector[0];
|
||||
pathStatus.correction_direction_east = progress.correction_vector[1];
|
||||
pathStatus.correction_direction_down = progress.correction_vector[2];
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -1036,8 +1209,9 @@ static bool correctCourse(float *C, float *V, float *F, float s)
|
||||
*/
|
||||
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
{
|
||||
const float dT = updatePeriod / 1000.0f;
|
||||
uint8_t result = 1;
|
||||
const float dT = updatePeriod / 1000.0f;
|
||||
uint8_t result = 1;
|
||||
bool manual_thrust = false;
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityStateData velocityState;
|
||||
@ -1045,6 +1219,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
AttitudeStateData attitudeState;
|
||||
StabilizationBankData stabSettings;
|
||||
SystemSettingsData systemSettings;
|
||||
AdjustmentsData adjustments;
|
||||
|
||||
float northError;
|
||||
float northCommand;
|
||||
@ -1062,36 +1237,121 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeStateGet(&attitudeState);
|
||||
StabilizationBankGet(&stabSettings);
|
||||
AdjustmentsGet(&adjustments);
|
||||
|
||||
// Testing code - refactor into manual control command
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
// scale velocity if it is above configured maximum
|
||||
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
|
||||
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
|
||||
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
|
||||
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
|
||||
}
|
||||
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
|
||||
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
|
||||
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
|
||||
// scale velocity if it is above configured maximum
|
||||
// for braking, we can not help it if initial velocity was greater
|
||||
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
|
||||
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
|
||||
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
|
||||
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
|
||||
}
|
||||
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
|
||||
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute desired north command
|
||||
northError = velocityDesired.North - velocityState.North;
|
||||
northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
|
||||
// calculate the velocity errors between desired and actual
|
||||
northError = velocityDesired.North - velocityState.North;
|
||||
eastError = velocityDesired.East - velocityState.East;
|
||||
downError = velocityDesired.Down - velocityState.Down;
|
||||
|
||||
// Compute desired east command
|
||||
eastError = velocityDesired.East - velocityState.East;
|
||||
eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
|
||||
|
||||
// Compute desired down command
|
||||
downError = velocityDesired.Down - velocityState.Down;
|
||||
// Must flip this sign
|
||||
downError = -downError;
|
||||
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
|
||||
downError = -downError;
|
||||
|
||||
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
// Compute desired commands
|
||||
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
|
||||
northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
|
||||
eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
|
||||
} else {
|
||||
northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward;
|
||||
eastCommand = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward;
|
||||
}
|
||||
downCommand = pid_apply(&global.PIDvel[2], downError, dT);
|
||||
|
||||
|
||||
if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL &&
|
||||
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
|
||||
(flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
|
||||
manual_thrust = true;
|
||||
}
|
||||
|
||||
// if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
|
||||
// note that arming into this flight mode is not allowed, so assumption here is that
|
||||
// we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
|
||||
// altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
|
||||
// In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
|
||||
// avoiding auto-takeoffs. Therefore no need to check that here.
|
||||
if (!manual_thrust && neutralThrustEst.have_correction != true) {
|
||||
// Assess if position hold state running. This can be normal position hold or
|
||||
// another mode with assist-hold active.
|
||||
bool ph_active =
|
||||
((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD &&
|
||||
flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) ||
|
||||
(flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE &&
|
||||
flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD));
|
||||
|
||||
|
||||
bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
|
||||
fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
|
||||
fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
|
||||
fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
|
||||
|
||||
if (ph_active && stable) {
|
||||
if (neutralThrustEst.start_sampling) {
|
||||
neutralThrustEst.count++;
|
||||
|
||||
|
||||
// delay count for 2 seconds into hold allowing for stablisation
|
||||
if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
|
||||
neutralThrustEst.sum += global.PIDvel[2].iAccumulator;
|
||||
if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) {
|
||||
neutralThrustEst.min = global.PIDvel[2].iAccumulator;
|
||||
}
|
||||
if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) {
|
||||
neutralThrustEst.max = global.PIDvel[2].iAccumulator;
|
||||
}
|
||||
}
|
||||
|
||||
if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
|
||||
// 6 seconds have past
|
||||
// lets take an average
|
||||
neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
|
||||
neutralThrustEst.correction = neutralThrustEst.average / 1000.0f;
|
||||
|
||||
global.PIDvel[2].iAccumulator -= neutralThrustEst.average;
|
||||
|
||||
neutralThrustEst.start_sampling = false;
|
||||
neutralThrustEst.have_correction = true;
|
||||
|
||||
// Write a new adjustment value
|
||||
// adjustments.NeutralThrustOffset was incremental adjusted above
|
||||
AdjustmentsData new_adjustments;
|
||||
// add the average remaining i value to the
|
||||
new_adjustments.NeutralThrustOffset = adjustments.NeutralThrustOffset + neutralThrustEst.correction;
|
||||
new_adjustments.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
|
||||
new_adjustments.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
|
||||
new_adjustments.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
|
||||
AdjustmentsSet(&new_adjustments);
|
||||
}
|
||||
} else {
|
||||
// start a tick count
|
||||
neutralThrustEst.start_sampling = true;
|
||||
neutralThrustEst.count = 0;
|
||||
neutralThrustEst.sum = 0.0f;
|
||||
neutralThrustEst.max = 0.0f;
|
||||
neutralThrustEst.min = 0.0f;
|
||||
}
|
||||
} else {
|
||||
// reset sampling as we did't get 6 continuous seconds
|
||||
neutralThrustEst.start_sampling = false;
|
||||
}
|
||||
} // else we already have a correction for this PH run
|
||||
|
||||
// Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff.
|
||||
stabDesired.Thrust = boundf(adjustments.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
|
||||
|
||||
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
|
||||
@ -1102,6 +1362,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if ( // emergency flyaway detection
|
||||
( // integral already at its limit
|
||||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
|
||||
@ -1124,19 +1385,22 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
|
||||
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
|
||||
float maxPitch = vtolPathFollowerSettings.MaxRollPitch;
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) {
|
||||
maxPitch = vtolPathFollowerSettings.BrakeMaxPitch;
|
||||
}
|
||||
|
||||
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
|
||||
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
|
||||
-maxPitch, maxPitch);
|
||||
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
|
||||
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
|
||||
-maxPitch, maxPitch);
|
||||
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
|
||||
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
|
||||
// For now override thrust with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
}
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
@ -1145,9 +1409,51 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
stabDesired.Yaw = yaw_direction;
|
||||
} else {
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
||||
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
|
||||
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
|
||||
}
|
||||
|
||||
// default thrust mode to cruise control
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
|
||||
|
||||
// when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
|
||||
if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
|
||||
FlightModeSettingsData settings;
|
||||
FlightModeSettingsGet(&settings);
|
||||
FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
|
||||
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
thrustMode = settings.Stabilization1Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
|
||||
thrustMode = settings.Stabilization2Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
|
||||
thrustMode = settings.Stabilization3Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
|
||||
thrustMode = settings.Stabilization4Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
|
||||
thrustMode = settings.Stabilization5Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
|
||||
thrustMode = settings.Stabilization6Settings.Thrust;
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
// we hard code the "GPS Assisted" PostionHold to use alt-vario which
|
||||
// is a more appropriate throttle mode. "GPSAssist" adds braking
|
||||
// and a better throttle management to the standard Position Hold.
|
||||
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
|
||||
break;
|
||||
}
|
||||
stabDesired.StabilizationMode.Thrust = thrustMode;
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
} else if (manual_thrust) {
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
}
|
||||
// else thrust is set by the PID controller
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
return result;
|
||||
|
||||
@ -39,27 +39,33 @@
|
||||
#include <receiveractivity.h>
|
||||
#include <flightstatus.h>
|
||||
#include <flighttelemetrystats.h>
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
#include <stabilizationsettings.h>
|
||||
#endif
|
||||
#include <flightmodesettings.h>
|
||||
#include <systemsettings.h>
|
||||
#include <taskinfo.h>
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_RCTX)
|
||||
#include "pios_usb_rctx.h"
|
||||
#endif /* PIOS_INCLUDE_USB_RCTX */
|
||||
|
||||
// Private constants
|
||||
#if defined(PIOS_RECEIVER_STACK_SIZE)
|
||||
#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE
|
||||
#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE
|
||||
#else
|
||||
#define STACK_SIZE_BYTES 1152
|
||||
#define STACK_SIZE_BYTES 1152
|
||||
#endif
|
||||
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
|
||||
#define UPDATE_PERIOD_MS 20
|
||||
#define THROTTLE_FAILSAFE -0.1f
|
||||
#define ARMED_THRESHOLD 0.50f
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control
|
||||
#define UPDATE_PERIOD_MS 20
|
||||
#define THROTTLE_FAILSAFE -0.1f
|
||||
#define ARMED_THRESHOLD 0.50f
|
||||
// safe band to allow a bit of calibration error or trim offset (in microseconds)
|
||||
#define CONNECTION_OFFSET 250
|
||||
#define CONNECTION_OFFSET 250
|
||||
|
||||
#define ASSISTEDCONTROL_DEADBAND_MINIMUM 0.02f // minimum value for a well bahaved Tx.
|
||||
|
||||
// Private types
|
||||
|
||||
@ -79,8 +85,12 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
|
||||
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
|
||||
static void applyDeadband(float *value, float deadband);
|
||||
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position);
|
||||
#endif
|
||||
|
||||
#ifdef USE_INPUT_LPF
|
||||
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT);
|
||||
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT);
|
||||
#endif
|
||||
|
||||
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
|
||||
@ -129,6 +139,10 @@ int32_t ReceiverInitialize()
|
||||
ManualControlCommandInitialize();
|
||||
ReceiverActivityInitialize();
|
||||
ManualControlSettingsInitialize();
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
StabilizationSettingsInitialize();
|
||||
#endif
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -363,11 +377,30 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1;
|
||||
}
|
||||
|
||||
float deadband_checked = settings.Deadband;
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
// AssistedControl must have deadband set for pitch/roll hence
|
||||
// we default to a higher value for badly behaved TXs and also enforce a minimum value
|
||||
// for well behaved TXs
|
||||
uint8_t assistedMode = isAssistedFlightMode(cmd.FlightModeSwitchPosition);
|
||||
if (assistedMode) {
|
||||
deadband_checked = settings.DeadbandAssistedControl;
|
||||
if (deadband_checked < ASSISTEDCONTROL_DEADBAND_MINIMUM) {
|
||||
deadband_checked = ASSISTEDCONTROL_DEADBAND_MINIMUM;
|
||||
}
|
||||
|
||||
// If user has set settings.Deadband to a higher value...we use that.
|
||||
if (deadband_checked < settings.Deadband) {
|
||||
deadband_checked = settings.Deadband;
|
||||
}
|
||||
}
|
||||
#endif // PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
|
||||
// Apply deadband for Roll/Pitch/Yaw stick inputs
|
||||
if (settings.Deadband > 0.0f) {
|
||||
applyDeadband(&cmd.Roll, settings.Deadband);
|
||||
applyDeadband(&cmd.Pitch, settings.Deadband);
|
||||
applyDeadband(&cmd.Yaw, settings.Deadband);
|
||||
if (deadband_checked > 0.0f) {
|
||||
applyDeadband(&cmd.Roll, deadband_checked);
|
||||
applyDeadband(&cmd.Pitch, deadband_checked);
|
||||
applyDeadband(&cmd.Yaw, deadband_checked);
|
||||
}
|
||||
#ifdef USE_INPUT_LPF
|
||||
// Apply Low Pass Filter to input channels, time delta between calls in ms
|
||||
@ -377,9 +410,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
(float)UPDATE_PERIOD_MS;
|
||||
lastSysTimeLPF = thisSysTime;
|
||||
|
||||
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
|
||||
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
|
||||
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
|
||||
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings.ResponseTime, deadband_checked, dT);
|
||||
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings.ResponseTime, deadband_checked, dT);
|
||||
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings.ResponseTime, deadband_checked, dT);
|
||||
#endif // USE_INPUT_LPF
|
||||
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
|
||||
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
|
||||
@ -389,7 +422,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
applyDeadband(&cmd.Collective, settings.Deadband);
|
||||
}
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT);
|
||||
applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings.ResponseTime, settings.Deadband, dT);
|
||||
#endif // USE_INPUT_LPF
|
||||
}
|
||||
|
||||
@ -409,7 +442,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings.ResponseTime, settings.Deadband, dT);
|
||||
#endif
|
||||
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
|
||||
@ -419,7 +452,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings.ResponseTime, settings.Deadband, dT);
|
||||
#endif
|
||||
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING);
|
||||
@ -429,7 +462,7 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings.ResponseTime, settings.Deadband, dT);
|
||||
#endif
|
||||
|
||||
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
|
||||
@ -440,6 +473,8 @@ static void receiverTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
// Update cmd object
|
||||
ManualControlCommandSet(&cmd);
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_USB_RCTX)
|
||||
if (pios_usb_rctx_id) {
|
||||
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
|
||||
@ -658,16 +693,42 @@ static void applyDeadband(float *value, float deadband)
|
||||
/**
|
||||
* @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel
|
||||
*/
|
||||
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
|
||||
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT)
|
||||
{
|
||||
if (ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel]) {
|
||||
float rt = (float)ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel];
|
||||
float rt = (float)ManualControlSettingsResponseTimeToArray((*responseTime))[channel];
|
||||
|
||||
if (rt > 0.0f) {
|
||||
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
|
||||
|
||||
// avoid a long tail of non-zero data. if we have deadband, once the filtered result reduces to 1/10th
|
||||
// of deadband revert to 0. We downstream rely on this to know when sticks are centered.
|
||||
if (deadband > 0.0f && fabsf(inputFiltered[channel]) < deadband / 10.0f) {
|
||||
inputFiltered[channel] = 0.0f;
|
||||
}
|
||||
*value = inputFiltered[channel];
|
||||
}
|
||||
}
|
||||
#endif // USE_INPUT_LPF
|
||||
|
||||
/**
|
||||
* Check and set modes for gps assisted stablised flight modes
|
||||
*/
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
static uint8_t isAssistedFlightMode(uint8_t position)
|
||||
{
|
||||
uint8_t isAssistedFlag = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
|
||||
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
|
||||
|
||||
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
|
||||
|
||||
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
|
||||
isAssistedFlag = FlightModeAssistMap[position];
|
||||
}
|
||||
|
||||
return isAssistedFlag;
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -19,6 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += pathplan
|
||||
UAVOBJSRCFILENAMES += pathstatus
|
||||
UAVOBJSRCFILENAMES += pathsummary
|
||||
UAVOBJSRCFILENAMES += positionstate
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
|
||||
@ -19,6 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += pathplan
|
||||
UAVOBJSRCFILENAMES += pathstatus
|
||||
UAVOBJSRCFILENAMES += pathsummary
|
||||
UAVOBJSRCFILENAMES += positionstate
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
|
||||
@ -19,6 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += pathplan
|
||||
UAVOBJSRCFILENAMES += pathstatus
|
||||
UAVOBJSRCFILENAMES += pathsummary
|
||||
UAVOBJSRCFILENAMES += positionstate
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
|
||||
@ -24,6 +24,7 @@
|
||||
# (all architectures)
|
||||
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
UAVOBJSRCFILENAMES += actuatordesired
|
||||
@ -77,6 +78,7 @@ UAVOBJSRCFILENAMES += pathaction
|
||||
UAVOBJSRCFILENAMES += pathdesired
|
||||
UAVOBJSRCFILENAMES += pathplan
|
||||
UAVOBJSRCFILENAMES += pathstatus
|
||||
UAVOBJSRCFILENAMES += pathsummary
|
||||
UAVOBJSRCFILENAMES += positionstate
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += revocalibration
|
||||
|
||||
@ -148,6 +148,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
|
||||
}
|
||||
|
||||
addWidgetBinding("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f);
|
||||
addWidgetBinding("ManualControlSettings", "DeadbandAssistedControl", ui->assistedControlDeadband, 0, 0.01f);
|
||||
|
||||
connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard()));
|
||||
connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int)));
|
||||
@ -1494,26 +1495,32 @@ void ConfigInputWidget::updatePositionSlider()
|
||||
case 6:
|
||||
ui->fmsModePos6->setEnabled(true);
|
||||
ui->pidBankSs1_5->setEnabled(true);
|
||||
ui->assistControlPos6->setEnabled(true);
|
||||
// pass through
|
||||
case 5:
|
||||
ui->fmsModePos5->setEnabled(true);
|
||||
ui->pidBankSs1_4->setEnabled(true);
|
||||
ui->assistControlPos5->setEnabled(true);
|
||||
// pass through
|
||||
case 4:
|
||||
ui->fmsModePos4->setEnabled(true);
|
||||
ui->pidBankSs1_3->setEnabled(true);
|
||||
ui->assistControlPos4->setEnabled(true);
|
||||
// pass through
|
||||
case 3:
|
||||
ui->fmsModePos3->setEnabled(true);
|
||||
ui->pidBankSs1_2->setEnabled(true);
|
||||
ui->assistControlPos3->setEnabled(true);
|
||||
// pass through
|
||||
case 2:
|
||||
ui->fmsModePos2->setEnabled(true);
|
||||
ui->pidBankSs1_1->setEnabled(true);
|
||||
ui->assistControlPos2->setEnabled(true);
|
||||
// pass through
|
||||
case 1:
|
||||
ui->fmsModePos1->setEnabled(true);
|
||||
ui->pidBankSs1_0->setEnabled(true);
|
||||
ui->assistControlPos1->setEnabled(true);
|
||||
// pass through
|
||||
case 0:
|
||||
break;
|
||||
@ -1523,26 +1530,32 @@ void ConfigInputWidget::updatePositionSlider()
|
||||
case 0:
|
||||
ui->fmsModePos1->setEnabled(false);
|
||||
ui->pidBankSs1_0->setEnabled(false);
|
||||
ui->assistControlPos1->setEnabled(false);
|
||||
// pass through
|
||||
case 1:
|
||||
ui->fmsModePos2->setEnabled(false);
|
||||
ui->pidBankSs1_1->setEnabled(false);
|
||||
ui->assistControlPos2->setEnabled(false);
|
||||
// pass through
|
||||
case 2:
|
||||
ui->fmsModePos3->setEnabled(false);
|
||||
ui->pidBankSs1_2->setEnabled(false);
|
||||
ui->assistControlPos3->setEnabled(false);
|
||||
// pass through
|
||||
case 3:
|
||||
ui->fmsModePos4->setEnabled(false);
|
||||
ui->pidBankSs1_3->setEnabled(false);
|
||||
ui->assistControlPos4->setEnabled(false);
|
||||
// pass through
|
||||
case 4:
|
||||
ui->fmsModePos5->setEnabled(false);
|
||||
ui->pidBankSs1_4->setEnabled(false);
|
||||
ui->assistControlPos5->setEnabled(false);
|
||||
// pass through
|
||||
case 5:
|
||||
ui->fmsModePos6->setEnabled(false);
|
||||
ui->pidBankSs1_5->setEnabled(false);
|
||||
ui->assistControlPos6->setEnabled(false);
|
||||
// pass through
|
||||
case 6:
|
||||
default:
|
||||
|
||||
@ -6,8 +6,8 @@
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>796</width>
|
||||
<height>591</height>
|
||||
<width>1250</width>
|
||||
<height>755</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="windowTitle">
|
||||
@ -234,6 +234,29 @@
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="labelAssistedControlDeadband">
|
||||
<property name="text">
|
||||
<string>Assisted Control stick deadband </string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QDoubleSpinBox" name="assistedControlDeadband">
|
||||
<property name="toolTip">
|
||||
<string>Assisted Control stick deadband in percents of full range (2-12) for use with GPSAssist. This can not be disabled.</string>
|
||||
</property>
|
||||
<property name="decimals">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<double>2.000000000000000</double>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>12.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
@ -1206,197 +1229,6 @@ font:bold;</string>
|
||||
<property name="horizontalSpacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item row="0" column="6">
|
||||
<widget class="QLabel" name="label_111">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>80</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Settings Bank</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="10">
|
||||
<widget class="QLabel" name="label_16">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Flight Mode Count</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="10">
|
||||
<widget class="QSpinBox" name="fmsPosNum">
|
||||
<property name="toolTip">
|
||||
<string>Number of positions your FlightMode switch has.
|
||||
|
||||
Default is 3.
|
||||
|
||||
It will be 2 or 3 for most of setups, but it also can be up to 6.
|
||||
In that case you have to configure your radio mixers so the whole range
|
||||
from min to max is split into N equal intervals, and you may set arbitrary
|
||||
channel value for each flight mode.</string>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>3</number>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="10">
|
||||
<widget class="QLabel" name="label_15">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Ignored" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>10</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string><html><head/><body><p>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</p></body></html></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<spacer name="horizontalSpacer_6">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="7">
|
||||
<spacer name="horizontalSpacer_16">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<spacer name="horizontalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="8" rowspan="3">
|
||||
<widget class="Line" name="line">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_13">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Flight Mode</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0" rowspan="2">
|
||||
<widget class="QFrame" name="frame">
|
||||
<property name="minimumSize">
|
||||
@ -1941,6 +1773,418 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="6">
|
||||
<widget class="QLabel" name="label_111">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Settings Bank</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="12">
|
||||
<widget class="QLabel" name="label_16">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Flight Mode Count</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="12">
|
||||
<widget class="QSpinBox" name="fmsPosNum">
|
||||
<property name="toolTip">
|
||||
<string>Number of positions your FlightMode switch has.
|
||||
|
||||
Default is 3.
|
||||
|
||||
It will be 2 or 3 for most of setups, but it also can be up to 6.
|
||||
In that case you have to configure your radio mixers so the whole range
|
||||
from min to max is split into N equal intervals, and you may set arbitrary
|
||||
channel value for each flight mode.</string>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="value">
|
||||
<number>3</number>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="12">
|
||||
<widget class="QLabel" name="label_15">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Ignored" vsizetype="Preferred">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>10</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string><html><head/><body><p>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</p></body></html></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
<spacer name="horizontalSpacer_6">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="7">
|
||||
<spacer name="horizontalSpacer_16">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<spacer name="horizontalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>10</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="10" rowspan="3">
|
||||
<widget class="Line" name="line">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<widget class="QLabel" name="label_13">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>16777215</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Flight Mode</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="8">
|
||||
<widget class="QLabel" name="label_20">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>138</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="layoutDirection">
|
||||
<enum>Qt::LeftToRight</enum>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||
color: rgb(255, 255, 255);
|
||||
border-radius: 5;
|
||||
margin:1px;
|
||||
font:bold;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Assisted Control</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="8" rowspan="2">
|
||||
<widget class="QFrame" name="frame_4">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="frameShape">
|
||||
<enum>QFrame::NoFrame</enum>
|
||||
</property>
|
||||
<property name="frameShadow">
|
||||
<enum>QFrame::Plain</enum>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_4">
|
||||
<property name="leftMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>1</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos1">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:0</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos2">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:1</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos3">
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:2</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos4">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:3</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos5">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:4</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QComboBox" name="assistControlPos6">
|
||||
<property name="enabled">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="minimumSize">
|
||||
<size>
|
||||
<width>75</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="focusPolicy">
|
||||
<enum>Qt::StrongFocus</enum>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html></string>
|
||||
</property>
|
||||
<property name="objrelation" stdset="0">
|
||||
<stringlist>
|
||||
<string>objname:StabilizationSettings</string>
|
||||
<string>fieldname:FlightModeAssistMap</string>
|
||||
<string>index:5</string>
|
||||
<string>haslimits:no</string>
|
||||
<string>scale:1</string>
|
||||
<string>buttongroup:16</string>
|
||||
</stringlist>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
@ -86,7 +86,7 @@ while (1)
|
||||
if (bufferlen < headerIdx + 12); break; end
|
||||
sync = buffer(headerIdx);
|
||||
% printf('%x ', sync);
|
||||
headerIdx += 1;
|
||||
headerIdx = headerIdx + 1;
|
||||
if sync ~= correctSyncByte
|
||||
wrongSyncByte = wrongSyncByte + 1;
|
||||
continue
|
||||
@ -99,7 +99,7 @@ while (1)
|
||||
|
||||
% get msg type (quint8 1 byte ) should be 0x20/0xA0, ignore the rest
|
||||
msgType = buffer(headerIdx);
|
||||
headerIdx += 1;
|
||||
headerIdx = headerIdx + 1;
|
||||
if msgType ~= correctMsgByte && msgType ~= correctTimestampedByte
|
||||
% fixme: it should read the whole message payload instead of skipping and blindly searching for next sync byte.
|
||||
fprintf('\nSkipping message type: %x \n', msgType);
|
||||
@ -108,16 +108,16 @@ while (1)
|
||||
|
||||
% get msg size (quint16 2 bytes) excludes crc, include msg header and data payload
|
||||
msgSize = uint32(typecast(buffer(headerIdx:headerIdx + 1), 'uint16'));
|
||||
headerIdx += 2;
|
||||
headerIdx = headerIdx + 2;
|
||||
|
||||
% get obj id (quint32 4 bytes)
|
||||
objID = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32');
|
||||
headerIdx += 4;
|
||||
headerIdx = headerIdx + 4;
|
||||
|
||||
% get instance id (quint16 2 bytes)
|
||||
instID = typecast(uint8(buffer(headerIdx:headerIdx + 1)), 'uint16');
|
||||
% printf('Id %x type %x size %u Inst %x ', objID, msgType, msgSize, instID);
|
||||
headerIdx += 2;
|
||||
headerIdx = headerIdx + 2;
|
||||
|
||||
% get timestamp if needed (quint32 4 bytes)
|
||||
if msgType == correctMsgByte
|
||||
@ -125,12 +125,12 @@ while (1)
|
||||
elseif msgType == correctTimestampedByte
|
||||
timestamp = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32');
|
||||
% printf('ts %u');
|
||||
headerIdx += 4;
|
||||
headerIdx = headerIdx + 4;
|
||||
datasize = msgSize - headerLen - timestampLen;
|
||||
end
|
||||
% printf('\n');
|
||||
bufferIdx = headerIdx;
|
||||
headerIdx += datasize + crcLen + oplHeaderLen;
|
||||
headerIdx = headerIdx + datasize + crcLen + oplHeaderLen;
|
||||
|
||||
%% Read object
|
||||
|
||||
|
||||
@ -27,6 +27,7 @@ OTHER_FILES += UAVObjects.pluginspec
|
||||
|
||||
# Add in all of the synthetic/generated uavobject files
|
||||
HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/adjustments.h \
|
||||
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/accessorydesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/barosensor.h \
|
||||
@ -82,6 +83,7 @@ HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/pathdesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/pathplan.h \
|
||||
$$UAVOBJECT_SYNTHETICS/pathstatus.h \
|
||||
$$UAVOBJECT_SYNTHETICS/pathsummary.h \
|
||||
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \
|
||||
$$UAVOBJECT_SYNTHETICS/positionstate.h \
|
||||
$$UAVOBJECT_SYNTHETICS/flightbatterystate.h \
|
||||
@ -131,6 +133,7 @@ HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/perfcounter.h
|
||||
|
||||
SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/adjustments.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \
|
||||
@ -186,6 +189,7 @@ SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/pathplan.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/pathsummary.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/positionstate.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \
|
||||
|
||||
15
shared/uavobjectdefinition/adjustments.xml
Normal file
15
shared/uavobjectdefinition/adjustments.xml
Normal file
@ -0,0 +1,15 @@
|
||||
<xml>
|
||||
<object name="Adjustments" singleinstance="true" settings="false" category="Navigation">
|
||||
<description>Automatically calculated adjustments to parameters used into auto flight modes. Can come from @ref PathFollower </description>
|
||||
|
||||
<field name="NeutralThrustOffset" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NeutralThrustCorrection" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NeutralThrustAccumulator" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NeutralThrustRange" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -5,6 +5,10 @@
|
||||
|
||||
<!-- Note these enumerated values should be the same as ManualControlSettings -->
|
||||
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise"/>
|
||||
<!-- the options for FlightModeAssist needs to match the StablizationSettings' assist options -->
|
||||
<field name="FlightModeAssist" units="" type="enum" elements="1" options="None,GPSAssist_PrimaryThrust,GPSAssist"/>
|
||||
<field name="AssistedControlState" units="" type="enum" elements="1" options="Primary,Brake,Hold" defaultvalue="Primary"/>
|
||||
<field name="AssistedThrottleState" units="" type="enum" elements="1" options="Manual,Auto,AutoOverride" defaultvalue="Manual"/>
|
||||
|
||||
<field name="ControlChain" units="bool" type="enum" options="false,true">
|
||||
<elementnames>
|
||||
|
||||
@ -16,6 +16,8 @@
|
||||
elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2"/>
|
||||
|
||||
<field name="Deadband" units="%" type="float" elements="1" defaultvalue="0.02"/>
|
||||
<!-- Note the following deadband is used in AssistedControl. Use of deadband with AssistedControl is not optional and will have a hardcoded minimum. -->
|
||||
<field name="DeadbandAssistedControl" units="%" type="float" elements="1" defaultvalue="0.08" description="Stick deadband used for AssistedControl"/>
|
||||
|
||||
|
||||
<!-- Note these options values should be identical to those defined in FlightMode -->
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
<description>The endpoint or path the craft is trying to achieve. Can come from @ref ManualControl or @ref PathPlanner </description>
|
||||
|
||||
<field name="Start" units="m" type="float" elementnames="North,East,Down" default="0"/>
|
||||
<field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
|
||||
<field name="End" units="m" type="float" elementnames="North,East,Down" default="0"/>
|
||||
<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
|
||||
<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
|
||||
|
||||
@ -12,7 +12,7 @@
|
||||
FixedAttitude,
|
||||
SetAccessory,
|
||||
Land,
|
||||
DisarmAlarm" default="0"/>
|
||||
DisarmAlarm,Brake" default="0"/>
|
||||
<!-- Endpoint mode - move directly towards endpoint regardless of position -->
|
||||
<!-- Straight Mode - move across linear path through Start towards the waypoint end, adjusting velocity - continue straight -->
|
||||
<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->
|
||||
|
||||
@ -13,6 +13,7 @@
|
||||
<field name="correction_direction_north" units="m" type="float" elements="1"/>
|
||||
<field name="correction_direction_east" units="m" type="float" elements="1"/>
|
||||
<field name="correction_direction_down" units="m" type="float" elements="1"/>
|
||||
<field name="path_time" units="s" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
19
shared/uavobjectdefinition/pathsummary.xml
Normal file
19
shared/uavobjectdefinition/pathsummary.xml
Normal file
@ -0,0 +1,19 @@
|
||||
<xml>
|
||||
<object name="PathSummary" singleinstance="true" settings="false" category="Navigation">
|
||||
<description>Summary of a completed path segment Can come from any @ref PathFollower module</description>
|
||||
|
||||
<field name="UID" units="" type="int16" elements="1" default="0"/>
|
||||
<!-- unique ID confirmed with pathfollower in pathsummary to acknowledge a completed in PathDesired brake sequence -->
|
||||
<field name="brake_exit_reason" units="" type="enum" elements="1" options="Timeout,PathCompleted,PathError"/>
|
||||
<field name="brake_distance_offset" units="m" type="float" elements="1"/>
|
||||
<field name="time_remaining" units="s" type="float" elements="1"/>
|
||||
<field name="fractional_progress" units="" type="float" elements="1"/>
|
||||
<field name="decelrate" units="m/s2" type="float" elements="1"/>
|
||||
<field name="brakeRateActualDesiredRatio" units="" type="float" elements="1"/>
|
||||
<field name="velocityIntoHold" units="m/s" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -46,6 +46,10 @@
|
||||
|
||||
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>
|
||||
<field name="FlightModeAssistMap" units="" type="enum"
|
||||
options="None,GPSAssist"
|
||||
elements="6"
|
||||
defaultvalue="None,None,None,None,None,None"/>
|
||||
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
@ -19,6 +19,10 @@
|
||||
<field name="EmergencyFallbackYawRate" units="(deg/s)/deg" type="float" elementnames="kP,Max" defaultvalue="2.0, 30.0"/>
|
||||
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
|
||||
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="50"/>
|
||||
<field name="BrakeRate" units="m/s2" type="float" elements="1" defaultvalue="2.5"/>
|
||||
<field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
|
||||
<field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="12.0, 0.0, 0.03, 15"/>
|
||||
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user