rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/OP-1036_fixed-wing-improvements' into next

Browse Source
This commit is contained in:
Corvus Corax 2013-07-18 15:05:51 +02:00
commit b195ffda2f
3 changed files with 68 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -86,30 +86,6 @@ 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 assumptions7 \
( \
(((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \
(((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2 == ARMING_CHANNEL_ROLL) && \
(((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) \
)
#define assumptions8 \
( \
(((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
(((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT - (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_YAWLEFT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 == 0) && \
(((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT - (int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2 != 0) \
)
#define assumptions_flightmode \
( \
((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \

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

@ -67,7 +67,6 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50f
// safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 250
@ -98,7 +97,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, int8_t armSwitch);
static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
@ -125,7 +124,7 @@ static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount)
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount)
/**
* Module starting
@ -317,6 +316,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
disconnected_count = 0;
}
int8_t armSwitch = 0;
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
@ -394,6 +394,13 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
@ -404,6 +411,13 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
@ -414,6 +428,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) {
if (accessory.AccessoryVal > ARMED_THRESHOLD) {
armSwitch = 1;
} else if (accessory.AccessoryVal < -ARMED_THRESHOLD) {
armSwitch = -1;
}
}
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
@ -423,7 +445,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 +1034,25 @@ 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, int8_t armSwitch)
{
bool lowThrottle = cmd->Throttle <= 0;
/**
* do NOT check throttle if disarming via switch, must be instant
*/
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
if (armSwitch < 0) {
lowThrottle = true;
}
break;
default:
break;
}
if (forcedDisArm()) {
// PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...)
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
@ -1061,16 +1098,29 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
float armingInputLevel = 0;
// Calc channel see assumptions7
int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
case ARMING_CHANNEL_ROLL:
armingInputLevel = sign * cmd->Roll;
switch (settings->Arming) {
case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT:
armingInputLevel = 1.0f * cmd->Roll;
break;
case ARMING_CHANNEL_PITCH:
armingInputLevel = sign * cmd->Pitch;
case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT:
armingInputLevel = -1.0f * cmd->Roll;
break;
case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw;
case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD:
armingInputLevel = 1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_PITCHAFT:
armingInputLevel = -1.0f * cmd->Pitch;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWLEFT:
armingInputLevel = 1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT:
armingInputLevel = -1.0f * cmd->Yaw;
break;
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1:
case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * (float)armSwitch;
break;
}
@ -1097,7 +1147,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) {
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmingSequenceTime)) {
armState = ARM_STATE_ARMED;
} else if (!manualArm) {
armState = ARM_STATE_DISARMED;
@ -1126,7 +1176,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) {
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->DisarmingSequenceTime)) {
armState = ARM_STATE_DISARMED;
} else if (!manualDisarm) {
armState = ARM_STATE_ARMED;

4
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,Accessory 0,Accessory 1,Accessory 2" defaultvalue="Always Disarmed"/>
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum"
@ -71,6 +71,8 @@
%0903NE:Autotune:AltitudeVario:AltitudeHold:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner:POI"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="FailsafeBehavior" units="" type="enum" elements="1" options="None,ModePos1,ModePos2,ModePos3,ModePos4,ModePos5,ModePos6" defaultvalue="None"/>
<field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
<access gcs="readwrite" flight="readwrite"/>