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

Merge branch 'next' into Mathieu/OP-1079_Update_to_FreeRTOS_v7_5_2

Browse Source
This commit is contained in:
mathieu 2013-09-11 18:57:37 -07:00
commit fb1f58c94b
36 changed files with 774 additions and 603 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[alarm] != severity) {
alarms.Alarm[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[alarm] != severity) {
alarms.ExtendedAlarmStatus[alarm] = status;
alarms.ExtendedAlarmSubStatus[alarm] = subStatus;
alarms.Alarm[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[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[n] >= severity) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}

6
flight/libraries/sanitycheck.c
View File

@ -204,13 +204,13 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
// Get the different axis modes for this switch position
switch (index) {
case 1:
ManualControlSettingsStabilization1SettingsGet(modes);
ManualControlSettingsStabilization1SettingsArrayGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsGet(modes);
ManualControlSettingsStabilization2SettingsArrayGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsGet(modes);
ManualControlSettingsStabilization3SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;

2
flight/modules/Airspeed/airspeed.c
View File

@ -109,7 +109,7 @@ int32_t AirspeedInitialize()
#endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingGet(adcRouting);
HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the barometric airspeed sensor is routed to an ADC pin
for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {

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

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

30
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[ATTITUDESETTINGS_ACCELBIAS_X];
accelbias[1] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y];
accelbias[2] = attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z];
accelbias[0] = attitudeSettings.AccelBias.X;
accelbias[1] = attitudeSettings.AccelBias.Y;
accelbias[2] = attitudeSettings.AccelBias.Z;
gyro_correct_int[0] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_X] / 100.0f;
gyro_correct_int[1] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
gyro_correct_int[2] = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_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[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 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[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] };
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
@ -643,11 +643,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
trim_requested = true;
} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
trim_requested = false;
attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples;
attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_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[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
attitudeSettings.AccelBias.Z = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings);
} else {
trim_requested = false;

88
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"
@ -366,7 +366,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
@ -834,12 +834,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
value_error = true;
}
if (invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_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[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] }
INSSetMagVar((float[3]) { ekfConfiguration.R.MagX,
ekfConfiguration.R.MagY,
ekfConfiguration.R.MagZ }
);
INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX,
ekfConfiguration.Q.AccelY,
ekfConfiguration.Q.AccelZ }
);
INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX,
ekfConfiguration.Q.GyroY,
ekfConfiguration.Q.GyroZ }
);
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX,
ekfConfiguration.Q.GyroDriftY,
ekfConfiguration.Q.GyroDriftZ }
);
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_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);
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[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_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[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_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[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_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);
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[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[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[t])) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) {
error = true;
}
}

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

@ -91,7 +91,7 @@ int32_t BatteryInitialize(void)
#endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingGet(adcRouting);
HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the battery sensors are routed to ADC pins
for (int i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
@ -135,13 +135,13 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev)
// calculate the battery parameters
if (voltageADCPin >= 0) {
flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; // in Volts
flightBatteryData.Voltage = (PIOS_ADC_PinGetVolt(voltageADCPin) - batterySettings.SensorCalibrations.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[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTZERO]) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_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[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_ALARM] * batterySettings.NbCells) {
if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * batterySettings.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);
} else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds[FLIGHTBATTERYSETTINGS_CELLVOLTAGETHRESHOLDS_WARNING] * batterySettings.NbCells) {
} else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * batterySettings.NbCells) {
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);

45
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
@ -96,12 +97,12 @@ int32_t CameraStabInitialize(void)
#ifdef MODULE_CAMERASTAB_BUILTIN
cameraStabEnabled = true;
#else
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesData optionalModules;
HwSettingsInitialize();
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
if (optionalModules.CameraStab == HWSETTINGS_OPTIONALMODULES_ENABLED) {
cameraStabEnabled = true;
} else {
cameraStabEnabled = false;
@ -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[i] != CAMERASTABSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(cameraStab.Input[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[i]) {
switch (cast_struct_to_array(cameraStab.StabilizationMode, cameraStab.StabilizationMode.Roll)[i]) {
case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
csd->inputs[i] = accessory.AccessoryVal * cameraStab.InputRange[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[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[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) {
float rt = (float)cameraStab.ResponseTime[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[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[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[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH] - fabsf(pitch))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_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[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL] - fabsf(roll))
/ cameraStab->OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_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[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[index] : cameraStab->DecelTime[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;
}

6
flight/modules/ComUsbBridge/ComUsbBridge.c
View File

@ -102,12 +102,12 @@ static int32_t comUsbBridgeInitialize(void)
bridge_enabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesGet(&optionalModules);
if (usart_port && vcp_port &&
(optionalModules[HWSETTINGS_OPTIONALMODULES_COMUSBBRIDGE] == HWSETTINGS_OPTIONALMODULES_ENABLED)) {
(optionalModules.ComUsbBridge == HWSETTINGS_OPTIONALMODULES_ENABLED)) {
bridge_enabled = true;
} else {
bridge_enabled = false;

159
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"
@ -109,9 +110,9 @@ int32_t FixedWingPathFollowerStart()
int32_t FixedWingPathFollowerInitialize()
{
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_FIXEDWINGPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) {
followerEnabled = true;
FixedWingPathFollowerSettingsInitialize();
FixedWingPathFollowerStatusInitialize();
@ -261,7 +262,9 @@ static void updatePathVelocity()
positionState.Down + (velocityState.Down * fixedwingpathfollowerSettings.CourseFeedForward) };
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
path_progress(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[2];
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[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
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[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_OVERSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
fixedwingpathfollowerStatus.Errors.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[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins.Highspeed) {
fixedwingpathfollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins.Lowspeed) {
fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_STALLSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins.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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_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[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] > 0) {
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT] / fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
-fixedwingpathfollowerSettings.PowerPI.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[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_KP],
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOPOWERCROSSFEED_MAX]
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
);
// Compute final throttle response
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
powerIntegral * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] +
powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp +
powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand;
fixedwingpathfollowerStatus.Error.Power = descentspeedError;
fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral;
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set throttle
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL] + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrottleLimit.Min,
fixedwingpathfollowerSettings.ThrottleLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
fixedwingpathfollowerStatus.Errors.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[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_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[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_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[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0) {
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT] / fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
-fixedwingpathfollowerSettings.SpeedPI.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[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_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[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
+ airspeedErrorInt * fixedwingpathfollowerSettings.SpeedPI.Ki
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
fixedwingpathfollowerStatus.Error.Speed = airspeedError;
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
fixedwingpathfollowerStatus.Errors.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[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_PITCHCONTROL] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if ((headingError > fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND] ||
headingError < -fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND]) &&
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors.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[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_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[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KI],
-fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT],
fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_ILIMIT]);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI[FIXEDWINGPATHFOLLOWERSETTINGS_COURSEPI_KP] +
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_COURSE] = courseError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_COURSE] = courseIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_COURSE] = courseCommand;
fixedwingpathfollowerStatus.Error.Course = courseError;
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_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[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired);

6
flight/modules/GPS/GPS.c
View File

@ -130,11 +130,11 @@ int32_t GPSInitialize(void)
gpsEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
if (optionalModules.GPS == HWSETTINGS_OPTIONALMODULES_ENABLED) {
gpsEnabled = true;
} else {
gpsEnabled = false;

139
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[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[n]], settings.ChannelNumber[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[n], settings.ChannelMin[n], settings.ChannelNeutral[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[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
if (settings.ChannelGroups.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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
valid_input_detected &= validInputRange(settings.ChannelMin.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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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[MANUALCONTROLSETTINGS_CHANNELGROUPS_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,
settings.ChannelMax,
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;
stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
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[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_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[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
cmd->Pitch * stabSettings.ManualRate.Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_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[STABILIZATIONSETTINGS_MANUALRATE_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,12 +759,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down - settings.ReturnToHomeAltitudeOffset;
pathDesired.Start.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;
@ -771,12 +776,12 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.Start.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;
@ -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[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionState.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionState.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionState.Down;
pathDesired.Start.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[PATHDESIRED_END_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[STABILIZATIONSETTINGS_MANUALRATE_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[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[SYSTEMALARMS_ALARM_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[channel]) {
float rt = (float)settings->ResponseTime[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[OSDSETTINGS_ATTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_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[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_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[OSDSETTINGS_SPEEDSETUP_X]),
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_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[OSDSETTINGS_ALTITUDESETUP_X]),
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_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[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_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[OSDSETTINGS_HEADINGSETUP_X]),
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_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);
}
}
}

6
flight/modules/Osd/osdoutput/osdoutput.c
View File

@ -276,9 +276,9 @@ static int32_t osdoutputInitialize(void)
osdoutputEnabled = 1;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_OSDHK] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.OsdHk == HWSETTINGS_OPTIONALMODULES_ENABLED) {
osdoutputEnabled = 1;
} else {
osdoutputEnabled = 0;

40
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[PATHDESIRED_END_NORTH] = waypointData.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypointData.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypointData.Position[WAYPOINT_POSITION_DOWN];
pathDesired.End.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,18 +246,18 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
pathDesired.Start[PATHDESIRED_START_NORTH] = positionState.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionState.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionState.Down;
pathDesired.Start.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[PATHDESIRED_START_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.Start.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[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - positionState.East, 2)
+ powf(waypoint.Position[2] - 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[0] - positionState.North, 2)
+ powf(waypoint.Position[1] - 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, pathDesired.End, 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, pathDesired.End, 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[0] - waypoint.Position[0]), (nextWaypoint.Position[1] - waypoint.Position[1]));
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[REVOCALIBRATION_MAG_BIAS_X];
mag_bias[1] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Y];
mag_bias[2] = cal.mag_bias[REVOCALIBRATION_MAG_BIAS_Z];
mag_scale[0] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_X];
mag_scale[1] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Y];
mag_scale[2] = cal.mag_scale[REVOCALIBRATION_MAG_SCALE_Z];
accel_bias[0] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_X];
accel_bias[1] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Y];
accel_bias[2] = cal.accel_bias[REVOCALIBRATION_ACCEL_BIAS_Z];
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
gyro_staticbias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_staticbias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_staticbias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
mag_bias[0] = cal.mag_bias.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[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
attitudeSettings.BoardRotation[2] == 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[ATTITUDESETTINGS_BOARDROTATION_ROLL],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] };
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;

23
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"
@ -71,8 +72,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
// On first run initialize estimates to something reasonable
if (reinit) {
rateRelayRunning[axis] = false;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
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[axis] < DEGLITCH_TIME) {
relay.Period[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[axis];
float phase = ((float)360 * (float)dT) / cast_struct_to_array(relay.Period, relay.Period.Roll)[axis];
if (phase >= 360) {
phase = 0;
}
@ -125,12 +126,16 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
if (rateRelayRunning[axis] == false) {
rateRelayRunning[axis] = true;
relay.Period[axis] = 200;
relay.Gain[axis] = 0;
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[axis] = relay.Gain[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
relay.Period[axis] = relay.Period[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[STABILIZATIONDESIRED_STABILIZATIONMODE_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[STABILIZATIONDESIRED_STABILIZATIONMODE_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[STABILIZATIONDESIRED_STABILIZATIONMODE_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[i] != previous_mode[i]);
previous_mode[i] = stabDesired.StabilizationMode[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[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[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[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[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[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[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[STABILIZATIONSETTINGS_ROLLRATEPID_KP],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD],
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID.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[STABILIZATIONSETTINGS_PITCHRATEPID_KP],
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD],
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID.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[STABILIZATIONSETTINGS_YAWRATEPID_KP],
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD],
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID.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[STABILIZATIONSETTINGS_ROLLPI_KP],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
pid_configure(&pids[PID_ROLL], settings.RollPI.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[STABILIZATIONSETTINGS_PITCHPI_KP],
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
pid_configure(&pids[PID_PITCH], settings.PitchPI.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[STABILIZATIONSETTINGS_YAWPI_KP],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_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[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.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[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.Ki;
break;
case PITCH:
kp = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarPitchPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.Ki;;
break;
case YAW:
kp = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
ki = settings->VbarYawPI[STABILIZATIONSETTINGS_VBARROLLPI_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[axis] -
*output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0;

15
flight/modules/StateEstimation/filtercf.c
View File

@ -47,6 +47,9 @@
#define STACK_REQUIRED 512
#define CALIBRATION_DELAY_MS 4000
#define CALIBRATION_DURATION_MS 6000
// Private types
struct data {
AttitudeSettingsData attitudeSettings;
@ -229,7 +232,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
mag[2] = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
this->magCalibrated = false;
@ -274,18 +277,18 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->grot_filtered[0] = 0.0f;
this->grot_filtered[1] = 0.0f;
this->grot_filtered[2] = 0.0f;
this->timeval = PIOS_DELAY_GetRaw();
this->starttime = this->timeval;
this->timeval = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing
this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals
return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion
}
if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 4000000) {
if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
// wait 4 seconds for the user to get his hands off in case the board was just powered
this->timeval = PIOS_DELAY_GetRaw();
return 1;
} else if (this->init == 0 && PIOS_DELAY_DiffuS(this->starttime) < 10000000) {
// For first 7 seconds use accels to get gyro bias
} else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) {
// For first 6 seconds use accels to get gyro bias
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f;

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[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[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[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[EKFCONFIGURATION_R_MAGX] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_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[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
this->ekfConfiguration.Q.AccelY,
this->ekfConfiguration.Q.AccelZ }
);
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q.GyroX,
this->ekfConfiguration.Q.GyroY,
this->ekfConfiguration.Q.GyroZ }
);
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q.GyroDriftX,
this->ekfConfiguration.Q.GyroDriftY,
this->ekfConfiguration.Q.GyroDriftZ }
);
INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_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);
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[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.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[EKFCONFIGURATION_R_GPSPOSNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R.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[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR.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);
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[t]) || vardata.P[t] <= 0.0f) {
INSResetP(this->ekfConfiguration.P);
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[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
taskData->StackRemaining[task_id] = task_info->stack_remaining;
taskData->RunningTime[task_id] = task_info->running_time_percentage;
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

99
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"
@ -88,12 +88,12 @@ static float scale(float val, float inMin, float inMax, float outMin, float outM
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesData optionalModules;
HwSettingsInitialize();
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_TXPID] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
@ -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[i] != TXPIDSETTINGS_PIDS_DISABLED) {
if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (inst.Inputs[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[TXPIDSETTINGS_THROTTLERANGE_MIN],
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, inst.MinPID[i], inst.MaxPID[i]);
inst.ThrottleRange.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[i]) {
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
needsUpdate |= update(&stab.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
needsUpdate |= update(&stab.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
needsUpdate |= update(&stab.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
needsUpdate |= update(&stab.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
needsUpdate |= update(&stab.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
needsUpdate |= update(&stab.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
needsUpdate |= update(&stab.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
needsUpdate |= update(&stab.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
needsUpdate |= update(&stab.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
needsUpdate |= update(&stab.RollRatePID.Kp, value);
needsUpdate |= update(&stab.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
needsUpdate |= update(&stab.RollRatePID.Ki, value);
needsUpdate |= update(&stab.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
needsUpdate |= update(&stab.RollRatePID.Kd, value);
needsUpdate |= update(&stab.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.RollRatePID.ILimit, value);
needsUpdate |= update(&stab.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
needsUpdate |= update(&stab.RollPI.Kp, value);
needsUpdate |= update(&stab.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
needsUpdate |= update(&stab.RollPI.Ki, value);
needsUpdate |= update(&stab.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
needsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
needsUpdate |= update(&stab.RollPI.ILimit, value);
needsUpdate |= update(&stab.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
needsUpdate |= update(&stab.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
needsUpdate |= update(&stab.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
needsUpdate |= update(&stab.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
needsUpdate |= update(&stab.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
needsUpdate |= update(&stab.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
needsUpdate |= update(&stab.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
needsUpdate |= update(&stab.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdate |= update(&stab.GyroTau, value);

107
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"
@ -125,11 +125,11 @@ int32_t VtolPathFollowerStart()
*/
int32_t VtolPathFollowerInitialize()
{
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_VTOLPATHFOLLOWER] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
if (optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) {
VtolPathFollowerSettingsInitialize();
NedAccelInitialize();
PathDesiredInitialize();
@ -337,8 +337,8 @@ static void updatePOIBearing()
// don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) {
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End.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[STABILIZATIONDESIRED_STABILIZATIONMODE_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, pathDesired.End, 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[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_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[2] + (pathDesired.End[2] - pathDesired.Start[2]) * 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[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.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[PATHDESIRED_END_NORTH] - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
northError = pathDesired.End.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[PATHDESIRED_END_EAST] - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
eastError = pathDesired.End.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[PATHDESIRED_END_DOWN] - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
downError = pathDesired.End.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[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_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[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.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[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.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[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.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[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
if (yaw_attitude) {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_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[HWSETTINGS_OPTIONALMODULES_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);
}

1
flight/uavobjects/inc/uavobject.h.template
View File

@ -51,6 +51,7 @@ int32_t $(NAME)Initialize();
UAVObjHandle $(NAME)Handle();
void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId);
$(DATASTRUCTURES)
/*
* Packed Object data (unaligned).
* Should only be used where 4 byte alignment can be guaranteed

4
ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp
View File

@ -40,7 +40,7 @@
#include <QEventLoop>
namespace Core {
ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack) :
ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow) :
QWidget(mainWindow),
m_availableDevList(0),
m_connectBtn(0),
@ -73,8 +73,6 @@ ConnectionManager::ConnectionManager(Internal::MainWindow *mainWindow, QTabWidge
setLayout(layout);
modeStack->setCornerWidget(this, Qt::TopRightCorner);
QObject::connect(m_connectBtn, SIGNAL(clicked()), this, SLOT(onConnectClicked()));
QObject::connect(m_availableDevList, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelectionChanged(int)));

14
ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h
View File

@ -29,11 +29,11 @@
#ifndef CONNECTIONMANAGER_H
#define CONNECTIONMANAGER_H
#include <QWidget>
#include "mainwindow.h"
#include "generalsettings.h"
#include "telemetrymonitorwidget.h"
#include <coreplugin/iconnection.h>
#include <QWidget>
#include <QtCore/QVector>
#include <QtCore/QIODevice>
#include <QtCore/QLinkedList>
@ -43,20 +43,14 @@
#include "core_global.h"
#include <QTimer>
QT_BEGIN_NAMESPACE
class QTabWidget;
QT_END_NAMESPACE
namespace Core {
class IConnection;
namespace Internal {
class FancyTabWidget;
class FancyActionBar;
class MainWindow;
class MainWindow;
} // namespace Internal
class DevListItem {
public:
DevListItem(IConnection *c, IConnection::device d) :
@ -86,7 +80,7 @@ class CORE_EXPORT ConnectionManager : public QWidget {
Q_OBJECT
public:
ConnectionManager(Internal::MainWindow *mainWindow, QTabWidget *modeStack);
ConnectionManager(Internal::MainWindow *mainWindow);
virtual ~ConnectionManager();
void init();

16
ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp
View File

@ -177,7 +177,8 @@ MainWindow::MainWindow() :
#endif
m_modeManager = new ModeManager(this, m_modeStack);
m_connectionManager = new ConnectionManager(this, m_modeStack);
m_connectionManager = new ConnectionManager(this);
m_modeStack->setCornerWidget(m_connectionManager, Qt::TopRightCorner);
m_messageManager = new MessageManager;
setCentralWidget(m_modeStack);
@ -1214,8 +1215,9 @@ void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly)
createWorkspaces(qs);
// Read tab ordering
// Restore tab ordering
qs->beginGroup(QLatin1String(modePriorities));
QStringList modeNames = qs->childKeys();
QMap<QString, int> map;
foreach(QString modeName, modeNames) {
@ -1224,6 +1226,12 @@ void MainWindow::readSettings(QSettings *qs, bool workspaceDiffOnly)
m_modeManager->reorderModes(map);
qs->endGroup();
// Restore selected tab
if (m_workspaceSettings->restoreSelectedOnStartup()) {
int index = qs->value(QLatin1String("SelectedWorkspace")).toInt();
m_modeStack->setCurrentIndex(index);
}
}
@ -1262,12 +1270,16 @@ void MainWindow::saveSettings(QSettings *qs)
}
qs->endGroup();
// Write selected tab
qs->setValue(QLatin1String("SelectedWorkspace"), m_modeStack->currentIndex());
foreach(UAVGadgetManager * manager, m_uavGadgetManagers) {
manager->saveSettings(qs);
}
m_actionManager->saveSettings(qs);
m_generalSettings->saveSettings(qs);
qs->beginGroup("General");
qs->setValue("Description", m_config_description);
qs->setValue("Details", m_config_details);

4
ground/openpilotgcs/src/plugins/coreplugin/uavgadgetinstancemanager.cpp
View File

@ -238,6 +238,10 @@ void UAVGadgetInstanceManager::createOptionsPages()
while (ite.hasNext()) {
IUAVGadgetConfiguration *config = ite.next();
IUAVGadgetFactory *f = factory(config->classId());
if (!f) {
qWarning() << "No gadget factory for configuration " + config->classId();
continue;
}
IOptionsPage *p = f->createOptionsPage(config);
if (p) {
IOptionsPage *page = new UAVGadgetOptionsPageDecorator(p, config, f->isSingleConfigurationGadget());

17
ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.cpp
View File

@ -33,18 +33,19 @@
#include "ui_workspacesettings.h"
using namespace Core;
using namespace Core::Internal;
const int WorkspaceSettings::MAX_WORKSPACES = 10;
WorkspaceSettings::WorkspaceSettings(QObject *parent) :
IOptionsPage(parent)
{}
IOptionsPage(parent)
{
}
WorkspaceSettings::~WorkspaceSettings()
{}
{
}
// IOptionsPage
@ -84,7 +85,6 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent)
m_page->iconPathChooser->setPromptDialogFilter(tr("Images (*.png *.jpg *.bmp *.xpm)"));
m_page->iconPathChooser->setPromptDialogTitle(tr("Choose icon"));
connect(m_page->workspaceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectWorkspace(int)));
connect(m_page->numberOfWorkspacesSpinBox, SIGNAL(valueChanged(int)), this, SLOT(numberOfWorkspacesChanged(int)));
connect(m_page->nameEdit, SIGNAL(textEdited(QString)), this, SLOT(textEdited(QString)));
@ -97,6 +97,7 @@ QWidget *WorkspaceSettings::createPage(QWidget *parent)
m_page->comboBoxTabBarPlacement->setCurrentIndex(m_tabBarPlacementIndex);
}
m_page->checkBoxAllowTabMovement->setChecked(m_allowTabBarMovement);
m_page->checkBoxRestoreSelectedOnStartup->setChecked(m_restoreSelectedOnStartup);
return w;
}
@ -122,7 +123,10 @@ void WorkspaceSettings::readSettings(QSettings *qs)
}
m_tabBarPlacementIndex = qs->value(QLatin1String("TabBarPlacementIndex"), 1).toInt(); // 1 == "Bottom"
m_allowTabBarMovement = qs->value(QLatin1String("AllowTabBarMovement"), false).toBool();
m_restoreSelectedOnStartup = qs->value(QLatin1String("RestoreSelectedOnStartup"), false).toBool();
qs->endGroup();
QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South;
emit tabBarSettingsApplied(pos, m_allowTabBarMovement);
}
@ -142,6 +146,7 @@ void WorkspaceSettings::saveSettings(QSettings *qs)
}
qs->setValue(QLatin1String("TabBarPlacementIndex"), m_tabBarPlacementIndex);
qs->setValue(QLatin1String("AllowTabBarMovement"), m_allowTabBarMovement);
qs->setValue(QLatin1String("RestoreSelectedOnStartup"), m_restoreSelectedOnStartup);
qs->endGroup();
}
@ -166,6 +171,8 @@ void WorkspaceSettings::apply()
}
m_tabBarPlacementIndex = m_page->comboBoxTabBarPlacement->currentIndex();
m_allowTabBarMovement = m_page->checkBoxAllowTabMovement->isChecked();
m_restoreSelectedOnStartup = m_page->checkBoxRestoreSelectedOnStartup->isChecked();
QTabWidget::TabPosition pos = m_tabBarPlacementIndex == 0 ? QTabWidget::North : QTabWidget::South;
emit tabBarSettingsApplied(pos, m_allowTabBarMovement);
}

5
ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.h
View File

@ -77,6 +77,10 @@ public:
{
return m_modeNames.at(i);
}
bool restoreSelectedOnStartup() const
{
return m_restoreSelectedOnStartup;
}
signals:
void tabBarSettingsApplied(QTabWidget::TabPosition pos, bool movable);
@ -98,6 +102,7 @@ private:
int m_numberOfWorkspaces;
int m_tabBarPlacementIndex;
bool m_allowTabBarMovement;
bool m_restoreSelectedOnStartup;
static const int MAX_WORKSPACES;
};
} // namespace Internal

123
ground/openpilotgcs/src/plugins/coreplugin/workspacesettings.ui
View File

@ -47,62 +47,20 @@
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Details</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Icon:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="Utils::PathChooser" name="iconPathChooser" native="true"/>
</item>
<item row="3" column="1">
<widget class="QLineEdit" name="nameEdit"/>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Name:</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Workspace panel</string>
<string>General</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="0">
<item row="1" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Placement:</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Allow reordering:</string>
</property>
</widget>
</item>
<item row="0" column="1">
<item row="1" column="1" colspan="2">
<widget class="QComboBox" name="comboBoxTabBarPlacement">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@ -125,14 +83,7 @@
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkBoxAllowTabMovement">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="2">
<item row="1" column="3">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -145,6 +96,37 @@
</property>
</spacer>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_7">
<property name="text">
<string>Allow reordering:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkBoxAllowTabMovement">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_8">
<property name="toolTip">
<string>Restore last selected workspace on startup</string>
</property>
<property name="text">
<string>Remember last used workspace on restart</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QCheckBox" name="checkBoxRestoreSelectedOnStartup">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -216,6 +198,41 @@
</property>
</spacer>
</item>
<item row="3" column="0" colspan="4">
<widget class="QGroupBox" name="groupBox_2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="title">
<string>Details</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Icon:</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="Utils::PathChooser" name="iconPathChooser" native="true"/>
</item>
<item row="3" column="1">
<widget class="QLineEdit" name="nameEdit"/>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Name:</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>

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

@ -118,19 +118,35 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// Replace the $(DATAFIELDS) tag
QString type;
QString fields;
QString dataStructures;
for (int n = 0; n < info->fields.length(); ++n) {
// Determine type
type = fieldTypeStrC[info->fields[n]->type];
// Append field
// Check if it a named set and creates structures accordingly
if (info->fields[n]->numElements > 1) {
fields.append(QString(" %1 %2[%3];\n").arg(type)
.arg(info->fields[n]->name).arg(info->fields[n]->numElements));
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
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++) {
structType.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->elementNames[f]));
}
structType.append(QString("} %1 ;\n\n").arg(structTypeName));
dataStructures.append(structType);
fields.append(QString(" %1 %2;\n").arg(structTypeName)
.arg(info->fields[n]->name));
} else {
fields.append(QString(" %1 %2[%3];\n").arg(type)
.arg(info->fields[n]->name).arg(info->fields[n]->numElements));
}
} else {
fields.append(QString(" %1 %2;\n").arg(type).arg(info->fields[n]->name));
}
}
outInclude.replace(QString("$(DATAFIELDS)"), fields);
outInclude.replace(QString("$(DATASTRUCTURES)"), dataStructures);
// Replace the $(DATAFIELDINFO) tag
QString enums;
for (int n = 0; n < info->fields.length(); ++n) {
@ -206,20 +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) {
if (info->fields[n]->type == FIELDTYPE_ENUM) {
initfields.append(QString(" data.%1[%2] = %3;\n")
if (info->fields[n]->elementNames[0].compare(QString("0")) == 0) {
initfields.append(QString(" data.%1[%2] = ")
.arg(info->fields[n]->name)
.arg(idx)
.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("%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] = %3f;\n")
.arg(info->fields[n]->name)
.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;\n")
.arg(info->fields[n]->name)
.arg(idx)
initfields.append(QString("%1;\n")
.arg(info->fields[n]->defaultValues[idx].toInt()));
}
}
@ -259,11 +280,49 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(fieldTypeStrC[info->fields[n]->type]));
setgetfields.append(QString("}\n"));
} else {
// When no struct accessor is available for a field array accessor is the default.
QString suffix = QString("");
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
// struct based field accessor
QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name);
/* SET */
setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n")
.arg(structTypeName)
.arg(info->name)
.arg(info->fields[n]->name));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n")
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
setgetfields.append(QString("}\n"));
/* GET */
setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n")
.arg(structTypeName)
.arg(info->name)
.arg(info->fields[n]->name));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n")
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
setgetfields.append(QString("}\n"));
// Append array suffix to array accessors
suffix = QString("Array");
}
// array based field accessor
/* SET */
setgetfields.append(QString("void %2%3Set( %1 *New%3 )\n")
setgetfields.append(QString("void %2%3%4Set( %1 *New%3 )\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(info->name)
.arg(info->fields[n]->name));
.arg(info->fields[n]->name)
.arg(suffix));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n")
.arg(info->name)
@ -273,10 +332,11 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
setgetfields.append(QString("}\n"));
/* GET */
setgetfields.append(QString("void %2%3Get( %1 *New%3 )\n")
setgetfields.append(QString("void %2%3%4Get( %1 *New%3 )\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(info->name)
.arg(info->fields[n]->name));
.arg(info->fields[n]->name)
.arg(suffix));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), %3*sizeof(%4));\n")
.arg(info->name)
@ -294,17 +354,37 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
for (int n = 0; n < info->fields.length(); ++n) {
// if (!info->fields[n]->defaultValues.isEmpty() )
{
QString suffix = QString("");
if (info->fields[n]->elementNames[0].compare(QString("0")) != 0) {
// struct based field accessor
QString structTypeName = QString("%1%2Data").arg(info->name).arg(info->fields[n]->name);
/* SET */
setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n")
.arg(structTypeName)
.arg(info->name)
.arg(info->fields[n]->name));
/* GET */
setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n")
.arg(structTypeName)
.arg(info->name)
.arg(info->fields[n]->name));
suffix = QString("Array");
}
/* SET */
setgetfieldsextern.append(QString("extern void %2%3Set(%1 *New%3);\n")
setgetfieldsextern.append(QString("extern void %2%3%4Set(%1 *New%3);\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(info->name)
.arg(info->fields[n]->name));
.arg(info->fields[n]->name)
.arg(suffix));
/* GET */
setgetfieldsextern.append(QString("extern void %2%3Get(%1 *New%3);\n")
setgetfieldsextern.append(QString("extern void %2%3%4Get(%1 *New%3);\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(info->name)
.arg(info->fields[n]->name));
.arg(info->fields[n]->name)
.arg(suffix));
}
}
outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern);

2
shared/uavobjectdefinition/hwsettings.xml
View File

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