rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +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 <pios_struct_helper.h>
#include "inc/alarms.h"
// Private constants
@ -74,8 +75,8 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (alarms.Alarm.data[alarm] != severity) {
alarms.Alarm.data[alarm] = severity;
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsSet(&alarms);
}
@ -109,10 +110,10 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (alarms.Alarm.data[alarm] != severity) {
alarms.ExtendedAlarmStatus.data[alarm] = status;
alarms.ExtendedAlarmSubStatus.data[alarm] = subStatus;
alarms.Alarm.data[alarm] = severity;
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsSet(&alarms);
}
@ -137,7 +138,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
// Read alarm
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
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);
return 1;
}

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

@ -392,9 +392,9 @@ static void altitudeHoldTask(__attribute__((unused)) void *parameters)
// shutdown motors
stabilizationDesired.Throttle = -1;
}
stabilizationDesired.StabilizationMode.fields.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.fields.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.fields.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
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;
bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;
accelbias[0] = attitudeSettings.AccelBias.fields.X;
accelbias[1] = attitudeSettings.AccelBias.fields.Y;
accelbias[2] = attitudeSettings.AccelBias.fields.Z;
accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.Z;
gyro_correct_int[0] = attitudeSettings.GyroBias.fields.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.fields.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.fields.Z / 100.0f;
gyro_correct_int[0] = attitudeSettings.GyroBias.X / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias.Y / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias.Z / 100.0f;
// Indicates not to expend cycles on rotation
if (attitudeSettings.BoardRotation.fields.Pitch == 0 && attitudeSettings.BoardRotation.fields.Roll == 0 &&
attitudeSettings.BoardRotation.fields.Yaw == 0) {
if (attitudeSettings.BoardRotation.Pitch == 0 && attitudeSettings.BoardRotation.Roll == 0 &&
attitudeSettings.BoardRotation.Yaw == 0) {
rotate = 0;
// Shouldn't be used but to be safe
@ -627,9 +627,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R);
} else {
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.fields.Roll,
attitudeSettings.BoardRotation.fields.Pitch,
attitudeSettings.BoardRotation.fields.Yaw };
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
@ -643,10 +643,10 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias.fields.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.fields.Y = trim_accels[1] / trim_samples;
attitudeSettings.AccelBias.X = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias.Y = trim_accels[1] / trim_samples;
// 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;
AttitudeSettingsSet(&attitudeSettings);
} else {

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

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

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

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

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

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

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

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

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

@ -34,7 +34,7 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "accessorydesired.h"
#include "actuatordesired.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) {
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;
} 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
@ -256,15 +258,18 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
} else {
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax.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
if (settings.ChannelGroups.fields.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.fields.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
||
// Check all channel mappings are valid
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 ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups.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_NODRIVER))) {
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
valid_input_detected &= validInputRange(settings.ChannelMin.fields.Throttle,
settings.ChannelMax.fields.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.fields.Roll,
settings.ChannelMax.fields.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.fields.Yaw,
settings.ChannelMax.fields.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.fields.Pitch,
settings.ChannelMax.fields.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
valid_input_detected &= validInputRange(settings.ChannelMin.Throttle,
settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin.Roll,
settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin.Yaw,
settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin.Pitch,
settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
@ -333,21 +338,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 1
if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
}
// Set Accessory 2
if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -389,7 +394,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups.fields.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
@ -406,7 +411,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
// Set Accessory 1
if (settings.ChannelGroups.fields.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
@ -423,7 +428,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
}
}
// Set Accessory 2
if (settings.ChannelGroups.fields.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
#ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
@ -453,8 +458,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters)
if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel,
settings.ChannelMin.data,
settings.ChannelMax.data,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll),
NELEMENTS(cmd.Channel));
}
#endif /* PIOS_INCLUDE_USB_RCTX */
@ -671,13 +676,13 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings.data;
stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings.data;
stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings.data;
stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll);
break;
default:
// 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
stabilization.StabilizationMode.fields.Roll = stab_settings[0];
stabilization.StabilizationMode.fields.Pitch = stab_settings[1];
stabilization.StabilizationMode.fields.Yaw = stab_settings[2];
stabilization.StabilizationMode.Roll = stab_settings[0];
stabilization.StabilizationMode.Pitch = stab_settings[1];
stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.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) ?
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_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_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
;
stabilization.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) ?
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_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_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
stabilization.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) ?
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_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_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
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
@ -754,14 +759,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.fields.North = 0;
pathDesired.Start.fields.East = 0;
pathDesired.Start.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.fields.North = 0;
pathDesired.End.fields.East = 0;
pathDesired.End.fields.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Start.North = 0;
pathDesired.Start.East = 0;
pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End.North = 0;
pathDesired.End.East = 0;
pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if (changed) {
@ -771,14 +776,14 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North;
pathDesired.End.fields.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
/* 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);
if (changed) {
// After not being in this mode for a while init at current height
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.End.fields.North = positionState.North;
pathDesired.End.fields.East = positionState.East;
pathDesired.End.fields.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.End.North = positionState.North;
pathDesired.End.East = positionState.East;
pathDesired.End.Down = positionState.Down;
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
}
pathDesired.End.fields.Down = positionState.Down + 5;
pathDesired.End.Down = positionState.Down + 5;
PathDesiredSet(&pathDesired);
}
@ -875,7 +880,7 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.fields.Yaw;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
float currentDown;
PositionStateDownGet(&currentDown);
@ -974,7 +979,7 @@ static bool okToArm(void)
// Check each alarm
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) {
continue;
}
@ -1007,7 +1012,7 @@ static bool forcedDisArm(void)
SystemAlarmsGet(&alarms);
if (alarms.Alarm.fields.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) {
return true;
}
return false;
@ -1246,8 +1251,8 @@ static void applyDeadband(float *value, float deadband)
*/
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{
if (settings->ResponseTime.data[channel]) {
float rt = (float)settings->ResponseTime.data[channel];
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel];
}

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

@ -2219,8 +2219,8 @@ void updateGraphics()
/* Draw Attitude Indicator */
if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.fields.X),
APPLY_VDEADBAND(OsdSettings.AttitudeSetup.fields.Y), attitude.Pitch, attitude.Roll, 96);
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup.X),
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);
// printText16( 60, 12,"Hello OP-OSD");
@ -2239,7 +2239,7 @@ void updateGraphics()
/* Print RTC time */
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 */
@ -2292,22 +2292,22 @@ void updateGraphics()
// drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
// Draw airspeed (left side.)
if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.fields.X),
APPLY_VDEADBAND(OsdSettings.SpeedSetup.fields.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup.X),
APPLY_VDEADBAND(OsdSettings.SpeedSetup.Y), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
}
// Draw altimeter (right side.)
if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.fields.X),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup.fields.Y), 100, 20, 100, 7, 12, 15, 500, 0);
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup.X),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup.Y), 100, 20, 100, 7, 12, 15, 500, 0);
}
// Draw compass.
if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
if (attitude.Yaw < 0) {
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X),
APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0);
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
APPLY_VDEADBAND(OsdSettings.HeadingSetup.Y), 15, 30, 7, 12, 0);
} else {
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.fields.X),
APPLY_VDEADBAND(OsdSettings.HeadingSetup.fields.Y), 15, 30, 7, 12, 0);
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup.X),
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 "waypointactive.h"
#include "taskinfo.h"
#include <pios_struct_helper.h>
#include "paths.h"
// Private constants
@ -227,9 +227,9 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
WaypointInstGet(waypointActiveData.Index, &waypointData);
PathActionInstGet(waypointData.Action, &pathActionData);
pathDesired.End.fields.North = waypointData.Position.fields.North;
pathDesired.End.fields.East = waypointData.Position.fields.East;
pathDesired.End.fields.Down = waypointData.Position.fields.Down;
pathDesired.End.North = waypointData.Position.North;
pathDesired.End.East = waypointData.Position.East;
pathDesired.End.Down = waypointData.Position.Down;
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
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_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start.fields.North = positionState.North;
pathDesired.Start.fields.East = positionState.East;
pathDesired.Start.fields.Down = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start.fields.North = waypointPrev.Position.fields.North;
pathDesired.Start.fields.East = waypointPrev.Position.fields.East;
pathDesired.Start.fields.Down = waypointPrev.Position.fields.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
pathDesired.Start.North = waypointPrev.Position.North;
pathDesired.Start.East = waypointPrev.Position.East;
pathDesired.Start.Down = waypointPrev.Position.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
}
@ -369,12 +369,12 @@ static uint8_t conditionDistanceToTarget()
PositionStateGet(&positionState);
if (pathAction.ConditionParameters[1] > 0.5f) {
distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position.fields.East - positionState.East, 2)
+ powf(waypoint.Position.fields.Down - positionState.Down, 2));
distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
+ powf(waypoint.Position.East - positionState.East, 2)
+ powf(waypoint.Position.Down - positionState.Down, 2));
} else {
distance = sqrtf(powf(waypoint.Position.fields.North - positionState.North, 2)
+ powf(waypoint.Position.fields.East - positionState.East, 2));
distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
+ powf(waypoint.Position.East - positionState.East, 2));
}
if (distance <= pathAction.ConditionParameters[0]) {
@ -400,7 +400,9 @@ static uint8_t conditionLegRemaining()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
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]) {
return true;
}
@ -423,7 +425,9 @@ static uint8_t conditionBelowError()
float cur[3] = { positionState.North, positionState.East, positionState.Down };
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]) {
return true;
}
@ -492,7 +496,7 @@ static uint8_t conditionPointingTowardsNext()
WaypointData 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;
VelocityStateGet(&velocity);

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

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

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

@ -32,6 +32,7 @@
*/
#include "openpilot.h"
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "relaytuning.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
if (reinit) {
rateRelayRunning[axis] = false;
relay.Period.data[axis] = 200;
relay.Gain.data[axis] = 0;
rateRelayRunning[axis] = false;
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200;
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0;
accum_sin = 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 ****/
// Make sure the period can't go below limit
if (relay.Period.data[axis] < DEGLITCH_TIME) {
relay.Period.data[axis] = DEGLITCH_TIME;
if (cast_struct_to_array(relay.Period, relay.Period.Roll)[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
// to accumulate the average amplitude
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) {
phase = 0;
}
@ -124,13 +125,17 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
accum_cos = 0;
if (rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
relay.Period.data[axis] = 200;
relay.Gain.data[axis] = 0;
rateRelayRunning[axis] = true;
cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200;
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0;
} else {
// Low pass filter each amplitude and period
relay.Gain.data[axis] = relay.Gain.data[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period.data[axis] = relay.Period.data[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] =
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;
high = true;

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

@ -32,7 +32,7 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "stabilization.h"
#include "stabilizationsettings.h"
#include "actuatordesired.h"
@ -201,11 +201,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
} else {
// 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);
if (speedScaleFactor < settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN]) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MIN];
if (speedScaleFactor < settings.ScaleToAirspeedLimits.Min) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Min;
}
if (speedScaleFactor > settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX]) {
speedScaleFactor = settings.ScaleToAirspeedLimits[STABILIZATIONSETTINGS_SCALETOAIRSPEEDLIMITS_MAX];
if (speedScaleFactor > settings.ScaleToAirspeedLimits.Max) {
speedScaleFactor = settings.ScaleToAirspeedLimits.Max;
}
}
#else
@ -220,19 +220,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
float local_error[3];
// 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;
} else {
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;
} else {
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;
} else {
rpy_desired[2] = attitudeState.Yaw;
@ -276,18 +276,19 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Run the selected stabilization algorithm on each axis:
for (uint8_t i = 0; i < MAX_AXES; i++) {
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode.data[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode.data[i];
bool reinit = (cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i] != previous_mode[i]);
previous_mode[i] = cast_struct_to_array(stabDesired.StabilizationMode, stabDesired.StabilizationMode.Roll)[i];
// 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:
if (reinit) {
pids[PID_RATE_ROLL + i].iAccumulator = 0;
}
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.data[i]);
rateDesiredAxis[i] =
bound(attitudeDesiredAxis[i], cast_struct_to_array(settings.ManualRate, settings.ManualRate.Roll)[i]);
// Compute the inner loop
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
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate.data[i]);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(settings.MaximumRate, settings.MaximumRate.Roll)[i]);
// Compute the inner loop
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] = 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] = bound(actuatorDesiredAxis[i], 1.0f);
@ -363,7 +366,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate.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
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
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
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);
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.fields.Kp,
settings.RollRatePID.fields.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.fields.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.fields.ILimit);
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.Kp,
settings.RollRatePID.Ki,
pids[PID_RATE_ROLL].d = settings.RollRatePID.Kd,
pids[PID_RATE_ROLL].iLim = settings.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.fields.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.fields.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.fields.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.fields.ILimit);
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.Kp,
pids[PID_RATE_PITCH].i = settings.PitchRatePID.Ki,
pids[PID_RATE_PITCH].d = settings.PitchRatePID.Kd,
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.fields.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.fields.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.fields.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.fields.ILimit);
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.Kp,
pids[PID_RATE_YAW].i = settings.YawRatePID.Ki,
pids[PID_RATE_YAW].d = settings.YawRatePID.Kd,
pids[PID_RATE_YAW].iLim = settings.YawRatePID.ILimit);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ROLL], settings.RollPI.fields.Kp,
settings.RollPI.fields.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.fields.ILimit);
pid_configure(&pids[PID_ROLL], settings.RollPI.Kp,
settings.RollPI.Ki, 0,
pids[PID_ROLL].iLim = settings.RollPI.ILimit);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_PITCH], settings.PitchPI.fields.Kp,
pids[PID_PITCH].i = settings.PitchPI.fields.Ki, 0,
settings.PitchPI.fields.ILimit);
pid_configure(&pids[PID_PITCH], settings.PitchPI.Kp,
pids[PID_PITCH].i = settings.PitchPI.Ki, 0,
settings.PitchPI.ILimit);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_YAW], settings.YawPI.fields.Kp,
settings.YawPI.fields.Ki, 0,
settings.YawPI.fields.ILimit);
pid_configure(&pids[PID_YAW], settings.YawPI.Kp,
settings.YawPI.Ki, 0,
settings.YawPI.ILimit);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
@ -531,9 +536,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.fields.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.fields.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.fields.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == 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
// 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 <pios_math.h>
#include <pios_struct_helper.h>
#include "stabilization.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
switch (axis) {
case ROLL:
kp = settings->VbarRollPI.fields.Kp;
ki = settings->VbarRollPI.fields.Ki;
kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.Ki;
break;
case PITCH:
kp = settings->VbarPitchPI.fields.Kp;
ki = settings->VbarPitchPI.fields.Ki;;
kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.Ki;;
break;
case YAW:
kp = settings->VbarYawPI.fields.Kp;
ki = settings->VbarYawPI.fields.Ki;
kp = settings->VbarYawPI.Kp;
ki = settings->VbarYawPI.Ki;
break;
default:
PIOS_DEBUG_Assert(0);
}
// Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * settings->VbarSensitivity.data[axis] -
*output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0;

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

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

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

@ -39,10 +39,11 @@
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
// private includes
#include "inc/systemmod.h"
// UAVOs
#include <objectpersistence.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
// of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
taskData->Running.data[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining.data[task_id] = task_info->stack_remaining;
taskData->RunningTime.data[task_id] = task_info->running_time_percentage;
cast_struct_to_array(taskData->Running, taskData->Running.System)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
}
#endif

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

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

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

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

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

@ -17,4 +17,13 @@
(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 */

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

@ -866,7 +866,7 @@ void PIOS_Board_Init(void)
{
HwSettingsData 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)) {
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
if (info->fields[n]->numElements > 1) {
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
QString unionTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name);
QString unionType = QString("typedef union {\n");
unionType.append(QString(" %1 data[%2];\n").arg(type).arg(info->fields[n]->numElements));
unionType.append(QString(" struct __attribute__ ((__packed__)) {\n"));
QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name);
QString structType = QString("typedef struct __attribute__ ((__packed__)) {\n");
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"));
unionType.append(QString("} %1 ;\n\n").arg(unionTypeName));
structType.append(QString("} %1 ;\n\n").arg(structTypeName));
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));
} else {
fields.append(QString(" %1 %2[%3];\n").arg(type)
@ -225,28 +222,25 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
} else {
// Initialize all fields in the array
for (int idx = 0; idx < info->fields[n]->numElements; ++idx) {
QString optIdentifier;
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
optIdentifier = QString(".data");
if (info->fields[n]->elementNames[0].compare(QString("0")) == 0) {
initfields.append(QString(" data.%1[%2] = ")
.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) {
initfields.append(QString(" data.%1%2[%3] = %4;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
initfields.append(QString("%1;\n")
.arg(info->fields[n]->options.indexOf(info->fields[n]->defaultValues[idx])));
} else if (info->fields[n]->type == FIELDTYPE_FLOAT32) {
initfields.append(QString(" data.%1%2[%3] = %4f;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
initfields.append(QString("%1f;\n")
.arg(info->fields[n]->defaultValues[idx].toFloat(), 0, 'e', 6));
} else {
initfields.append(QString(" data.%1%2[%3] = %4;\n")
.arg(info->fields[n]->name)
.arg(optIdentifier)
.arg(idx)
initfields.append(QString("%1;\n")
.arg(info->fields[n]->defaultValues[idx].toInt()));
}
}