rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16: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) \
)
#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 \

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 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;

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -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"