void vtol_follower_control_settings_updated(UAVObjEvent * ev,
void *ctx, void *obj, int len)
{
(void) ctx; (void) obj; (void) len;
VtolPathFollowerSettingsGet(&guidanceSettings);
// Configure the velocity control PID loops
pid_configure(&vtol_pids[NORTH_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
pid_configure(&vtol_pids[EAST_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
// Configure the position control (velocity output) PID loops
pid_configure(&vtol_pids[NORTH_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
pid_configure(&vtol_pids[EAST_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
// The parameters for vertical control are shared with Altitude Hold
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&vtol_pids[DOWN_POSITION], altitudeHoldSettings.PositionKp, 0, 0, 0);
pid_configure(&vtol_pids[DOWN_VELOCITY],
altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi,
0, 1); // Note the ILimit here is 1 because we use this offset to set the throttle offset
// Calculate the constants used in the deadband calculation
cubic_deadband_setup(guidanceSettings.EndpointDeadbandWidth,
guidanceSettings.EndpointDeadbandCenterGain,
&vtol_end_m, &vtol_end_r);
cubic_deadband_setup(guidanceSettings.PathDeadbandWidth,
guidanceSettings.PathDeadbandCenterGain,
&vtol_path_m, &vtol_path_r);
// calculate the loiter time constants.
loiter_brakealpha = expf(-(guidanceSettings.UpdatePeriod / 1000.0f) / guidanceSettings.LoiterBrakingTimeConstant);
loiter_errordecayalpha = expf(-(guidanceSettings.UpdatePeriod / 1000.0f) / guidanceSettings.LoiterErrorDecayConstant);
}
void vtol_follower_control_settings_updated(UAVObjEvent * ev)
{
VtolPathFollowerSettingsGet(&guidanceSettings);
// Configure the velocity control PID loops
pid_configure(&vtol_pids[NORTH_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
pid_configure(&vtol_pids[EAST_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
// Configure the position control (velocity output) PID loops
pid_configure(&vtol_pids[NORTH_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
pid_configure(&vtol_pids[EAST_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
// The parameters for vertical control are shared with Altitude Hold
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&vtol_pids[DOWN_POSITION], altitudeHoldSettings.PositionKp, 0, 0, 0);
pid_configure(&vtol_pids[DOWN_VELOCITY],
altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi,
0, 1); // Note the ILimit here is 1 because we use this offset to set the throttle offset
// Calculate the constants used in the deadband calculation
vtol_deadband_setup(guidanceSettings.EndpointDeadbandWidth,
guidanceSettings.EndpointDeadbandCenterGain,
&vtol_end_m, &vtol_end_r);
vtol_deadband_setup(guidanceSettings.PathDeadbandWidth,
guidanceSettings.PathDeadbandCenterGain,
&vtol_path_m, &vtol_path_r);
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
VtolPathFollowerSettingsGet(&guidanceSettings);
// Configure the velocity control PID loops
pid_configure(&vtol_pids[NORTH_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
pid_configure(&vtol_pids[EAST_VELOCITY],
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki
0, // Kd
guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
pid_configure(&vtol_pids[DOWN_VELOCITY],
guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP], // Kp
guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], // Ki
0, // Kd
guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
// Configure the position control (velocity output) PID loops
pid_configure(&vtol_pids[NORTH_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
pid_configure(&vtol_pids[EAST_POSITION],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP],
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
0,
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
pid_configure(&vtol_pids[DOWN_POSITION],
guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP],
guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
0,
guidanceSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
PathDesiredGet(&pathDesired);
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
float downVel = velocityActual.Down;
// Compute desired north command from velocity error
northError = velocityDesired.North - northVel;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward;
// Compute desired east command from velocity error
eastError = velocityDesired.East - eastVel;
eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward;
// Compute desired down command. Using NED accel as the damping term
downError = velocityDesired.Down - downVel;
// Negative is critical here since throttle is negative with down
downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) +
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD];
// If this setting is zero then the throttle level available when enabled is used for hover:wq
float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle;
stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw.
// For this to behave well the craft should move similarly for 5 deg roll versus 5 deg pitch.
// Notice the inputs are crudely bounded by the anti-winded but if both N and E were
// saturated and the craft were at 45 degrees that would result in a value greater than
// the limit, so apply limit again here.
stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) +
-eastCommand * sinf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) +
eastCommand * cosf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
float yaw;
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
}
/**
* Module thread, should not return.
*/
static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
portTickType lastUpdateTime;
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
VtolPathFollowerSettingsGet(&guidanceSettings);
PathDesiredGet(&pathDesired);
// Main task loop
lastUpdateTime = xTaskGetTickCount();
while (1) {
// Conditions when this runs:
// 1. Must have VTOL type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
vTaskDelay(1000);
continue;
}
// Continue collecting data if not enough time
vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod));
// Convert the accels into the NED frame
updateNedAccel();
FlightStatusGet(&flightStatus);
// Check the combinations of flightmode and pathdesired mode
switch(flightStatus.FlightMode) {
/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
* is correct. RETURNTOHOME mode uses HOLDPOSITION with the position
* set to home */
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
updateEndpointVelocity();
updateVtolDesiredAttitude();
} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
updatePathVelocity();
updateVtolDesiredAttitude();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
}
break;
default:
for (uint32_t i = 0; i < VTOL_PID_NUM; i++)
pid_zero(&vtol_pids[i]);
// Track throttle before engaging this mode. Cheap system ident
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
throttleOffset = stabDesired.Throttle;
break;
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
}
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool module_enabled;
#ifdef MODULE_TxPID_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (module_enabled) {
TxPIDSettingsInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
#endif
return 0;
}
return -1;
}
MODULE_INITCALL(TxPIDInitialize, NULL);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent* ev)
{
if (ev->obj != AccessoryDesiredHandle())
return;
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER)
return;
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED))
return;
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
VtolPathFollowerSettingsData vtolPathFollowerSettingsData;
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsGet(&vtolPathFollowerSettingsData);
}
bool vtolPathFollowerSettingsNeedsUpdate = false;
#endif
AccessoryDesiredData accessory;
bool stabilizationSettingsNeedsUpdate = false;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]);
} else {
continue;
}
switch (inst.PIDs[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_GYROCUTOFF:
stabilizationSettingsNeedsUpdate |= update(&stab.GyroCutoff, value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD], value);
break;
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKD:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value);
break;
#endif /* UAVOBJ_INIT_vtolpathfollowersettings */
default:
// Previously this would assert. But now the
// object may be missing and it's not worth a
// crash.
break;
}
}
}
// Update UAVOs, if necessary
if (stabilizationSettingsNeedsUpdate) {
StabilizationSettingsSet(&stab);
}
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
if (vtolPathFollowerSettingsNeedsUpdate) {
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsSet(&vtolPathFollowerSettingsData);
}
}
#endif
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) val = inMax;
if (val < inMin) val = inMin;
// normalize input value to [0..1]
if (inMax <= inMin)
val = 0.0;
else
val = (val - inMin) / (inMax - inMin);
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static bool update(float *var, float val)
{
if (*var != val) {
*var = val;
return true;
}
return false;
}
