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

OP-1154 Add FlightModeSwitchPosition to ManualControlCommand

Browse Source
This commit is contained in:
Cliff Geerdes 2014-01-15 13:53:14 -05:00
parent ccc09721cd
commit e6490c0cea
4 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -96,7 +96,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
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 processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd);
static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch);
static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent *ev);
@ -358,6 +358,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
cmd.FlightModeSwitchPosition = (uint8_t) 255;
} else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
@ -446,7 +447,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
processFlightMode(&settings, flightMode);
processFlightMode(&settings, flightMode, &cmd);
}
// Process arming outside conditional so system will disarm when disconnected
@ -1204,7 +1205,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData
* @param[in] settings The settings which indicate which position is which mode
* @param[in] flightMode the value of the switch position
*/
static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd)
{
FlightStatusData flightStatus;
@ -1216,6 +1217,8 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
pos = settings->FlightModeNumber - 1;
}
cmd->FlightModeSwitchPosition = pos;
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {

17
flight/modules/Stabilization/stabilization.c
View File

@ -44,6 +44,7 @@
#include "airspeedstate.h"
#include "gyrostate.h"
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
@ -71,6 +72,9 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define FAILSAFE_TIMEOUT_MS 30
// number of flight mode switch positions
#define NUM_FMS_POSITIONS 6
// The PID_RATE_ROLL set is used by Rate mode and the rate portion of Attitude mode
// The PID_RATE set is used by the attitude portion of Attitude mode
// The PID_RATEA_ROLL set is used by Rattitude mode because it needs to maintain
@ -99,6 +103,7 @@ static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
static uint8_t cruise_control_flight_mode_switch_pos_enable[NUM_FMS_POSITIONS];
// Private functions
static void stabilizationTask(void *parameters);
@ -567,8 +572,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (cruise_control_max_power_factor > 0.0001f) {
uint8_t flight_mode_switch_position;
ManualControlCommandFlightModeSwitchPositionGet(&flight_mode_switch_position);
if (flight_mode_switch_position < NUM_FMS_POSITIONS
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t) 0
&& cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
float angle;
@ -827,6 +835,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float) settings.CruiseControlNeutralThrust / 100.0f;
memcpy(
cruise_control_flight_mode_switch_pos_enable,
settings.CruiseControlFlightModeSwitchPosEnable,
sizeof(cruise_control_flight_mode_switch_pos_enable));
}

1
shared/uavobjectdefinition/manualcontrolcommand.xml
View File

@ -8,6 +8,7 @@
<field name="Yaw" units="%" type="float" elements="1"/>
<field name="Collective" units="%" type="float" elements="1"/>
<field name="Channel" units="us" type="uint16" elements="9"/>
<field name="FlightModeSwitchPosition" units="" type="uint8" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="2000"/>

1
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -42,6 +42,7 @@
<field name="CruiseControlPowerTrim" units="%" type="float" elements="1" defaultvalue="100.0"/>
<field name="CruiseControlInvertedPowerSwitch" units="" type="int8" elements="1" defaultvalue="0"/>
<field name="CruiseControlNeutralThrust" units="%" type="uint8" elements="1" defaultvalue="0"/>
<field name="CruiseControlFlightModeSwitchPosEnable" units="" type="uint8" elements="6" defaultvalue="0"/>
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>