diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index 214241874..37a5fa063 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -147,15 +147,15 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
- if(!assumptions)
+ if(!assumptions)
return -1;
-
+
AccessoryDesiredInitialize();
ManualControlSettingsInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
-
+
// Start main task
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
@@ -307,14 +307,14 @@ static void manualControlTask(void *parameters)
ManualControlCommandSet(&cmd);
}
-
+
} else {
ManualControlCommandGet(&cmd); /* Under GCS control */
}
FlightStatusGet(&flightStatus);
-
+
// Depending on the mode update the Stabilization or Actuator objects
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED:
@@ -330,11 +330,11 @@ static void manualControlTask(void *parameters)
case FLIGHTMODE_GUIDANCE:
// TODO: Implement
break;
- }
+ }
}
}
-static void updateActuatorDesired(ManualControlCommandData * cmd)
+static void updateActuatorDesired(ManualControlCommandData * cmd)
{
ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator);
@@ -342,17 +342,17 @@ static void updateActuatorDesired(ManualControlCommandData * cmd)
actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
- ActuatorDesiredSet(&actuator);
+ ActuatorDesiredSet(&actuator);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
-
+
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
-
+
uint8_t * stab_settings;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
@@ -369,30 +369,32 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
- return;
+ return;
}
-
+
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
-
+
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
- (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
- (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+ (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+ (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+ (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
- (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
- (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
- 0; // this is an invalid mode
+ (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+ (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
+ (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax :
+ 0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
- (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
- (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
- 0; // this is an invalid mode
-
- stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
+ (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+ (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) :
+ 0; // this is an invalid mode
+
+ stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
@@ -478,11 +480,11 @@ 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 lowThrottle = cmd->Throttle <= 0;
-
+
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
// In this configuration we always disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
@@ -490,7 +492,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
// Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
return;
-
+
// The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) {
switch(armState) {
@@ -507,20 +509,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
}
return;
}
-
+
// The rest of these cases throttle is low
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
return;
}
-
-
+
+
// When the configuration is not "Always armed" and no "Always disarmed",
// the state will not be changed when the throttle is not low
static portTickType armedDisarmStart;
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 ) {
@@ -528,26 +530,26 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
}
-
+
bool manualArm = false;
bool manualDisarm = false;
-
+
if (armingInputLevel <= -ARMED_THRESHOLD)
manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD)
manualDisarm = true;
-
+
switch(armState) {
case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-
+
// only allow arming if it's OK too
if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
-
+
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
@@ -556,7 +558,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
else if (!manualArm)
armState = ARM_STATE_DISARMED;
break;
-
+
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
@@ -564,19 +566,19 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
armState = ARM_STATE_DISARMING_TIMEOUT;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
break;
-
+
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
-
+
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
-
+
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
armState = ARM_STATE_DISARMED;
@@ -593,25 +595,25 @@ 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)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
-
+
uint8_t newMode;
// Note here the code is ass
- if (flightMode < -FLIGHT_MODE_LIMIT)
+ if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
- else if (flightMode > FLIGHT_MODE_LIMIT)
+ else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
- else
- newMode = settings->FlightModePosition[1];
-
+ else
+ newMode = settings->FlightModePosition[1];
+
if(flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
-
+
}
/**
diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml
index 95423a3f5..375de146b 100644
--- a/shared/uavobjectdefinition/manualcontrolsettings.xml
+++ b/shared/uavobjectdefinition/manualcontrolsettings.xml
@@ -11,15 +11,15 @@
-
+
-
-
-
-
+
+
+
+
-
+
diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml
index c3bda9a2b..f8e6e173d 100644
--- a/shared/uavobjectdefinition/stabilizationdesired.xml
+++ b/shared/uavobjectdefinition/stabilizationdesired.xml
@@ -6,7 +6,7 @@
-
+