rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +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
SystemAlarmsGet(&alarms);
if (alarms.Alarm[alarm] != severity) {
alarms.Alarm[alarm] = severity;
if (alarms.Alarm.data[alarm] != severity) {
alarms.Alarm.data[alarm] = severity;
SystemAlarmsSet(&alarms);
}
@ -109,10 +109,10 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (alarms.Alarm[alarm] != severity) {
alarms.ExtendedAlarmStatus[alarm] = status;
alarms.ExtendedAlarmSubStatus[alarm] = subStatus;
alarms.Alarm[alarm] = severity;
if (alarms.Alarm.data[alarm] != severity) {
alarms.ExtendedAlarmStatus.data[alarm] = status;
alarms.ExtendedAlarmSubStatus.data[alarm] = subStatus;
alarms.Alarm.data[alarm] = severity;
SystemAlarmsSet(&alarms);
}
@ -137,7 +137,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
// Read alarm
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
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (alarms.Alarm[n] >= severity) {
if (alarms.Alarm.data[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}

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

@ -392,9 +392,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
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
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 {
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) {
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) {
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.
/*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);
} 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);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);

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

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

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

@ -261,7 +261,7 @@ static void updatePathVelocity()
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) };
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 altitudeSetpoint;
@ -271,7 +271,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End[2];
altitudeSetpoint = pathDesired.End.fields.Down;
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
@ -280,7 +280,7 @@ static void updatePathVelocity()
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
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);
break;
}
@ -358,9 +358,9 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
StabilizationDesiredSet(&stabDesired);
}
@ -444,29 +444,29 @@ static uint8_t updateFixedDesiredAttitude()
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_OVERSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
fixedwingpathfollowerStatus.Errors.fields.Highspeed = 0;
fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.fields.Overspeed) {
fixedwingpathfollowerStatus.Errors.fields.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.fields.Highspeed) {
fixedwingpathfollowerStatus.Errors.fields.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.fields.Lowspeed) {
fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_STALLSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.fields.Stallspeed) {
fixedwingpathfollowerStatus.Errors.fields.Stallspeed = 1;
result = 0;
}
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
// 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;
}
@ -474,53 +474,53 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired throttle command
*/
// 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,
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
-fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki,
fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP],
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX]
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max
);
// Compute final throttle response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] +
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.fields.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.fields.Ki +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand;
fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.fields.Power = powerCommand;
// set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.fields.Min,
fixedwingpathfollowerSettings.ThrottleLimit.fields.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
fixedwingpathfollowerStatus.Errors.fields.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Max && // throttle at maximum
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
fixedwingpathfollowerSettings.Safetymargins.fields.Lowpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum
fixedwingpathfollowerStatus.Errors.fields.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Min && // throttle at minimum
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Highpower = 1;
result = 0;
}
@ -529,40 +529,40 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired pitch command
*/
if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) {
if (fixedwingpathfollowerSettings.SpeedPI.fields.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
-fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki,
fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki);
}
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.fields.Kp
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError;
fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.fields.Min,
fixedwingpathfollowerSettings.PitchLimit.fields.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.fields.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_PITCHCONTROL] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1;
result = 0;
}
@ -579,12 +579,12 @@ static uint8_t updateFixedDesiredAttitude()
headingError -= 360.0f;
}
// Error condition: wind speed is higher than airspeed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND] ||
headingError < -fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND]) &&
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Wind = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins.fields.Wind ||
headingError < -fixedwingpathfollowerSettings.Safetymargins.fields.Wind) &&
fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on
// we are flying backwards
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
fixedwingpathfollowerStatus.Errors.fields.Wind = 1;
result = 0;
}
@ -608,20 +608,20 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f;
}
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI],
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.fields.Ki,
-fixedwingpathfollowerSettings.CoursePI.fields.ILimit,
fixedwingpathfollowerSettings.CoursePI.fields.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp +
courseIntegral);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_COURSE] = courseError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_COURSE] = courseIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_COURSE] = courseCommand;
fixedwingpathfollowerStatus.Error.fields.Course = courseError;
fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.fields.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MAX]);
fixedwingpathfollowerSettings.RollLimit.fields.Min,
fixedwingpathfollowerSettings.RollLimit.fields.Max);
// 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.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
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) {
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;
} 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
@ -256,15 +256,15 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} 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
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
if (settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
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 ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((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_NODRIVER))) {
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
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
valid_input_detected &= validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax.data[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
@ -333,21 +333,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory;
// 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;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// 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;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// 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;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -389,7 +389,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory;
// 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];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
@ -406,7 +406,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
// 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];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
@ -423,7 +423,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
// 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];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
@ -671,13 +671,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
stab_settings = settings->Stabilization1Settings.data;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
stab_settings = settings->Stabilization2Settings.data;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
stab_settings = settings->Stabilization3Settings.data;
break;
default:
// 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
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.StabilizationMode.fields.Roll = stab_settings[0];
stabilization.StabilizationMode.fields.Pitch = stab_settings[1];
stabilization.StabilizationMode.fields.Yaw = stab_settings[2];
stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll:
(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_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_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
;
stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.fields.Pitch :
(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_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_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
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw :
(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_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_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
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
@ -754,12 +754,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.Start.fields.North = 0;
pathDesired.Start.fields.East = 0;
pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.fields.North = 0;
pathDesired.End.fields.East = 0;
pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -771,12 +771,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North;
pathDesired.End.fields.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -813,17 +813,17 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredGet(&pathDesired);
if (changed) {
// After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North;
pathDesired.End.fields.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down + 5;
pathDesired.End.fields.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
@ -875,7 +875,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
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;
PositionStateDownGet(&currentDown);
@ -974,7 +974,7 @@ static bool okToArm(void)
// Check each alarm
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) {
continue;
}
@ -1007,7 +1007,7 @@ static bool forcedDisArm(void)
SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
@ -1246,8 +1246,8 @@ static void applyDeadband(float *value, float deadband)
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (settings->ResponseTime[channel]) {
float rt = (float)settings->ResponseTime[channel];
if (settings->ResponseTime.data[channel]) {
float rt = (float)settings->ResponseTime.data[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*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);
PathActionInstGet(waypointData.Action, &pathActionData);
pathDesired.End[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN];
pathDesired.End.fields.North = waypointData.Position.fields.North;
pathDesired.End.fields.East = waypointData.Position.fields.East;
pathDesired.End.fields.Down = waypointData.Position.fields.Down;
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
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_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Start.fields.North = waypointPrev.Position.fields.North;
pathDesired.Start.fields.East = waypointPrev.Position.fields.East;
pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
@ -369,12 +369,12 @@ static uint8_t conditionDistanceToTarget()
PositionStateGet(&positionState);
if (pathAction.ConditionParameters[1] > 0.5f) {
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2)
+ powf(waypoint.Position[2] - positionState.Down, 2));
distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position.fields.East - positionState.East, 2)
+ powf(waypoint.Position.fields.Down - positionState.Down, 2));
} else {
distance = sqrtf(powf(waypoint.Position[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2));
distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position.fields.East - positionState.East, 2));
}
if (distance <= pathAction.ConditionParameters[0]) {
@ -400,7 +400,7 @@ static uint8_t conditionLegRemaining()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
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]) {
return true;
}
@ -423,7 +423,7 @@ static uint8_t conditionBelowError()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
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]) {
return true;
}
@ -492,7 +492,7 @@ static uint8_t conditionPointingTowardsNext()
WaypointData 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;
VelocityStateGet(&velocity);

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

@ -422,38 +422,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{
RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_X];
mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
mag_bias[0] = cal.mag_bias.fields.X;
mag_bias[1] = cal.mag_bias.fields.Y;
mag_bias[2] = cal.mag_bias.fields.Z;
mag_scale[0] = cal.mag_scale.fields.X;
mag_scale[1] = cal.mag_scale.fields.Y;
mag_scale[2] = cal.mag_scale.fields.Z;
accel_bias[0] = cal.accel_bias.fields.X;
accel_bias[1] = cal.accel_bias.fields.Y;
accel_bias[2] = cal.accel_bias.fields.Z;
accel_scale[0] = cal.accel_scale.fields.X;
accel_scale[1] = cal.accel_scale.fields.Y;
accel_scale[2] = cal.accel_scale.fields.Z;
gyro_staticbias[0] = cal.gyro_bias.fields.X;
gyro_staticbias[1] = cal.gyro_bias.fields.Y;
gyro_staticbias[2] = cal.gyro_bias.fields.Z;
gyro_scale[0] = cal.gyro_scale.fields.X;
gyro_scale[1] = cal.gyro_scale.fields.Y;
gyro_scale[2] = cal.gyro_scale.fields.Z;
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 0) {
if (attitudeSettings.BoardRotation.fields.Roll== 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 &&
attitudeSettings.BoardRotation.fields.Yaw == 0) {
rotate = 0;
} else {
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] };
const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll,
attitudeSettings.BoardRotation.fields.Pitch ,
attitudeSettings.BoardRotation.fields.Yaw};
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
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
if (reinit) {
rateRelayRunning[axis] = false;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
relay.Period.data[axis] = 200;
relay.Gain.data[axis] = 0;
accum_sin = 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 ****/
// Make sure the period can't go below limit
if (relay.Period[axis] < DEGLITCH_TIME) {
relay.Period[axis] = DEGLITCH_TIME;
if (relay.Period.data[axis] < DEGLITCH_TIME) {
relay.Period.data[axis] = DEGLITCH_TIME;
}
// Project the error onto a sine and cosine of the same frequency
// to accumulate the average amplitude
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) {
phase = 0;
}
@ -125,12 +125,12 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
if (rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
relay.Period.data[axis] = 200;
relay.Gain.data[axis] = 0;
} else {
// Low pass filter each amplitude and period
relay.Gain[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period[axis] = relay.Period[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period.data[axis] = relay.Period.data[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
}
lastHighTime = thisTime;
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:
for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode[i];
bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode.data[i];
// Apply the selected control law
switch (stabDesired.StabilizationMode[i]) {
switch (stabDesired.StabilizationMode.data[i]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// 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
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
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
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] = 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] = bound(actuatorDesiredAxis[i], 1.0f);
@ -335,7 +335,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// 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
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
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
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);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp,
settings.RollRatePID.fields.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP],
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP],
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp,
settings.RollPI.fields.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp,
pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0,
settings.PitchPI.fields.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp,
settings.YawPI.fields.Ki, 0,
settings.YawPI.fields.ILimit);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -503,9 +503,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// 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[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll== STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == 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
// 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
switch (axis) {
case ROLL:
kp = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
kp = settings->VbarRollPI.fields.Kp;
ki = settings->VbarRollPI.fields.Ki;
break;
case PITCH:
kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
kp = settings->VbarPitchPI.fields.Kp;
ki = settings->VbarPitchPI.fields.Ki;;
break;
case YAW:
kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
kp = settings->VbarYawPI.fields.Kp;
ki = settings->VbarYawPI.fields.Ki;
break;
default:
PIOS_DEBUG_Assert(0);
}
// 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);
return 0;

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

@ -163,17 +163,17 @@ static int32_t maininit(stateFilter *self)
int t;
// plausibility check
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;
}
}
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;
}
}
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;
}
}
@ -242,23 +242,23 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
INSGPSInit();
// 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];
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 }
INSSetMagVar((float[3]) { this->ekfConfiguration.R.fields.MagX / Be2,
this->ekfConfiguration.R.fields.MagY / Be2,
this->ekfConfiguration.R.fields.MagZ / Be2 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.fields.AccelX,
this->ekfConfiguration.Q.fields.AccelY,
this->ekfConfiguration.Q.fields.AccelZ }
);
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.fields.GyroX,
this->ekfConfiguration.Q.fields.GyroY,
this->ekfConfiguration.Q.fields.GyroZ }
);
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.fields.GyroDriftX,
this->ekfConfiguration.Q.fields.GyroDriftY,
this->ekfConfiguration.Q.fields.GyroDriftZ }
);
INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
INSSetBaroVar(this->ekfConfiguration.R.fields.BaroZ);
// Initialize the gyro bias
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);
INSResetP(this->ekfConfiguration.P);
INSResetP(this->ekfConfiguration.P.data);
} else {
// Run prediction a bit before any corrections
@ -368,21 +368,21 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (!this->usePos) {
// position and velocity variance used in indoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor },
(float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSVelIndoor }
);
} else {
// position and velocity variance used in outdoor mode
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.fields.GPSPosNorth,
this->ekfConfiguration.R.fields.GPSPosEast,
this->ekfConfiguration.R.fields.GPSPosDown },
(float[3]) { this->ekfConfiguration.R.fields.GPSVelNorth,
this->ekfConfiguration.R.fields.GPSVelEast,
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)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor,
this->ekfConfiguration.FakeR.fields.FakeGPSPosIndoor },
(float[3]) { this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed,
this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed,
this->ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
@ -421,12 +421,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(vardata.P);
INSGetP(vardata.P.data);
EKFStateVarianceSet(&vardata);
int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (!IS_REAL(vardata.P[t]) || vardata.P[t] <= 0.0f) {
INSResetP(this->ekfConfiguration.P);
if (!IS_REAL(vardata.P.data[t]) || vardata.P.data[t] <= 0.0f) {
INSResetP(this->ekfConfiguration.P.data);
this->init_stage = -1;
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
// of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
taskData->Running[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining[task_id] = task_info->stack_remaining;
taskData->RunningTime[task_id] = task_info->running_time_percentage;
taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining.data[task_id] = task_info->stack_remaining;
taskData->RunningTime.data[task_id] = task_info->running_time_percentage;
}
#endif

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

@ -171,111 +171,111 @@ static void updatePIDs(UAVObjEvent *ev)
// Loop through every enabled instance
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;
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
if (inst.Inputs.data[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]);
inst.ThrottleRange.fields.Min,
inst.ThrottleRange.fields.Max,
inst.MinPID.data[i], inst.MaxPID.data[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs.data[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID.data[i], inst.MaxPID.data[i]);
} else {
continue;
}
switch (inst.PIDs[i]) {
switch (inst.PIDs.data[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
needsUpdate |= update(&stab.RollRatePID.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
needsUpdate |= update(&stab.RollRatePID.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
needsUpdate |= update(&stab.RollRatePID.fields.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
needsUpdate |= update(&stab.RollPI.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
needsUpdate |= update(&stab.RollPI.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
needsUpdate |= update(&stab.RollPI.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
needsUpdate |= update(&stab.PitchPI.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
needsUpdate |= update(&stab.PitchPI.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
needsUpdate |= update(&stab.PitchPI.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
needsUpdate |= update(&stab.RollRatePID.fields.Kp, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
needsUpdate |= update(&stab.RollRatePID.fields.Ki, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
needsUpdate |= update(&stab.RollRatePID.fields.Kd, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
needsUpdate |= update(&stab.RollPI.fields.Kp, value);
needsUpdate |= update(&stab.PitchPI.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
needsUpdate |= update(&stab.RollPI.fields.Ki, value);
needsUpdate |= update(&stab.PitchPI.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
needsUpdate |= update(&stab.RollPI.fields.ILimit, value);
needsUpdate |= update(&stab.PitchPI.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
needsUpdate |= update(&stab.YawRatePID.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
needsUpdate |= update(&stab.YawRatePID.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
needsUpdate |= update(&stab.YawRatePID.fields.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.YawRatePID.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
needsUpdate |= update(&stab.YawPI.fields.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
needsUpdate |= update(&stab.YawPI.fields.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
needsUpdate |= update(&stab.YawPI.fields.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
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
if (poiRadius >= 3.0f || changeRadius > 0) {
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[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.fields.North = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.fields.East = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1.0f;
pathDesired.EndingVelocity = 0.0f;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -351,7 +351,7 @@ static void updatePOIBearing()
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
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.PitchOrServo2=elevation;
@ -382,7 +382,7 @@ static void updatePathVelocity()
{ positionState.North, positionState.East, positionState.Down };
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;
switch (pathDesired.Mode) {
@ -414,7 +414,7 @@ static void updatePathVelocity()
velocityDesired.North = progress.path_direction[0] * 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] =
{ 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.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;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki,
-vtolpathfollowerSettings.VerticalPosPI.fields.ILimit,
vtolpathfollowerSettings.VerticalPosPI.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
@ -505,17 +505,17 @@ void updateEndpointVelocity()
}
// Compute desired north command
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
northError = pathDesired.End.fields.North - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + northPosIntegral);
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
eastError = pathDesired.End.fields.East - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.fields.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.fields.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.fields.Kp + eastPosIntegral);
// Limit the maximum velocity
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
@ -527,11 +527,11 @@ void updateEndpointVelocity()
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downError = pathDesired.End.fields.Down - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.fields.Ki,
-vtolpathfollowerSettings.VerticalPosPI.fields.ILimit,
vtolpathfollowerSettings.VerticalPosPI.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.fields.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
@ -550,9 +550,9 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabDesired);
}
@ -629,31 +629,31 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.fields.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.fields.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.fields.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.fields.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired down command
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.fields.Ki,
-vtolpathfollowerSettings.VerticalVelPID.fields.ILimit,
vtolpathfollowerSettings.VerticalVelPID.fields.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.fields.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.fields.Kd);
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
@ -673,13 +673,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] * manualControlData.Yaw;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.fields.Yaw * manualControlData.Yaw;
}
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="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"/>
<access gcs="readwrite" flight="readwrite"/>