rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

cherry-pick of Sambas Arm-On-A-Switch capability implementation

Browse Source
This commit is contained in:
Corvus Corax 2013-07-18 10:45:36 +02:00
parent 6c88184b32
commit 658b2fc804
3 changed files with 28 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/modules/ManualControl/inc/manualcontrol.h
View File

@ -86,9 +86,10 @@ int32_t ManualControlInitialize();
((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \
) )
#define ARMING_CHANNEL_ROLL 0 #define ARMING_CHANNEL_ROLL 0
#define ARMING_CHANNEL_PITCH 1 #define ARMING_CHANNEL_PITCH 1
#define ARMING_CHANNEL_YAW 2 #define ARMING_CHANNEL_YAW 2
#define ARMING_CHANNEL_ACCESSORY2 3
#define assumptions7 \ #define assumptions7 \
( \ ( \
@ -97,7 +98,9 @@ int32_t ManualControlInitialize();
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \ (((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_PITCHAFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_PITCH) && \
(((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_YAW) && \ (((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 \ #define assumptions8 \
@ -107,7 +110,9 @@ int32_t ManualControlInitialize();
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \ (((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
(((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT - (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_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 \ #define assumptions_flightmode \

20
flight/modules/ManualControl/manualcontrol.c
View File

@ -98,7 +98,7 @@ static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData *settings, float flightMode); 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 setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev); static void configurationUpdatedCb(UAVObjEvent *ev);
@ -317,6 +317,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
disconnected_count = 0; disconnected_count = 0;
} }
bool armSwitch = false;
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0; cmd.Roll = 0;
@ -414,6 +415,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif #endif
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = true;
} else {
armSwitch = false;
}
if (AccessoryDesiredInstSet(2, &accessory) != 0) { if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); 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 // Process arming outside conditional so system will disarm when disconnected
processArm(&cmd, &settings); processArm(&cmd, &settings, armSwitch);
// Update cmd object // Update cmd object
ManualControlCommandSet(&cmd); ManualControlCommandSet(&cmd);
@ -1012,10 +1019,14 @@ static void setArmedIfChanged(uint8_t val)
* @param[out] cmd The structure to set the armed in * @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position * @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; bool lowThrottle = cmd->Throttle <= 0;
if (((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) == ARMING_CHANNEL_ACCESSORY2) {
lowThrottle = true;
}
if (forcedDisArm()) { if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
@ -1072,6 +1083,9 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
case ARMING_CHANNEL_YAW: case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw; armingInputLevel = sign * cmd->Yaw;
break; break;
case ARMING_CHANNEL_ACCESSORY2:
armingInputLevel = (armSwitch ? -1 : 1);
break;
} }
bool manualArm = false; bool manualArm = false;

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -17,7 +17,7 @@
<field name="Deadband" units="%" type="float" elements="1" defaultvalue="0"/> <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 --> <!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum" <field name="Stabilization1Settings" units="" type="enum"