rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-1058: fix needed for fw_revolution code compilation

Browse Source
This commit is contained in:
Alessio Morale 2013-07-31 15:24:26 +02:00
parent d99790be71
commit 9e1acc3165
16 changed files with 391 additions and 391 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/libraries/alarms.c
View File

@ -74,8 +74,8 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
// Read alarm and update its severity only if it was changed // Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm[alarm] != severity) { if (alarms.Alarm.data[alarm] != severity) {
alarms.Alarm[alarm] = severity; alarms.Alarm.data[alarm] = severity;
SystemAlarmsSet(&alarms); SystemAlarmsSet(&alarms);
} }
@ -109,10 +109,10 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed // Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm[alarm] != severity) { if (alarms.Alarm.data[alarm] != severity) {
alarms.ExtendedAlarmStatus[alarm] = status; alarms.ExtendedAlarmStatus.data[alarm] = status;
alarms.ExtendedAlarmSubStatus[alarm] = subStatus; alarms.ExtendedAlarmSubStatus.data[alarm] = subStatus;
alarms.Alarm[alarm] = severity; alarms.Alarm.data[alarm] = severity;
SystemAlarmsSet(&alarms); SystemAlarmsSet(&alarms);
} }
@ -137,7 +137,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
// Read alarm // Read alarm
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
return alarms.Alarm[alarm]; return alarms.Alarm.data[alarm];
} }
/** /**
@ -229,7 +229,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
// Go through alarms and check if any are of the given severity or higher // Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (alarms.Alarm[n] >= severity) { if (alarms.Alarm.data[n] >= severity) {
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(lock);
return 1; return 1;
} }

6
flight/modules/AltitudeHold/altitudehold.c
View File

@ -392,9 +392,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// shutdown motors // shutdown motors
stabilizationDesired.Throttle = -1; stabilizationDesired.Throttle = -1;
} }
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;

8
flight/modules/Battery/battery.c
View File

@ -135,13 +135,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
// calculate the battery parameters // calculate the battery parameters
if (voltageADCPin >= 0) { if (voltageADCPin >= 0) {
flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; // in Volts flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.fields.VoltageZero) * batterySettings.SensorCalibrations.fields.VoltageFactor; // in Volts
} else { } else {
flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC flightBatteryData.Voltage = 0; // Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC
} }
if (currentADCPin >= 0) { if (currentADCPin >= 0) {
flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; // in Amps flightBatteryData.Current = (PIOS_ADC_PinGetVolt(currentADCPin) - batterySettings.SensorCalibrations.fields.CurrentZero) * batterySettings.SensorCalibrations.fields.CurrentFactor; // in Amps
if (flightBatteryData.Current > flightBatteryData.PeakCurrent) { if (flightBatteryData.Current > flightBatteryData.PeakCurrent) {
flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps flightBatteryData.PeakCurrent = flightBatteryData.Current; // in Amps
} }
@ -184,9 +184,9 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
// FIXME: should make the battery voltage detection dependent on battery type. // FIXME: should make the battery voltage detection dependent on battery type.
/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/
if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_ALARM] * batterySettings.NbCells) { if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Alarm * batterySettings.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
} else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_WARNING] * batterySettings.NbCells) { } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.fields.Warning * batterySettings.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
} else { } else {
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);

32
flight/modules/CameraStab/camerastab.c
View File

@ -171,17 +171,17 @@ static void attitudeUpdated(UAVObjEvent *ev)
// process axes // process axes
for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) {
// read and process control input // read and process control input
if (cameraStab.Input[i] != CAMERASTABSETTINGS_INPUT_NONE) { if (cameraStab.Input.data[i] != CAMERASTABSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(cameraStab.Input[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { if (AccessoryDesiredInstGet(cameraStab.Input.data[i] - CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
float input_rate; float input_rate;
switch (cameraStab.StabilizationMode[i]) { switch (cameraStab.StabilizationMode.data[i]) {
case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[i]; csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange.data[i];
break; break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal * cameraStab.InputRate[i]; input_rate = accessory.AccessoryVal * cameraStab.InputRate.data[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange[i]); csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis, cameraStab.InputRange.data[i]);
} }
break; break;
default: default:
@ -208,14 +208,14 @@ static void attitudeUpdated(UAVObjEvent *ev)
} }
#ifdef USE_GIMBAL_LPF #ifdef USE_GIMBAL_LPF
if (cameraStab.ResponseTime) { if (cameraStab.ResponseTime.data) {
float rt = (float)cameraStab.ResponseTime[i]; float rt = (float)cameraStab.ResponseTime.data[i];
attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis);
} }
#endif #endif
#ifdef USE_GIMBAL_FF #ifdef USE_GIMBAL_FF
if (cameraStab.FeedForward[i]) { if (cameraStab.FeedForward.data[i]) {
applyFeedForward(i, dT_millis, &attitude, &cameraStab); applyFeedForward(i, dT_millis, &attitude, &cameraStab);
} }
#endif #endif
@ -223,7 +223,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output // bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range // to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll // of both the unmixed pitch and unmixed roll
float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange[i], 1.0f); float output = bound((attitude + csd->inputs[i]) / cameraStab.OutputRange.data[i], 1.0f);
// set output channels // set output channels
switch (i) { switch (i) {
@ -298,16 +298,16 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
if (index == CAMERASTABSETTINGS_INPUT_ROLL) { if (index == CAMERASTABSETTINGS_INPUT_ROLL) {
float pitch; float pitch;
AttitudeStatePitchGet(&pitch); AttitudeStatePitchGet(&pitch);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch)) gimbalTypeCorrection = (cameraStab->OutputRange.fields.Pitch - fabsf(pitch))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; / cameraStab->OutputRange.fields.Pitch;
} }
break; break;
case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL: case CAMERASTABSETTINGS_GIMBALTYPE_YAWPITCHROLL:
if (index == CAMERASTABSETTINGS_INPUT_PITCH) { if (index == CAMERASTABSETTINGS_INPUT_PITCH) {
float roll; float roll;
AttitudeStateRollGet(&roll); AttitudeStateRollGet(&roll);
gimbalTypeCorrection = (cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll)) gimbalTypeCorrection = (cameraStab->OutputRange.fields.Roll - fabsf(roll))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; / cameraStab->OutputRange.fields.Roll;
} }
break; break;
default: default:
@ -316,11 +316,11 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
// apply feed forward // apply feed forward
float accumulator = csd->ffFilterAccumulator[index]; float accumulator = csd->ffFilterAccumulator[index];
accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward[index] * gimbalTypeCorrection; accumulator += (*attitude - csd->ffLastAttitude[index]) * (float)cameraStab->FeedForward.data[index] * gimbalTypeCorrection;
csd->ffLastAttitude[index] = *attitude; csd->ffLastAttitude[index] = *attitude;
*attitude += accumulator; *attitude += accumulator;
float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime[index] : cameraStab->DecelTime[index]) / dT_millis; float filter = (float)((accumulator > 0.0f) ? cameraStab->AccelTime.data[index] : cameraStab->DecelTime.data[index]) / dT_millis;
if (filter < 1.0f) { if (filter < 1.0f) {
filter = 1.0f; filter = 1.0f;
} }

150
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -261,7 +261,7 @@ static void updatePathVelocity()
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode);
float groundspeed; float groundspeed;
float altitudeSetpoint; float altitudeSetpoint;
@ -271,7 +271,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity; groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End[2]; altitudeSetpoint = pathDesired.End.fields.Down;
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT:
@ -280,7 +280,7 @@ static void updatePathVelocity()
default: default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress, 0, 1); bound(progress.fractional_progress, 0, 1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) *
bound(progress.fractional_progress, 0, 1); bound(progress.fractional_progress, 0, 1);
break; break;
} }
@ -358,9 +358,9 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1]; stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2]; stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3]; stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }
@ -444,29 +444,29 @@ static uint8_t updateFixedDesiredAttitude()
descentspeedError = descentspeedDesired - velocityState.Down; descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast // Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; fixedwingpathfollowerStatus.Errors.fields.Highspeed = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_OVERSPEED]) { if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.fields.Overspeed) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; fixedwingpathfollowerStatus.Errors.fields.Overspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHSPEED]) { if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.fields.Highspeed) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; fixedwingpathfollowerStatus.Errors.fields.Highspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWSPEED]) { if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.fields.Lowspeed) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_STALLSPEED]) { if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.fields.Stallspeed) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; fixedwingpathfollowerStatus.Errors.fields.Stallspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < 1e-6f) { if (indicatedAirspeedState < 1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops // also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1;
return 0; return 0;
} }
@ -474,53 +474,53 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired throttle command * Compute desired throttle command
*/ */
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] > 0) { if (fixedwingpathfollowerSettings.PowerPI.fields.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT, powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], -fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki,
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; } } else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation // Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound( float speedErrorToPowerCommandComponent = bound(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP], (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX], -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX] fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max
); );
// Compute final throttle response // Compute final throttle response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.fields.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] + powerIntegral * fixedwingpathfollowerSettings.PowerPI.fields.Ki +
speedErrorToPowerCommandComponent; speedErrorToPowerCommandComponent;
// Output internal state to telemetry // Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError; fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral; fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand; fixedwingpathfollowerStatus.Command.fields.Power = powerCommand;
// set throttle // set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand, stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], fixedwingpathfollowerSettings.ThrottleLimit.fields.Min,
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); fixedwingpathfollowerSettings.ThrottleLimit.fields.Max);
// Error condition: plane cannot hold altitude at current speed. // Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0; fixedwingpathfollowerStatus.Errors.fields.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Max && // throttle at maximum
velocityState.Down > 0 && // we ARE going down velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already airspeedError > 0 && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWPOWER] > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.fields.Lowpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1; fixedwingpathfollowerStatus.Errors.fields.Lowpower = 1;
result = 0; result = 0;
} }
// Error condition: plane keeps climbing despite minimum throttle (opposite of above) // Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0; fixedwingpathfollowerStatus.Errors.fields.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Min && // throttle at minimum
velocityState.Down < 0 && // we ARE going up velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1; fixedwingpathfollowerStatus.Errors.fields.Highpower = 1;
result = 0; result = 0;
} }
@ -529,40 +529,40 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired pitch command * Compute desired pitch command
*/ */
if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) { if (fixedwingpathfollowerSettings.SpeedPI.fields.Ki > 0) {
// Integrate with saturation // Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI], -fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki,
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]); fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki);
} }
// Compute the cross feed from vertical speed to pitch, with saturation // Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max
); );
// Compute the pitch command as err*Kp + errInt*Ki + X_feed. // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP] pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.fields.Kp
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki
) + verticalSpeedToPitchCommandComponent; ) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand; fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand, stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN], fixedwingpathfollowerSettings.PitchLimit.fields.Min,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); fixedwingpathfollowerSettings.PitchLimit.fields.Max);
// Error condition: high speed dive // Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0; fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.fields.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_PITCHCONTROL] > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1; fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1;
result = 0; result = 0;
} }
@ -579,12 +579,12 @@ static uint8_t updateFixedDesiredAttitude()
headingError -= 360.0f; headingError -= 360.0f;
} }
// Error condition: wind speed is higher than airspeed. We are forced backwards! // Error condition: wind speed is higher than airspeed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; fixedwingpathfollowerStatus.Errors.fields.Wind = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND] || if ((headingError > fixedwingpathfollowerSettings.Safetymargins.fields.Wind ||
headingError < -fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND]) && headingError < -fixedwingpathfollowerSettings.Safetymargins.fields.Wind) &&
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on
// we are flying backwards // we are flying backwards
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; fixedwingpathfollowerStatus.Errors.fields.Wind = 1;
result = 0; result = 0;
} }
@ -608,20 +608,20 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f; courseError -= 360.0f;
} }
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI], courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.fields.Ki,
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT], -fixedwingpathfollowerSettings.CoursePI.fields.ILimit,
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]); fixedwingpathfollowerSettings.CoursePI.fields.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp +
courseIntegral); courseIntegral);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_COURSE] = courseError; fixedwingpathfollowerStatus.Error.fields.Course = courseError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_COURSE] = courseIntegral; fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_COURSE] = courseCommand; fixedwingpathfollowerStatus.Command.fields.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral +
courseCommand, courseCommand,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN], fixedwingpathfollowerSettings.RollLimit.fields.Min,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]); fixedwingpathfollowerSettings.RollLimit.fields.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
@ -633,9 +633,9 @@ static uint8_t updateFixedDesiredAttitude()
stabDesired.Yaw = 0; stabDesired.Yaw = 0;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);

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

@ -245,10 +245,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[]; extern uint32_t pios_rcvr_group_map[];
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID; cmd.Channel[n] = PIOS_RCVR_INVALID;
} else { } else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]); cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups.data[n]], settings.ChannelNumber.data[n]);
} }
// If a channel has timed out this is not valid data and we shouldn't update anything // If a channel has timed out this is not valid data and we shouldn't update anything
@ -256,15 +256,15 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false; valid_input_detected = false;
} else { } else {
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax.data[n], settings.ChannelMin.data[n], settings.ChannelNeutral.data[n]);
} }
} }
// Check settings, if error raise alarm // Check settings, if error raise alarm
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| ||
// Check all channel mappings are valid // Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
@ -281,7 +281,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) ((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE && (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -296,14 +296,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
// decide if we have valid manual input or not // decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], valid_input_detected &= validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], && validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status // Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) { if (valid_input_detected && (++connected_count > 10)) {
@ -333,21 +333,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) { if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) { if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) { if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -389,7 +389,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
@ -406,7 +406,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
@ -423,7 +423,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
@ -671,13 +671,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings; stab_settings = settings->Stabilization1Settings.data;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings; stab_settings = settings->Stabilization2Settings.data;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings; stab_settings = settings->Stabilization3Settings.data;
break; break;
default: default:
// Major error, this should not occur because only enter this block when one of these is true // Major error, this should not occur because only enter this block when one of these is true
@ -686,44 +686,44 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
} }
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order // 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.fields.Roll = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode.fields.Pitch = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.StabilizationMode.fields.Yaw = stab_settings[2];
stabilization.Roll = stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->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_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll:
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : cmd->Roll * stabSettings.ManualRate.fields.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll:
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : cmd->Roll * stabSettings.ManualRate.fields.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
; ;
stabilization.Pitch = stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->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_RATE) ? cmd->Pitch * stabSettings.ManualRate.fields.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate.fields.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate.fields.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate.fields.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
stabilization.Yaw = stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->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_RATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : cmd->Yaw * stabSettings.ManualRate.fields.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
@ -754,12 +754,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start.fields.North = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0; pathDesired.Start.fields.East = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End.fields.North = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End.fields.East = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -771,12 +771,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; pathDesired.Start.fields.North = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; pathDesired.Start.fields.East = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; pathDesired.End.fields.North = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; pathDesired.End.fields.East = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -813,17 +813,17 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
if (changed) { if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; pathDesired.Start.fields.North = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; pathDesired.Start.fields.East = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North; pathDesired.End.fields.North = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East; pathDesired.End.fields.East = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down; pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
} }
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down + 5; pathDesired.End.fields.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
@ -875,7 +875,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.fields.Yaw;
float currentDown; float currentDown;
PositionStateDownGet(&currentDown); PositionStateDownGet(&currentDown);
@ -974,7 +974,7 @@ static bool okToArm(void)
// Check each alarm // Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (alarms.Alarm.data[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue; continue;
} }
@ -1007,7 +1007,7 @@ static bool forcedDisArm(void)
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true; return true;
} }
return false; return false;
@ -1246,8 +1246,8 @@ static void applyDeadband(float *value, float deadband)
*/ */
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{ {
if (settings->ResponseTime[channel]) { if (settings->ResponseTime.data[channel]) {
float rt = (float)settings->ResponseTime[channel]; float rt = (float)settings->ResponseTime.data[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel]; *value = inputFiltered[channel];
} }

34
flight/modules/PathPlanner/pathplanner.c
View File

@ -227,9 +227,9 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
WaypointInstGet(waypointActiveData.Index, &waypointData); WaypointInstGet(waypointActiveData.Index, &waypointData);
PathActionInstGet(waypointData.Action, &pathActionData); PathActionInstGet(waypointData.Action, &pathActionData);
pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH]; pathDesired.End.fields.North = waypointData.Position.fields.North;
pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST]; pathDesired.End.fields.East = waypointData.Position.fields.East;
pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN]; pathDesired.End.fields.Down = waypointData.Position.fields.Down;
pathDesired.EndingVelocity = waypointData.Velocity; pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode; pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0]; pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
@ -246,18 +246,18 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; /*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/ pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North; pathDesired.Start.fields.North = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East; pathDesired.Start.fields.East = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down; pathDesired.Start.fields.Down = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity; pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else { } else {
// Get previous waypoint as start point // Get previous waypoint as start point
WaypointData waypointPrev; WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev); WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start.fields.North = waypointPrev.Position.fields.North;
pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start.fields.East = waypointPrev.Position.fields.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity; pathDesired.StartingVelocity = waypointPrev.Velocity;
} }
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
@ -369,12 +369,12 @@ static uint8_t conditionDistanceToTarget()
PositionStateGet(&positionState); PositionStateGet(&positionState);
if (pathAction.ConditionParameters[1] > 0.5f) { if (pathAction.ConditionParameters[1] > 0.5f) {
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2) + powf(waypoint.Position.fields.East - positionState.East, 2)
+ powf(waypoint.Position[2] - positionState.Down, 2)); + powf(waypoint.Position.fields.Down - positionState.Down, 2));
} else { } else {
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2) distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2)); + powf(waypoint.Position.fields.East - positionState.East, 2));
} }
if (distance <= pathAction.ConditionParameters[0]) { if (distance <= pathAction.ConditionParameters[0]) {
@ -400,7 +400,7 @@ static uint8_t conditionLegRemaining()
float cur[3] = { positionState.North, positionState.East, positionState.Down }; float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode);
if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) { if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
return true; return true;
} }
@ -423,7 +423,7 @@ static uint8_t conditionBelowError()
float cur[3] = { positionState.North, positionState.East, positionState.Down }; float cur[3] = { positionState.North, positionState.East, positionState.Down };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode);
if (progress.error <= pathAction.ConditionParameters[0]) { if (progress.error <= pathAction.ConditionParameters[0]) {
return true; return true;
} }
@ -492,7 +492,7 @@ static uint8_t conditionPointingTowardsNext()
WaypointData nextWaypoint; WaypointData nextWaypoint;
WaypointInstGet(nextWaypointId, &nextWaypoint); WaypointInstGet(nextWaypointId, &nextWaypoint);
float angle1 = atan2f((nextWaypoint.Position[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1])); float angle1 = atan2f((nextWaypoint.Position.fields.North - waypoint.Position.fields.North), (nextWaypoint.Position.fields.East - waypoint.Position.fields.East));
VelocityStateData velocity; VelocityStateData velocity;
VelocityStateGet(&velocity); VelocityStateGet(&velocity);

46
flight/modules/Sensors/sensors.c
View File

@ -422,38 +422,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{ {
RevoCalibrationGet(&cal); RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X]; mag_bias[0] = cal.mag_bias.fields.X;
mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y]; mag_bias[1] = cal.mag_bias.fields.Y;
mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z]; mag_bias[2] = cal.mag_bias.fields.Z;
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X]; mag_scale[0] = cal.mag_scale.fields.X;
mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y]; mag_scale[1] = cal.mag_scale.fields.Y;
mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z]; mag_scale[2] = cal.mag_scale.fields.Z;
accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X]; accel_bias[0] = cal.accel_bias.fields.X;
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y]; accel_bias[1] = cal.accel_bias.fields.Y;
accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z]; accel_bias[2] = cal.accel_bias.fields.Z;
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X]; accel_scale[0] = cal.accel_scale.fields.X;
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y]; accel_scale[1] = cal.accel_scale.fields.Y;
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z]; accel_scale[2] = cal.accel_scale.fields.Z;
gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X]; gyro_staticbias[0] = cal.gyro_bias.fields.X;
gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y]; gyro_staticbias[1] = cal.gyro_bias.fields.Y;
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z]; gyro_staticbias[2] = cal.gyro_bias.fields.Z;
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X]; gyro_scale[0] = cal.gyro_scale.fields.X;
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y]; gyro_scale[1] = cal.gyro_scale.fields.Y;
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z]; gyro_scale[2] = cal.gyro_scale.fields.Z;
AttitudeSettingsData attitudeSettings; AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 && if (attitudeSettings.BoardRotation.fields.Roll== 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 &&
attitudeSettings.BoardRotation[2] == 0) { attitudeSettings.BoardRotation.fields.Yaw == 0) {
rotate = 0; rotate = 0;
} else { } else {
float rotationQuat[4]; float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL], const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH], attitudeSettings.BoardRotation.fields.Pitch ,
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] }; attitudeSettings.BoardRotation.fields.Yaw};
RPY2Quaternion(rpy, rotationQuat); RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
rotate = 1; rotate = 1;

18
flight/modules/Stabilization/relay_tuning.c
View File

@ -71,8 +71,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
// On first run initialize estimates to something reasonable // On first run initialize estimates to something reasonable
if (reinit) { if (reinit) {
rateRelayRunning[axis] = false; rateRelayRunning[axis] = false;
relay.Period[axis] = 200; relay.Period.data[axis] = 200;
relay.Gain[axis] = 0; relay.Gain.data[axis] = 0;
accum_sin = 0; accum_sin = 0;
accum_cos = 0; accum_cos = 0;
@ -95,14 +95,14 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
/**** The code below here is to estimate the properties of the oscillation ****/ /**** The code below here is to estimate the properties of the oscillation ****/
// Make sure the period can't go below limit // Make sure the period can't go below limit
if (relay.Period[axis] < DEGLITCH_TIME) { if (relay.Period.data[axis] < DEGLITCH_TIME) {
relay.Period[axis] = DEGLITCH_TIME; relay.Period.data[axis] = DEGLITCH_TIME;
} }
// Project the error onto a sine and cosine of the same frequency // Project the error onto a sine and cosine of the same frequency
// to accumulate the average amplitude // to accumulate the average amplitude
int32_t dT = thisTime - lastHighTime; int32_t dT = thisTime - lastHighTime;
float phase = ((float)360 * (float)dT) / relay.Period[axis]; float phase = ((float)360 * (float)dT) / relay.Period.data[axis];
if (phase >= 360) { if (phase >= 360) {
phase = 0; phase = 0;
} }
@ -125,12 +125,12 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
if (rateRelayRunning[axis] == false) { if (rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true; rateRelayRunning[axis] = true;
relay.Period[axis] = 200; relay.Period.data[axis] = 200;
relay.Gain[axis] = 0; relay.Gain.data[axis] = 0;
} else { } else {
// Low pass filter each amplitude and period // Low pass filter each amplitude and period
relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); relay.Period.data[axis] = relay.Period.data[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
} }
lastHighTime = thisTime; lastHighTime = thisTime;
high = true; high = true;

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

@ -248,18 +248,18 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Run the selected stabilization algorithm on each axis: // Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) { for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized // Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode[i]; previous_mode[i] = stabDesired.StabilizationMode.data[i];
// Apply the selected control law // Apply the selected control law
switch (stabDesired.StabilizationMode[i]) { switch (stabDesired.StabilizationMode.data[i]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if (reinit) { if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0;
} }
// Store to rate desired variable for storing to UAVO // Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]);
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
@ -275,7 +275,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop // Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]);
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
@ -326,7 +326,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
} }
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate.data[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -335,7 +335,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO // Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]); rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]);
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -350,7 +350,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop like attitude mode // Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]);
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -456,37 +456,37 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
StabilizationSettingsGet(&settings); StabilizationSettingsGet(&settings);
// Set the roll rate PID constants // Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp,
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], settings.RollRatePID.fields.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit);
// Set the pitch rate PID constants // Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit);
// Set the yaw rate PID constants // Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit);
// Set the roll attitude PI constants // Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp,
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, settings.RollPI.fields.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit);
// Set the pitch attitude PI constants // Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp,
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); settings.PitchPI.fields.ILimit);
// Set the yaw attitude PI constants // Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, settings.YawPI.fields.Ki, 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); settings.YawPI.fields.ILimit);
// Set up the derivative term // Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -503,9 +503,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll== STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
// The dT has some jitter iteration to iteration that we don't want to // The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant // make thie result unpredictable. Still, it's nicer to specify the constant

14
flight/modules/Stabilization/virtualflybar.c
View File

@ -64,23 +64,23 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Get the settings for the correct axis // Get the settings for the correct axis
switch (axis) { switch (axis) {
case ROLL: case ROLL:
kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; kp = settings->VbarRollPI.fields.Kp;
ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; ki = settings->VbarRollPI.fields.Ki;
break; break;
case PITCH: case PITCH:
kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; kp = settings->VbarPitchPI.fields.Kp;
ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; ki = settings->VbarPitchPI.fields.Ki;;
break; break;
case YAW: case YAW:
kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; kp = settings->VbarYawPI.fields.Kp;
ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; ki = settings->VbarYawPI.fields.Ki;
break; break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
// Command signal is composed of stick input added to the gyro and virtual flybar // Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * settings->VbarSensitivity[axis] - *output = command * settings->VbarSensitivity.data[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp); gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0; return 0;

76
flight/modules/StateEstimation/filterekf.c
View File

@ -163,17 +163,17 @@ static int32_t maininit(stateFilter *self)
int t; int t;
// plausibility check // plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.P[t])) { if (invalid_var(this->ekfConfiguration.P.data[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.Q[t])) { if (invalid_var(this->ekfConfiguration.Q.data[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(this->ekfConfiguration.R[t])) { if (invalid_var(this->ekfConfiguration.R.data[t])) {
return 2; return 2;
} }
} }
@ -242,23 +242,23 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSGPSInit(); INSGPSInit();
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2 // variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2
float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2]; float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2];
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / Be2, INSSetMagVar((float[3]) { this->ekfConfiguration.R.fields.MagX / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2, this->ekfConfiguration.R.fields.MagY / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 } this->ekfConfiguration.R.fields.MagZ / Be2 }
); );
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX], INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.fields.AccelX,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY], this->ekfConfiguration.Q.fields.AccelY,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] } this->ekfConfiguration.Q.fields.AccelZ }
); );
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX], INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.fields.GyroX,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY], this->ekfConfiguration.Q.fields.GyroY,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] } this->ekfConfiguration.Q.fields.GyroZ }
); );
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX], INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.fields.GyroDriftX,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY], this->ekfConfiguration.Q.fields.GyroDriftY,
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] } this->ekfConfiguration.Q.fields.GyroDriftZ }
); );
INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]); INSSetBaroVar(this->ekfConfiguration.R.fields.BaroZ);
// Initialize the gyro bias // Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
@ -294,7 +294,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros); INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(this->ekfConfiguration.P); INSResetP(this->ekfConfiguration.P.data);
} else { } else {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
@ -368,21 +368,21 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (!this->usePos) { if (!this->usePos) {
// position and velocity variance used in indoor mode // position and velocity variance used in indoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR], this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] } this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor }
); );
} else { } else {
// position and velocity variance used in outdoor mode // position and velocity variance used in outdoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.fields.GPSPosNorth,
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST], this->ekfConfiguration.R.fields.GPSPosEast,
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] }, this->ekfConfiguration.R.fields.GPSPosDown },
(float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH], (float[3]) { this->ekfConfiguration.R.fields.GPSVelNorth,
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST], this->ekfConfiguration.R.fields.GPSVelEast,
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] } this->ekfConfiguration.R.fields.GPSVelDown }
); );
} }
@ -397,12 +397,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) { if (IS_SET(this->work.updated, SENSORUPDATES_airspeed) && ((!IS_SET(this->work.updated, SENSORUPDATES_vel) && !IS_SET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance // HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS; sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR], this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] }, this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], (float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED], this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed,
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] } this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed }
); );
// rotate airspeed vector into NED frame - airspeed is measured in X axis only // rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3]; float R[3][3];
@ -421,12 +421,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
EKFStateVarianceData vardata; EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata); EKFStateVarianceGet(&vardata);
INSGetP(vardata.P); INSGetP(vardata.P.data);
EKFStateVarianceSet(&vardata); EKFStateVarianceSet(&vardata);
int t; int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (!IS_REAL(vardata.P[t]) || vardata.P[t] <= 0.0f) { if (!IS_REAL(vardata.P.data[t]) || vardata.P.data[t] <= 0.0f) {
INSResetP(this->ekfConfiguration.P); INSResetP(this->ekfConfiguration.P.data);
this->init_stage = -1; this->init_stage = -1;
break; break;
} }

6
flight/modules/System/systemmod.c
View File

@ -426,9 +426,9 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
// By convention, there is a direct mapping between task monitor task_id's and members // By convention, there is a direct mapping between task monitor task_id's and members
// of the TaskInfoXXXXElem enums // of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
taskData->Running[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining[task_id] = task_info->stack_remaining; taskData->StackRemaining.data[task_id] = task_info->stack_remaining;
taskData->RunningTime[task_id] = task_info->running_time_percentage; taskData->RunningTime.data[task_id] = task_info->running_time_percentage;
} }
#endif #endif

86
flight/modules/TxPID/txpid.c
View File

@ -171,111 +171,111 @@ static void updatePIDs(UAVObjEvent *ev)
// Loop through every enabled instance // Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { if (inst.PIDs.data[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value; float value;
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { if (inst.Inputs.data[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value); ManualControlCommandThrottleGet(&value);
value = scale(value, value = scale(value,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], inst.ThrottleRange.fields.Min,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], inst.ThrottleRange.fields.Max,
inst.MinPID[i], inst.MaxPID[i]); inst.MinPID.data[i], inst.MaxPID.data[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { } else if (AccessoryDesiredInstGet(inst.Inputs.data[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]); value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID.data[i], inst.MaxPID.data[i]);
} else { } else {
continue; continue;
} }
switch (inst.PIDs[i]) { switch (inst.PIDs.data[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP: case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); needsUpdate |= update(&stab.RollRatePID.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI: case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); needsUpdate |= update(&stab.RollRatePID.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD: case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); needsUpdate |= update(&stab.RollRatePID.fields.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); needsUpdate |= update(&stab.RollPI.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); needsUpdate |= update(&stab.RollPI.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); needsUpdate |= update(&stab.RollPI.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP: case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI: case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD: case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); needsUpdate |= update(&stab.PitchPI.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); needsUpdate |= update(&stab.PitchPI.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); needsUpdate |= update(&stab.PitchPI.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); needsUpdate |= update(&stab.RollRatePID.fields.Kp, value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); needsUpdate |= update(&stab.RollRatePID.fields.Ki, value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); needsUpdate |= update(&stab.RollRatePID.fields.Kd, value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); needsUpdate |= update(&stab.RollPI.fields.Kp, value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); needsUpdate |= update(&stab.PitchPI.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); needsUpdate |= update(&stab.RollPI.fields.Ki, value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); needsUpdate |= update(&stab.PitchPI.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); needsUpdate |= update(&stab.RollPI.fields.ILimit, value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); needsUpdate |= update(&stab.PitchPI.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKP: case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); needsUpdate |= update(&stab.YawRatePID.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKI: case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); needsUpdate |= update(&stab.YawRatePID.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKD: case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); needsUpdate |= update(&stab.YawRatePID.fields.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); needsUpdate |= update(&stab.YawRatePID.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); needsUpdate |= update(&stab.YawPI.fields.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); needsUpdate |= update(&stab.YawPI.fields.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); needsUpdate |= update(&stab.YawPI.fields.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_GYROTAU: case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdate |= update(&stab.GyroTau, value); needsUpdate |= update(&stab.GyroTau, value);

96
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -337,8 +337,8 @@ static void updatePOIBearing()
// don't try to move any closer // don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) { if (poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) { if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.End.fields.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f))); pathDesired.End.fields.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f; pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -351,7 +351,7 @@ static void updatePOIBearing()
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/ /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
stabDesired.Yaw = yaw + (pathAngle / 2.0f); stabDesired.Yaw = yaw + (pathAngle / 2.0f);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
// cameraDesired.Yaw=yaw; // cameraDesired.Yaw=yaw;
// cameraDesired.PitchOrServo2=elevation; // cameraDesired.PitchOrServo2=elevation;
@ -382,7 +382,7 @@ static void updatePathVelocity()
{ positionState.North, positionState.East, positionState.Down }; { positionState.North, positionState.East, positionState.Down };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode); path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode);
float groundspeed; float groundspeed;
switch (pathDesired.Mode) { switch (pathDesired.Mode) {
@ -414,7 +414,7 @@ static void updatePathVelocity()
velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp;
float correction_velocity[2] = float correction_velocity[2] =
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed }; { progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
@ -427,13 +427,13 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1); float altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * bound(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionState.Down; float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki,
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit,
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); vtolpathfollowerSettings.VerticalPosPI.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus // update pathstatus
@ -505,17 +505,17 @@ void updateEndpointVelocity()
} }
// Compute desired north command // Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northError = pathDesired.End.fields.North - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki,
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit,
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; eastError = pathDesired.End.fields.East - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki,
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], -vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit,
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + eastPosIntegral);
// Limit the maximum velocity // Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2)); float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
@ -527,11 +527,11 @@ void updateEndpointVelocity()
velocityDesired.North = northCommand * scale; velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale; velocityDesired.East = eastCommand * scale;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; downError = pathDesired.End.fields.Down - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI], downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki,
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT], -vtolpathfollowerSettings.VerticalPosPI.fields.ILimit,
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]); vtolpathfollowerSettings.VerticalPosPI.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral); downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax); velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired); VelocityDesiredSet(&velocityDesired);
@ -550,9 +550,9 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1]; stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2]; stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3]; stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }
@ -629,31 +629,31 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Compute desired north command // Compute desired north command
northError = velocityDesired.North - northVel; northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki,
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit,
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward); + velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command // Compute desired east command
eastError = velocityDesired.East - eastVel; eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki,
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], -vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit,
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] - nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward); + velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command // Compute desired down command
downError = velocityDesired.Down - downVel; downError = velocityDesired.Down - downVel;
// Must flip this sign // Must flip this sign
downError = -downError; downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI], downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.fields.Ki,
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT], -vtolpathfollowerSettings.VerticalVelPID.fields.ILimit,
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]); vtolpathfollowerSettings.VerticalVelPID.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.fields.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]); - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.fields.Kd);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
@ -673,13 +673,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
stabDesired.Throttle = manualControl.Throttle; stabDesired.Throttle = manualControl.Throttle;
} }
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) { if (yaw_attitude) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else { } else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw; stabDesired.Yaw = stabSettings.MaximumRate.fields.Yaw * manualControlData.Yaw;
} }
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -23,7 +23,7 @@
<field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/> <field name="USB_VCPPort" units="function" type="enum" elements="1" options="USBTelemetry,ComBridge,DebugConsole,Disabled" defaultvalue="Disabled"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/> <field name="OptionalModules" units="" type="enum" elementnames="CameraStab,GPS,ComUsbBridge,Fault,Altitude,Airspeed,TxPID,Autotune,VtolPathFollower,FixedWingPathFollower,Battery,Overo,MagBaro,OsdHk" options="Disabled,Enabled" defaultvalue="Disabled"/>
<field name="ADCRouting" units="" type="enum" elementnames="ADC0,ADC1,ADC2,ADC3" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/> <field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/> <field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>