mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
cherry-pick of Sambas Arm-On-A-Switch capability implementation
This commit is contained in:
parent
6c88184b32
commit
658b2fc804
@ -86,9 +86,10 @@ int32_t ManualControlInitialize();
|
||||
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
|
||||
)
|
||||
|
||||
#define ARMING_CHANNEL_ROLL 0
|
||||
#define ARMING_CHANNEL_PITCH 1
|
||||
#define ARMING_CHANNEL_YAW 2
|
||||
#define ARMING_CHANNEL_ROLL 0
|
||||
#define ARMING_CHANNEL_PITCH 1
|
||||
#define ARMING_CHANNEL_YAW 2
|
||||
#define ARMING_CHANNEL_ACCESSORY2 3
|
||||
|
||||
#define assumptions7 \
|
||||
( \
|
||||
@ -97,7 +98,9 @@ int32_t ManualControlInitialize();
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_ACC2LOW - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ACCESSORY2) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_ACC2HIGH - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ACCESSORY2) \
|
||||
)
|
||||
|
||||
#define assumptions8 \
|
||||
@ -107,7 +110,9 @@ int32_t ManualControlInitialize();
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_ACC2LOW - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
|
||||
(((int)MANUALCONTROLSETTINGS_ARMING_ACC2HIGH - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) \
|
||||
)
|
||||
|
||||
#define assumptions_flightmode \
|
||||
|
@ -98,7 +98,7 @@ static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
|
||||
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
|
||||
static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
|
||||
static void processFlightMode(ManualControlSettingsData *settings, float flightMode);
|
||||
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings);
|
||||
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, bool armSwitch);
|
||||
static void setArmedIfChanged(uint8_t val);
|
||||
static void configurationUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
@ -317,6 +317,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
|
||||
disconnected_count = 0;
|
||||
}
|
||||
|
||||
bool armSwitch = false;
|
||||
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
|
||||
cmd.Throttle = -1; // Shut down engine with no control
|
||||
cmd.Roll = 0;
|
||||
@ -414,6 +415,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
|
||||
#endif
|
||||
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
|
||||
armSwitch = true;
|
||||
} else {
|
||||
armSwitch = false;
|
||||
}
|
||||
|
||||
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
@ -423,7 +430,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
|
||||
// Process arming outside conditional so system will disarm when disconnected
|
||||
processArm(&cmd, &settings);
|
||||
processArm(&cmd, &settings, armSwitch);
|
||||
|
||||
// Update cmd object
|
||||
ManualControlCommandSet(&cmd);
|
||||
@ -1012,10 +1019,14 @@ static void setArmedIfChanged(uint8_t val)
|
||||
* @param[out] cmd The structure to set the armed in
|
||||
* @param[in] settings Settings indicating the necessary position
|
||||
*/
|
||||
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
|
||||
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, bool armSwitch)
|
||||
{
|
||||
bool lowThrottle = cmd->Throttle <= 0;
|
||||
|
||||
if (((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) == ARMING_CHANNEL_ACCESSORY2) {
|
||||
lowThrottle = true;
|
||||
}
|
||||
|
||||
if (forcedDisArm()) {
|
||||
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
|
||||
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
||||
@ -1072,6 +1083,9 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
|
||||
case ARMING_CHANNEL_YAW:
|
||||
armingInputLevel = sign * cmd->Yaw;
|
||||
break;
|
||||
case ARMING_CHANNEL_ACCESSORY2:
|
||||
armingInputLevel = (armSwitch ? -1 : 1);
|
||||
break;
|
||||
}
|
||||
|
||||
bool manualArm = false;
|
||||
|
@ -17,7 +17,7 @@
|
||||
|
||||
<field name="Deadband" units="%" type="float" elements="1" defaultvalue="0"/>
|
||||
|
||||
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right" defaultvalue="Always Disarmed"/>
|
||||
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right,ACC2 low,ACC2 High" defaultvalue="Always Disarmed"/>
|
||||
|
||||
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
|
||||
<field name="Stabilization1Settings" units="" type="enum"
|
||||
|
Loading…
x
Reference in New Issue
Block a user