/**
* Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_CONFIGERROR_NONE or SYSTEMALARMS_CONFIGERROR_MULTIROTOR
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
return SYSTEMALARMS_CONFIGERROR_MULTIROTOR;
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch(index) {
case 1:
ManualControlSettingsStabilization1SettingsGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsGet(modes);
break;
default:
return SYSTEMALARMS_CONFIGERROR_NONE;
}
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for(uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE)
return SYSTEMALARMS_CONFIGERROR_MULTIROTOR;
// If this axis allows enabling an autotune behavior without the module
// running then set an alarm now that aututune module initializes the
// appropriate objects
if ((modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
(!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE)))
return SYSTEMALARMS_CONFIGERROR_AUTOTUNE;
}
}
// POI mode is only valid for YAW in the case it is enabled and camera stab is running
if (modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ROLL] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI ||
modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_PITCH] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI)
return SYSTEMALARMS_CONFIGERROR_STABILIZATION;
if (modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_YAW] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI) {
#if !defined(CAMERASTAB_POI_MODE)
return SYSTEMALARMS_CONFIGERROR_STABILIZATION;
#endif
// TODO: Need to check camera stab is actually running
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_CONFIGERROR_NONE;
}
void Stabilization1SettingsSet(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
if (!security(1))
return;
ManualControlSettingsData data;
ManualControlSettingsStabilization1SettingsGet(data.Stabilization1Settings);
data.Stabilization1Settings[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ROLL] = Param[0]->Val->Integer;
data.Stabilization1Settings[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_PITCH] = Param[1]->Val->Integer;
data.Stabilization1Settings[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_YAW] = Param[2]->Val->Integer;
ManualControlSettingsStabilization1SettingsSet(data.Stabilization1Settings);
}
