rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-1058 Get rid of Unions.

Browse Source 
They caused stack usage increase with -fstrict-aliasing as stack slots are not reused when dealing with unions.
It has now been added the cast_struct_to_array macro in pios_struct_helper.h to handle such case where it is useful to access those homogeneous structs as arrays

+review OPReview-552
This commit is contained in:
Alessio Morale 2013-09-01 12:10:55 +02:00
parent 11b269f898
commit e91bc28667
21 changed files with 553 additions and 509 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
flight/libraries/alarms.c
View File

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

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

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

28
flight/modules/Attitude/attitude.c
View File

@ -609,17 +609,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE; zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE; bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias.fields.X; accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.fields.Y; accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.fields.Z; accelbias[2] = attitudeSettings.AccelBias.Z;
gyro_correct_int[0] = attitudeSettings.GyroBias.fields.X / 100.0f; gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.fields.Y / 100.0f; gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.fields.Z / 100.0f; gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Roll == 0 && if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
attitudeSettings.BoardRotation.fields.Yaw == 0) { attitudeSettings.BoardRotation.Yaw == 0) {
rotate = 0; rotate = 0;
// Shouldn't be used but to be safe // Shouldn't be used but to be safe
@ -627,9 +627,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
} else { } else {
float rotationQuat[4]; float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.fields.Pitch, attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.fields.Yaw }; attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat); RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
rotate = 1; rotate = 1;
@ -643,10 +643,10 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true; trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) { } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false; trim_requested = false;
attitudeSettings.AccelBias.fields.X = trim_accels[0] / trim_samples; attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.fields.Y = trim_accels[1] / trim_samples; attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
// Z should average -grav // Z should average -grav
attitudeSettings.AccelBias.fields.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE; attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings); AttitudeSettingsSet(&attitudeSettings);
} else { } else {

86
flight/modules/Attitude/revolution/attitude.c
View File

@ -49,7 +49,7 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include "attitude.h" #include "attitude.h"
#include "accelsensor.h" #include "accelsensor.h"
#include "accelstate.h" #include "accelstate.h"
@ -834,12 +834,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
value_error = true; value_error = true;
} }
if (invalid_var(ekfConfiguration.R.fields.GPSPosNorth) || if (invalid_var(ekfConfiguration.R.GPSPosNorth) ||
invalid_var(ekfConfiguration.R.fields.GPSPosEast) || invalid_var(ekfConfiguration.R.GPSPosEast) ||
invalid_var(ekfConfiguration.R.fields.GPSPosDown) || invalid_var(ekfConfiguration.R.GPSPosDown) ||
invalid_var(ekfConfiguration.R.fields.GPSVelNorth) || invalid_var(ekfConfiguration.R.GPSVelNorth) ||
invalid_var(ekfConfiguration.R.fields.GPSVelEast) || invalid_var(ekfConfiguration.R.GPSVelEast) ||
invalid_var(ekfConfiguration.R.fields.GPSVelDown)) { invalid_var(ekfConfiguration.R.GPSVelDown)) {
gps_updated = false; gps_updated = false;
value_error = true; value_error = true;
} }
@ -892,23 +892,23 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (init_stage == 0) { if (init_stage == 0) {
// Reset the INS algorithm // Reset the INS algorithm
INSGPSInit(); INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R.fields.MagX, INSSetMagVar((float[3]) { ekfConfiguration.R.MagX,
ekfConfiguration.R.fields.MagY, ekfConfiguration.R.MagY,
ekfConfiguration.R.fields.MagZ } ekfConfiguration.R.MagZ }
); );
INSSetAccelVar((float[3]) { ekfConfiguration.Q.fields.AccelX, INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX,
ekfConfiguration.Q.fields.AccelY, ekfConfiguration.Q.AccelY,
ekfConfiguration.Q.fields.AccelZ } ekfConfiguration.Q.AccelZ }
); );
INSSetGyroVar((float[3]) { ekfConfiguration.Q.fields.GyroX, INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX,
ekfConfiguration.Q.fields.GyroY, ekfConfiguration.Q.GyroY,
ekfConfiguration.Q.fields.GyroZ } ekfConfiguration.Q.GyroZ }
); );
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.fields.GyroDriftX, INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX,
ekfConfiguration.Q.fields.GyroDriftY, ekfConfiguration.Q.GyroDriftY,
ekfConfiguration.Q.fields.GyroDriftZ } ekfConfiguration.Q.GyroDriftZ }
); );
INSSetBaroVar(ekfConfiguration.R.fields.BaroZ); INSSetBaroVar(ekfConfiguration.R.BaroZ);
// Initialize the gyro bias // Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
@ -966,7 +966,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 };
INSSetState(pos, zeros, q, zeros, zeros); INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(ekfConfiguration.P.data); INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1));
} else { } else {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
@ -1028,12 +1028,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagNorth(homeLocation.Be); INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode) { if (gps_updated && outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.R.fields.GPSPosNorth, INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth,
ekfConfiguration.R.fields.GPSPosEast, ekfConfiguration.R.GPSPosEast,
ekfConfiguration.R.fields.GPSPosDown }, ekfConfiguration.R.GPSPosDown },
(float[3]) { ekfConfiguration.R.fields.GPSVelNorth, (float[3]) { ekfConfiguration.R.GPSVelNorth,
ekfConfiguration.R.fields.GPSVelEast, ekfConfiguration.R.GPSVelEast,
ekfConfiguration.R.fields.GPSVelDown } ekfConfiguration.R.GPSVelDown }
); );
sensors |= POS_SENSORS; sensors |= POS_SENSORS;
@ -1051,12 +1051,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
(1.0f - BARO_OFFSET_LOWPASS_ALPHA) (1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-NED[2] - baroData.Altitude); * (-NED[2] - baroData.Altitude);
} else if (!outdoor_mode) { } else if (!outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, (float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.fields.FakeGPSVelIndoor, ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.fields.FakeGPSVelIndoor } ekfConfiguration.FakeR.FakeGPSVelIndoor }
); );
vel[0] = vel[1] = vel[2] = 0.0f; vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f; NED[0] = NED[1] = 0.0f;
@ -1084,12 +1084,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (!gps_vel_updated && !gps_updated) { if (!gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance // feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS; sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.fields.FakeGPSPosIndoor, ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.fields.FakeGPSPosIndoor }, ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, (float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed, ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.fields.FakeGPSVelAirspeed } ekfConfiguration.FakeR.FakeGPSVelAirspeed }
); );
// rotate airspeed vector into NED frame - airspeed is measured in X axis only // rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3]; float R[3][3];
@ -1130,7 +1130,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
EKFStateVarianceData vardata; EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata); EKFStateVarianceGet(&vardata);
INSGetP(vardata.P.data); INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1));
EKFStateVarianceSet(&vardata); EKFStateVarianceSet(&vardata);
return 0; return 0;
@ -1180,17 +1180,17 @@ static void settingsUpdatedCb(UAVObjEvent *ev)
EKFConfigurationGet(&ekfConfiguration); EKFConfigurationGet(&ekfConfiguration);
int t; int t;
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.P.data[t])) { if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) {
error = true; error = true;
} }
} }
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q.data[t])) { if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) {
error = true; error = true;
} }
} }
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R.data[t])) { if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) {
error = true; error = true;
} }
} }

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

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

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

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

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

@ -62,6 +62,7 @@
#include "velocitydesired.h" #include "velocitydesired.h"
#include "velocitystate.h" #include "velocitystate.h"
#include "taskinfo.h" #include "taskinfo.h"
#include <pios_struct_helper.h>
#include "paths.h" #include "paths.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
@ -261,7 +262,9 @@ static void updatePathVelocity()
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) }; positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) };
struct path_status progress; struct path_status progress;
path_progress(pathDesired.Start.data, pathDesired.End.data, cur, &progress, pathDesired.Mode); path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
float groundspeed; float groundspeed;
float altitudeSetpoint; float altitudeSetpoint;
@ -271,7 +274,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity; groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End.fields.Down; altitudeSetpoint = pathDesired.End.Down;
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT: case PATHDESIRED_MODE_DRIVEENDPOINT:
@ -280,7 +283,7 @@ static void updatePathVelocity()
default: default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress, 0, 1); bound(progress.fractional_progress, 0, 1);
altitudeSetpoint = pathDesired.Start.fields.Down + (pathDesired.End.fields.Down - pathDesired.Start.fields.Down) * altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
bound(progress.fractional_progress, 0, 1); bound(progress.fractional_progress, 0, 1);
break; break;
} }
@ -358,9 +361,9 @@ static void updateFixedAttitude(float *attitude)
stabDesired.Pitch = attitude[1]; stabDesired.Pitch = attitude[1];
stabDesired.Yaw = attitude[2]; stabDesired.Yaw = attitude[2];
stabDesired.Throttle = attitude[3]; stabDesired.Throttle = attitude[3];
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);
} }
@ -444,29 +447,29 @@ static uint8_t updateFixedDesiredAttitude()
descentspeedError = descentspeedDesired - velocityState.Down; descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast // Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors.fields.Highspeed = 0; fixedwingpathfollowerStatus.Errors.Highspeed = 0;
fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 0; fixedwingpathfollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.fields.Overspeed) { if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins.Overspeed) {
fixedwingpathfollowerStatus.Errors.fields.Overspeed = 1; fixedwingpathfollowerStatus.Errors.Overspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.fields.Highspeed) { if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) {
fixedwingpathfollowerStatus.Errors.fields.Highspeed = 1; fixedwingpathfollowerStatus.Errors.Highspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.fields.Lowspeed) { if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) {
fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.fields.Stallspeed) { if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.Stallspeed) {
fixedwingpathfollowerStatus.Errors.fields.Stallspeed = 1; fixedwingpathfollowerStatus.Errors.Stallspeed = 1;
result = 0; result = 0;
} }
if (indicatedAirspeedState < 1e-6f) { if (indicatedAirspeedState < 1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops // also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors.fields.Lowspeed = 1; fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
return 0; return 0;
} }
@ -474,53 +477,53 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired throttle command * Compute desired throttle command
*/ */
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.fields.Ki > 0) { if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT, powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki, -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.fields.ILimit / fixedwingpathfollowerSettings.PowerPI.fields.Ki fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; } } else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation // Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound( float speedErrorToPowerCommandComponent = bound(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Kp, (airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max, -fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.fields.Max fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
); );
// Compute final throttle response // Compute final throttle response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.fields.Kp + powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.fields.Ki + powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent; speedErrorToPowerCommandComponent;
// Output internal state to telemetry // Output internal state to telemetry
fixedwingpathfollowerStatus.Error.fields.Power = descentspeedError; fixedwingpathfollowerStatus.Error.Power = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt.fields.Power = powerIntegral; fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.fields.Power = powerCommand; fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set throttle // set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.fields.Neutral + powerCommand, stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.fields.Min, fixedwingpathfollowerSettings.ThrottleLimit.Min,
fixedwingpathfollowerSettings.ThrottleLimit.fields.Max); fixedwingpathfollowerSettings.ThrottleLimit.Max);
// Error condition: plane cannot hold altitude at current speed. // Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.fields.Lowpower = 0; fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Max && // throttle at maximum if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum
velocityState.Down > 0 && // we ARE going down velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already airspeedError > 0 && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins.fields.Lowpower > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Lowpower = 1; fixedwingpathfollowerStatus.Errors.Lowpower = 1;
result = 0; result = 0;
} }
// Error condition: plane keeps climbing despite minimum throttle (opposite of above) // Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors.fields.Highpower = 0; fixedwingpathfollowerStatus.Errors.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.fields.Min && // throttle at minimum if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum
velocityState.Down < 0 && // we ARE going up velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Highpower = 1; fixedwingpathfollowerStatus.Errors.Highpower = 1;
result = 0; result = 0;
} }
@ -529,40 +532,40 @@ static uint8_t updateFixedDesiredAttitude()
* Compute desired pitch command * Compute desired pitch command
*/ */
if (fixedwingpathfollowerSettings.SpeedPI.fields.Ki > 0) { if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation // Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT, airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki, -fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.fields.ILimit / fixedwingpathfollowerSettings.SpeedPI.fields.Ki); fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
} }
// Compute the cross feed from vertical speed to pitch, with saturation // Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Kp, float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max, -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.fields.Max fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
); );
// Compute the pitch command as err*Kp + errInt*Ki + X_feed. // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.fields.Kp pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.fields.Ki + airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki
) + verticalSpeedToPitchCommandComponent; ) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error.fields.Speed = airspeedError; fixedwingpathfollowerStatus.Error.Speed = airspeedError;
fixedwingpathfollowerStatus.ErrorInt.fields.Speed = airspeedErrorInt; fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.fields.Speed = pitchCommand; fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.fields.Neutral + pitchCommand, stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.fields.Min, fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.fields.Max); fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive // Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 0; fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.fields.Max && // pitch demand is full up if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins.fields.Pitchcontrol > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.fields.Pitchcontrol = 1; fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
result = 0; result = 0;
} }
@ -579,12 +582,12 @@ static uint8_t updateFixedDesiredAttitude()
headingError -= 360.0f; headingError -= 360.0f;
} }
// Error condition: wind speed is higher than airspeed. We are forced backwards! // Error condition: wind speed is higher than airspeed. We are forced backwards!
fixedwingpathfollowerStatus.Errors.fields.Wind = 0; fixedwingpathfollowerStatus.Errors.Wind = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins.fields.Wind || if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind ||
headingError < -fixedwingpathfollowerSettings.Safetymargins.fields.Wind) && headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) &&
fixedwingpathfollowerSettings.Safetymargins.fields.Highpower > 0.5f) { // alarm switched on fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
// we are flying backwards // we are flying backwards
fixedwingpathfollowerStatus.Errors.fields.Wind = 1; fixedwingpathfollowerStatus.Errors.Wind = 1;
result = 0; result = 0;
} }
@ -608,20 +611,20 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f; courseError -= 360.0f;
} }
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.fields.Ki, courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.fields.ILimit, -fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.fields.ILimit); fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.fields.Kp + courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral); courseIntegral);
fixedwingpathfollowerStatus.Error.fields.Course = courseError; fixedwingpathfollowerStatus.Error.Course = courseError;
fixedwingpathfollowerStatus.ErrorInt.fields.Course = courseIntegral; fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.fields.Course = courseCommand; fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.fields.Neutral + stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand, courseCommand,
fixedwingpathfollowerSettings.RollLimit.fields.Min, fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.fields.Max); fixedwingpathfollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
@ -633,9 +636,9 @@ static uint8_t updateFixedDesiredAttitude()
stabDesired.Yaw = 0; stabDesired.Yaw = 0;
stabDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired); StabilizationDesiredSet(&stabDesired);

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

@ -34,7 +34,7 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include "accessorydesired.h" #include "accessorydesired.h"
#include "actuatordesired.h" #include "actuatordesired.h"
#include "altitudeholddesired.h" #include "altitudeholddesired.h"
@ -245,10 +245,12 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[]; extern uint32_t pios_rcvr_group_map[];
if (settings.ChannelGroups.data[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID; cmd.Channel[n] = PIOS_RCVR_INVALID;
} else { } else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups.data[n]], settings.ChannelNumber.data[n]); cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]);
} }
// If a channel has timed out this is not valid data and we shouldn't update anything // If a channel has timed out this is not valid data and we shouldn't update anything
@ -256,15 +258,18 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false; valid_input_detected = false;
} else { } else {
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax.data[n], settings.ChannelMin.data[n], settings.ChannelNeutral.data[n]); scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]);
} }
} }
// Check settings, if error raise alarm // Check settings, if error raise alarm
if (settings.ChannelGroups.fields.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| ||
// Check all channel mappings are valid // Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
@ -281,7 +286,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) ((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups.fields.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
@ -296,14 +301,14 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
// decide if we have valid manual input or not // decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin.fields.Throttle, valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.fields.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.fields.Roll, && validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.fields.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.fields.Yaw, && validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.fields.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.fields.Pitch, && validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.fields.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status // Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) { if (valid_input_detected && (++connected_count > 10)) {
@ -333,21 +338,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) { if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) { if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
} }
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) { if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -389,7 +394,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
@ -406,7 +411,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
@ -423,7 +428,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
} }
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
@ -453,8 +458,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (pios_usb_rctx_id) { if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id, PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel, cmd.Channel,
settings.ChannelMin.data, cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
settings.ChannelMax.data, cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel)); NELEMENTS(cmd.Channel));
} }
#endif /* PIOS_INCLUDE_USB_RCTX */ #endif /* PIOS_INCLUDE_USB_RCTX */
@ -671,13 +676,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings.data; stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings.data; stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings.data; stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll);
break; break;
default: default:
// Major error, this should not occur because only enter this block when one of these is true // Major error, this should not occur because only enter this block when one of these is true
@ -686,44 +691,44 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
} }
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode.fields.Roll = stab_settings[0]; stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.fields.Pitch = stab_settings[1]; stabilization.StabilizationMode.Pitch = stab_settings[1];
stabilization.StabilizationMode.fields.Yaw = stab_settings[2]; stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Roll = stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate.fields.Roll : cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.fields.Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Roll * stabSettings.ManualRate.fields.Roll : cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
; ;
stabilization.Pitch = stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.fields.Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate.fields.Pitch : cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
cmd->Pitch * stabSettings.ManualRate.fields.Pitch : cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate.fields.Pitch : cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
stabilization.Yaw = stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate.fields.Yaw : cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.fields.Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
@ -754,14 +759,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start.fields.North = 0; pathDesired.Start.North = 0;
pathDesired.Start.fields.East = 0; pathDesired.Start.East = 0;
pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.fields.North = 0; pathDesired.End.North = 0;
pathDesired.End.fields.East = 0; pathDesired.End.East = 0;
pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} else if (changed) { } else if (changed) {
@ -771,14 +776,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start.fields.North = positionState.North; pathDesired.Start.North = positionState.North;
pathDesired.Start.fields.East = positionState.East; pathDesired.Start.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down; pathDesired.Start.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North; pathDesired.End.North = positionState.North;
pathDesired.End.fields.East = positionState.East; pathDesired.End.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down; pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
@ -813,17 +818,17 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
if (changed) { if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
pathDesired.Start.fields.North = positionState.North; pathDesired.Start.North = positionState.North;
pathDesired.Start.fields.East = positionState.East; pathDesired.Start.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down; pathDesired.Start.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North; pathDesired.End.North = positionState.North;
pathDesired.End.fields.East = positionState.East; pathDesired.End.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down; pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
} }
pathDesired.End.fields.Down = positionState.Down + 5; pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
@ -875,7 +880,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.fields.Yaw; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
float currentDown; float currentDown;
PositionStateDownGet(&currentDown); PositionStateDownGet(&currentDown);
@ -974,7 +979,7 @@ static bool okToArm(void)
// Check each alarm // Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm.data[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue; continue;
} }
@ -1007,7 +1012,7 @@ static bool forcedDisArm(void)
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true; return true;
} }
return false; return false;
@ -1246,8 +1251,8 @@ static void applyDeadband(float *value, float deadband)
*/ */
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{ {
if (settings->ResponseTime.data[channel]) { if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)settings->ResponseTime.data[channel]; float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel]; *value = inputFiltered[channel];
} }

22
flight/modules/Osd/osdgen/osdgen.c
View File

@ -2219,8 +2219,8 @@ void updateGraphics()
/* Draw Attitude Indicator */ /* Draw Attitude Indicator */
if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) { if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.fields.X), drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.X),
APPLY_VDEADBAND(OsdSettings.AttitudeSetup.fields.Y), attitude.Pitch, attitude.Roll, 96); APPLY_VDEADBAND(OsdSettings.AttitudeSetup.Y), attitude.Pitch, attitude.Roll, 96);
} }
// write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0); // write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
// printText16( 60, 12,"Hello OP-OSD"); // printText16( 60, 12,"Hello OP-OSD");
@ -2239,7 +2239,7 @@ void updateGraphics()
/* Print RTC time */ /* Print RTC time */
if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) { if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.fields.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.fields.Y)); printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup.X), APPLY_VDEADBAND(OsdSettings.TimeSetup.Y));
} }
/* Print Number of detected video Lines */ /* Print Number of detected video Lines */
@ -2292,22 +2292,22 @@ void updateGraphics()
// drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32); // drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.) // Draw airspeed (left side.)
if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) { if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.fields.X), hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.X),
APPLY_VDEADBAND(OsdSettings.SpeedSetup.fields.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE); APPLY_VDEADBAND(OsdSettings.SpeedSetup.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
} }
// Draw altimeter (right side.) // Draw altimeter (right side.)
if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) { if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.fields.X), hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.X),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup.fields.Y), 100, 20, 100, 7, 12, 15, 500, 0); APPLY_VDEADBAND(OsdSettings.AltitudeSetup.Y), 100, 20, 100, 7, 12, 15, 500, 0);
} }
// Draw compass. // Draw compass.
if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) { if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
if (attitude.Yaw < 0) { if (attitude.Yaw < 0) {
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0);
} else { } else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X), hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0); APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0);
} }
} }
} }

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

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

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

@ -422,38 +422,38 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
{ {
RevoCalibrationGet(&cal); RevoCalibrationGet(&cal);
mag_bias[0] = cal.mag_bias.fields.X; mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.fields.Y; mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.fields.Z; mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.fields.X; mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.fields.Y; mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.fields.Z; mag_scale[2] = cal.mag_scale.Z;
accel_bias[0] = cal.accel_bias.fields.X; accel_bias[0] = cal.accel_bias.X;
accel_bias[1] = cal.accel_bias.fields.Y; accel_bias[1] = cal.accel_bias.Y;
accel_bias[2] = cal.accel_bias.fields.Z; accel_bias[2] = cal.accel_bias.Z;
accel_scale[0] = cal.accel_scale.fields.X; accel_scale[0] = cal.accel_scale.X;
accel_scale[1] = cal.accel_scale.fields.Y; accel_scale[1] = cal.accel_scale.Y;
accel_scale[2] = cal.accel_scale.fields.Z; accel_scale[2] = cal.accel_scale.Z;
gyro_staticbias[0] = cal.gyro_bias.fields.X; gyro_staticbias[0] = cal.gyro_bias.X;
gyro_staticbias[1] = cal.gyro_bias.fields.Y; gyro_staticbias[1] = cal.gyro_bias.Y;
gyro_staticbias[2] = cal.gyro_bias.fields.Z; gyro_staticbias[2] = cal.gyro_bias.Z;
gyro_scale[0] = cal.gyro_scale.fields.X; gyro_scale[0] = cal.gyro_scale.X;
gyro_scale[1] = cal.gyro_scale.fields.Y; gyro_scale[1] = cal.gyro_scale.Y;
gyro_scale[2] = cal.gyro_scale.fields.Z; gyro_scale[2] = cal.gyro_scale.Z;
AttitudeSettingsData attitudeSettings; AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings); AttitudeSettingsGet(&attitudeSettings);
// Indicates not to expend cycles on rotation // Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.fields.Roll == 0 && attitudeSettings.BoardRotation.fields.Pitch == 0 && if (attitudeSettings.BoardRotation.Roll == 0 && attitudeSettings.BoardRotation.Pitch == 0 &&
attitudeSettings.BoardRotation.fields.Yaw == 0) { attitudeSettings.BoardRotation.Yaw == 0) {
rotate = 0; rotate = 0;
} else { } else {
float rotationQuat[4]; float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll, const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.fields.Pitch, attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.fields.Yaw }; attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat); RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
rotate = 1; rotate = 1;

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

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

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

@ -32,7 +32,7 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include "stabilization.h" #include "stabilization.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "actuatordesired.h" #include "actuatordesired.h"
@ -201,11 +201,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
} else { } else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2 // scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed); speedScaleFactor = (settings.ScaleToAirspeed * settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed);
if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) { if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]; speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
} }
if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) { if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]; speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
} }
} }
#else #else
@ -220,19 +220,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
float local_error[3]; float local_error[3];
// Essentially zero errors for anything in rate or none // Essentially zero errors for anything in rate or none
if (stabDesired.StabilizationMode.fields.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { if (stabDesired.StabilizationMode.Roll == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[0] = stabDesired.Roll; rpy_desired[0] = stabDesired.Roll;
} else { } else {
rpy_desired[0] = attitudeState.Roll; rpy_desired[0] = attitudeState.Roll;
} }
if (stabDesired.StabilizationMode.fields.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { if (stabDesired.StabilizationMode.Pitch == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[1] = stabDesired.Pitch; rpy_desired[1] = stabDesired.Pitch;
} else { } else {
rpy_desired[1] = attitudeState.Pitch; rpy_desired[1] = attitudeState.Pitch;
} }
if (stabDesired.StabilizationMode.fields.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { if (stabDesired.StabilizationMode.Yaw == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) {
rpy_desired[2] = stabDesired.Yaw; rpy_desired[2] = stabDesired.Yaw;
} else { } else {
rpy_desired[2] = attitudeState.Yaw; rpy_desired[2] = attitudeState.Yaw;
@ -276,18 +276,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Run the selected stabilization algorithm on each axis: // Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) { for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized // Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]); bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode.data[i]; previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
// Apply the selected control law // Apply the selected control law
switch (stabDesired.StabilizationMode.data[i]) { switch (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if (reinit) { if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0; pids[PID_RATE_ROLL + i].iAccumulator = 0;
} }
// Store to rate desired variable for storing to UAVO // Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); rateDesiredAxis[i] =
bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -302,8 +303,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
} }
// Compute the outer loop // Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
// Compute the inner loop // Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
@ -354,7 +356,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
} }
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.ManualRate.data[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f); actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
@ -363,7 +366,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO // Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]); rateDesiredAxis[i] = bound(attitudeDesiredAxis[i],
cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -378,7 +382,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop like attitude mode // Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT); rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]); rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters // Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit); stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
@ -484,37 +489,37 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
StabilizationSettingsGet(&settings); StabilizationSettingsGet(&settings);
// Set the roll rate PID constants // Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp, pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp,
settings.RollRatePID.fields.Ki, settings.RollRatePID.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd, pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit); pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit);
// Set the pitch rate PID constants // Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp, pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki, pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd, pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit); pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants // Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp, pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki, pids[PID_RATE_YAW].i = settings.YawRatePID.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd, pids[PID_RATE_YAW].d = settings.YawRatePID.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit); pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit);
// Set the roll attitude PI constants // Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp, pid_configure(&pids[PID_ROLL], settings.RollPI.Kp,
settings.RollPI.fields.Ki, 0, settings.RollPI.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit); pids[PID_ROLL].iLim = settings.RollPI.ILimit);
// Set the pitch attitude PI constants // Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp, pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp,
pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0, pids[PID_PITCH].i = settings.PitchPI.Ki, 0,
settings.PitchPI.fields.ILimit); settings.PitchPI.ILimit);
// Set the yaw attitude PI constants // Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp, pid_configure(&pids[PID_YAW], settings.YawPI.Kp,
settings.YawPI.fields.Ki, 0, settings.YawPI.Ki, 0,
settings.YawPI.fields.ILimit); settings.YawPI.ILimit);
// Set up the derivative term // Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -531,9 +536,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
// The dT has some jitter iteration to iteration that we don't want to // The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant // make thie result unpredictable. Still, it's nicer to specify the constant

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

@ -32,6 +32,7 @@
#include "openpilot.h" #include "openpilot.h"
#include <pios_math.h> #include <pios_math.h>
#include <pios_struct_helper.h>
#include "stabilization.h" #include "stabilization.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
@ -64,23 +65,23 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Get the settings for the correct axis // Get the settings for the correct axis
switch (axis) { switch (axis) {
case ROLL: case ROLL:
kp = settings->VbarRollPI.fields.Kp; kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.fields.Ki; ki = settings->VbarRollPI.Ki;
break; break;
case PITCH: case PITCH:
kp = settings->VbarPitchPI.fields.Kp; kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.fields.Ki;; ki = settings->VbarPitchPI.Ki;;
break; break;
case YAW: case YAW:
kp = settings->VbarYawPI.fields.Kp; kp = settings->VbarYawPI.Kp;
ki = settings->VbarYawPI.fields.Ki; ki = settings->VbarYawPI.Ki;
break; break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
// Command signal is composed of stick input added to the gyro and virtual flybar // Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * settings->VbarSensitivity.data[axis] - *output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp); gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0; return 0;

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

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

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

@ -39,10 +39,11 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
// private includes // private includes
#include "inc/systemmod.h" #include "inc/systemmod.h"
// UAVOs // UAVOs
#include <objectpersistence.h> #include <objectpersistence.h>
#include <flightstatus.h> #include <flightstatus.h>
@ -433,9 +434,9 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
// By convention, there is a direct mapping between task monitor task_id's and members // By convention, there is a direct mapping between task monitor task_id's and members
// of the TaskInfoXXXXElem enums // of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; cast_struct_to_array(taskData->Running, taskData->Running.System)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining.data[task_id] = task_info->stack_remaining; ((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
taskData->RunningTime.data[task_id] = task_info->running_time_percentage; ((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
} }
#endif #endif

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

@ -51,7 +51,7 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <pios_struct_helper.h>
#include "txpidsettings.h" #include "txpidsettings.h"
#include "accessorydesired.h" #include "accessorydesired.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
@ -171,111 +171,116 @@ static void updatePIDs(UAVObjEvent *ev)
// Loop through every enabled instance // Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (inst.PIDs.data[i] != TXPIDSETTINGS_PIDS_DISABLED) { if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value; float value;
if (inst.Inputs.data[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value); ManualControlCommandThrottleGet(&value);
value = scale(value, value = scale(value,
inst.ThrottleRange.fields.Min, inst.ThrottleRange.Min,
inst.ThrottleRange.fields.Max, inst.ThrottleRange.Max,
inst.MinPID.data[i], inst.MaxPID.data[i]); cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
} else if (AccessoryDesiredInstGet(inst.Inputs.data[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID.data[i], inst.MaxPID.data[i]); } else if (AccessoryDesiredInstGet(
cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
} else { } else {
continue; continue;
} }
switch (inst.PIDs.data[i]) { switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP: case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); needsUpdate |= update(&stab.RollRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI: case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); needsUpdate |= update(&stab.RollRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD: case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); needsUpdate |= update(&stab.RollRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); needsUpdate |= update(&stab.RollRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI.fields.Kp, value); needsUpdate |= update(&stab.RollPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI.fields.Ki, value); needsUpdate |= update(&stab.RollPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.fields.ILimit, value); needsUpdate |= update(&stab.RollPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP: case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); needsUpdate |= update(&stab.PitchRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI: case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); needsUpdate |= update(&stab.PitchRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD: case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); needsUpdate |= update(&stab.PitchRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI.fields.Kp, value); needsUpdate |= update(&stab.PitchPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI.fields.Ki, value); needsUpdate |= update(&stab.PitchPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); needsUpdate |= update(&stab.PitchPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID.fields.Kp, value); needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kp, value); needsUpdate |= update(&stab.PitchRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID.fields.Ki, value); needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Ki, value); needsUpdate |= update(&stab.PitchRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID.fields.Kd, value); needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdate |= update(&stab.PitchRatePID.fields.Kd, value); needsUpdate |= update(&stab.PitchRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID.fields.ILimit, value); needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID.fields.ILimit, value); needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI.fields.Kp, value); needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdate |= update(&stab.PitchPI.fields.Kp, value); needsUpdate |= update(&stab.PitchPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI.fields.Ki, value); needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdate |= update(&stab.PitchPI.fields.Ki, value); needsUpdate |= update(&stab.PitchPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI.fields.ILimit, value); needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdate |= update(&stab.PitchPI.fields.ILimit, value); needsUpdate |= update(&stab.PitchPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKP: case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID.fields.Kp, value); needsUpdate |= update(&stab.YawRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKI: case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID.fields.Ki, value); needsUpdate |= update(&stab.YawRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKD: case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID.fields.Kd, value); needsUpdate |= update(&stab.YawRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID.fields.ILimit, value); needsUpdate |= update(&stab.YawRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI.fields.Kp, value); needsUpdate |= update(&stab.YawPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI.fields.Ki, value); needsUpdate |= update(&stab.YawPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI.fields.ILimit, value); needsUpdate |= update(&stab.YawPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_GYROTAU: case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdate |= update(&stab.GyroTau, value); needsUpdate |= update(&stab.GyroTau, value);

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

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

9
flight/pios/inc/pios_struct_helper.h
View File

@ -17,4 +17,13 @@
(type *)((char *)__mptr - offsetof(type, member)); } \ (type *)((char *)__mptr - offsetof(type, member)); } \
) )
/**
* cast_struct_to_array casts an homogeneous structure instance to an array
* of typeof(struct_field). struct_field need to be any of the fields
* containing inside the struct
* @instance: homogeneous structure to cast
* @struct_field: a field contained inside the structure
*/
#define cast_struct_to_array(instance, struct_field) \
((typeof(struct_field) *) & (instance))
#endif /* PIOS_STRUCT_HELPER_H */ #endif /* PIOS_STRUCT_HELPER_H */

2
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -866,7 +866,7 @@ void PIOS_Board_Init(void)
{ {
HwSettingsData hwSettings; HwSettingsData hwSettings;
HwSettingsGet(&hwSettings); HwSettingsGet(&hwSettings);
if (hwSettings.OptionalModules.fields.Overo == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (hwSettings.OptionalModules.Overo == HWSETTINGS_OPTIONALMODULES_ENABLED) {
if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) {
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }

42
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -126,19 +126,16 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// Check if it a named set and creates structures accordingly // Check if it a named set and creates structures accordingly
if (info->fields[n]->numElements > 1) { if (info->fields[n]->numElements > 1) {
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
QString unionTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name); QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name);
QString unionType = QString("typedef union {\n"); QString structType = QString("typedef struct __attribute__ ((__packed__)) {\n");
unionType.append(QString(" %1 data[%2];\n").arg(type).arg(info->fields[n]->numElements));
unionType.append(QString(" struct __attribute__ ((__packed__)) {\n"));
for (int f = 0; f < info->fields[n]->elementNames.count(); f++) { for (int f = 0; f < info->fields[n]->elementNames.count(); f++) {
unionType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f])); structType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f]));
} }
unionType.append(QString(" } fields;\n")); structType.append(QString("} %1 ;\n\n").arg(structTypeName));
unionType.append(QString("} %1 ;\n\n").arg(unionTypeName));
dataStructures.append(unionType); dataStructures.append(structType);
fields.append(QString(" %1 %2;\n").arg(unionTypeName) fields.append(QString(" %1 %2;\n").arg(structTypeName)
.arg(info->fields[n]->name)); .arg(info->fields[n]->name));
} else { } else {
fields.append(QString(" %1 %2[%3];\n").arg(type) fields.append(QString(" %1 %2[%3];\n").arg(type)
@ -225,28 +222,25 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
} else { } else {
// Initialize all fields in the array // Initialize all fields in the array
for (int idx = 0; idx < info->fields[n]->numElements; ++idx) { for (int idx = 0; idx < info->fields[n]->numElements; ++idx) {
QString optIdentifier; if (info->fields[n]->elementNames[0].compare(QString("0")) == 0) {
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) { initfields.append(QString(" data.%1[%2] = ")
optIdentifier = QString(".data"); .arg(info->fields[n]->name)
.arg(idx));
} else {
initfields.append(QString(" data.%1.%2 = ")
.arg(info->fields[n]->name)
.arg(info->fields[n]->elementNames[idx]));
} }
if (info->fields[n]->type == FIELDTYPE_ENUM) { if (info->fields[n]->type == FIELDTYPE_ENUM) {
initfields.append(QString(" data.%1%2[%3] = %4;\n") initfields.append(QString("%1;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
.arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx]))); .arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx])));
} else if (info->fields[n]->type == FIELDTYPE_FLOAT32) { } else if (info->fields[n]->type == FIELDTYPE_FLOAT32) {
initfields.append(QString(" data.%1%2[%3] = %4f;\n") initfields.append(QString("%1f;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
.arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6)); .arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6));
} else { } else {
initfields.append(QString(" data.%1%2[%3] = %4;\n") initfields.append(QString("%1;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
.arg(info->fields[n]->defaultValues[idx].toInt())); .arg(info->fields[n]->defaultValues[idx].toInt()));
} }
} }