1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00

OP-1456 get rid of cast_struct_to_array because it is error prone!

This commit is contained in:
Corvus Corax 2014-08-24 00:16:34 +02:00
parent e7aa12be94
commit 6faf1a73ac
20 changed files with 127 additions and 144 deletions

View File

@ -28,7 +28,6 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include "inc/alarms.h" #include "inc/alarms.h"
// Private constants // Private constants
@ -83,9 +82,9 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
SystemAlarmsAlarmGet(&alarms); SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME && if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity) SystemAlarmsAlarmToArray(alarms)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) { || SystemAlarmsAlarmToArray(alarms)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity; SystemAlarmsAlarmToArray(alarms)[alarm] = severity;
lastAlarmChange[alarm] = flightTime; lastAlarmChange[alarm] = flightTime;
SystemAlarmsAlarmSet(&alarms); SystemAlarmsAlarmSet(&alarms);
} }
@ -122,11 +121,11 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME && if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) { || SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status; SystemAlarmsExtendedAlarmStatusToArray(alarms.ExtendedAlarmStatus)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus; SystemAlarmsExtendedAlarmSubStatusToArray(alarms.ExtendedAlarmSubStatus)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity; SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] = severity;
lastAlarmChange[alarm] = flightTime; lastAlarmChange[alarm] = flightTime;
SystemAlarmsSet(&alarms); SystemAlarmsSet(&alarms);
} }
@ -152,7 +151,7 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
// Read alarm // Read alarm
SystemAlarmsAlarmGet(&alarms); SystemAlarmsAlarmGet(&alarms);
return cast_struct_to_array(alarms, alarms.Actuator)[alarm]; return SystemAlarmsAlarmToArray(alarms)[alarm];
} }
/** /**
@ -244,7 +243,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
// Go through alarms and check if any are of the given severity or higher // Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) { if (SystemAlarmsAlarmToArray(alarms)[n] >= severity) {
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(lock);
return 1; return 1;
} }
@ -272,8 +271,8 @@ SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
// Go through alarms and find the highest severity // Go through alarms and find the highest severity
uint32_t n = 0; uint32_t n = 0;
while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) { while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { if (SystemAlarmsAlarmToArray(alarmsData)[n] > highest) {
highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n]; highest = SystemAlarmsAlarmToArray(alarmsData)[n];
} }
n++; n++;
} }

View File

@ -25,7 +25,6 @@
*/ */
#include "inc/notification.h" #include "inc/notification.h"
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include <systemalarms.h> #include <systemalarms.h>
#include <flightstatus.h> #include <flightstatus.h>
#include <pios_notify.h> #include <pios_notify.h>

View File

@ -52,7 +52,6 @@
#include "camerastabsettings.h" #include "camerastabsettings.h"
#include "cameradesired.h" #include "cameradesired.h"
#include "hwsettings.h" #include "hwsettings.h"
#include <pios_struct_helper.h>
// //
// Configuration // Configuration
@ -172,22 +171,22 @@ static void attitudeUpdated(UAVObjEvent *ev)
// process axes // process axes
for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) { for (uint8_t i = 0; i < CAMERASTABSETTINGS_INPUT_NUMELEM; i++) {
// read and process control input // read and process control input
if (cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] != CAMERASTABSETTINGS_INPUT_NONE) { if (CameraStabSettingsInputToArray(cameraStab.Input)[i] != CAMERASTABSETTINGS_INPUT_NONE) {
if (AccessoryDesiredInstGet(cast_struct_to_array(cameraStab.Input, cameraStab.Input.Roll)[i] - if (AccessoryDesiredInstGet(CameraStabSettingsInputToArray(cameraStab.Input)[i] -
CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) { CAMERASTABSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
float input_rate; float input_rate;
switch (cast_struct_to_array(cameraStab.StabilizationMode, cameraStab.StabilizationMode.Roll)[i]) { switch (CameraStabSettingsStabilizationModeToArray(cameraStab.StabilizationMode)[i]) {
case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE: case CAMERASTABSETTINGS_STABILIZATIONMODE_ATTITUDE:
csd->inputs[i] = accessory.AccessoryVal * csd->inputs[i] = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]; CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i];
break; break;
case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK: case CAMERASTABSETTINGS_STABILIZATIONMODE_AXISLOCK:
input_rate = accessory.AccessoryVal * input_rate = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i]; CameraStabSettingsInputRateToArray(cameraStab.InputRate)[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) { if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis, csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
-cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i], -CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i],
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]); CameraStabSettingsInputRangeToArray(cameraStab.InputRange)[i]);
} }
break; break;
default: default:
@ -214,14 +213,14 @@ static void attitudeUpdated(UAVObjEvent *ev)
} }
#ifdef USE_GIMBAL_LPF #ifdef USE_GIMBAL_LPF
if (cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i]) { if (CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i]) {
float rt = (float)cast_struct_to_array(cameraStab.ResponseTime, cameraStab.ResponseTime.Roll)[i]; float rt = (float)CameraStabSettingsResponseTimeToArray(cameraStab.ResponseTime)[i];
attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis); attitude = csd->attitudeFiltered[i] = ((rt * csd->attitudeFiltered[i]) + (dT_millis * attitude)) / (rt + dT_millis);
} }
#endif #endif
#ifdef USE_GIMBAL_FF #ifdef USE_GIMBAL_FF
if (cast_struct_to_array(cameraStab.FeedForward, cameraStab.FeedForward.Roll)[i]) { if (CameraStabSettingsFeedForwardToArray(cameraStab.FeedForward)[i]) {
applyFeedForward(i, dT_millis, &attitude, &cameraStab); applyFeedForward(i, dT_millis, &attitude, &cameraStab);
} }
#endif #endif
@ -229,7 +228,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output // bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range // to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll // of both the unmixed pitch and unmixed roll
float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f); float output = boundf((attitude + csd->inputs[i]) / CameraStabSettingsOutputRangeToArray(cameraStab.OutputRange)[i], -1.0f, 1.0f);
// set output channels // set output channels
switch (i) { switch (i) {
@ -316,12 +315,12 @@ void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraSta
// apply feed forward // apply feed forward
float accumulator = csd->ffFilterAccumulator[index]; float accumulator = csd->ffFilterAccumulator[index];
accumulator += (*attitude - csd->ffLastAttitude[index]) * accumulator += (*attitude - csd->ffLastAttitude[index]) *
(float)cast_struct_to_array(cameraStab->FeedForward, cameraStab->FeedForward.Roll)[index] * gimbalTypeCorrection; (float)CameraStabSettingsFeedForwardToArray(cameraStab->FeedForward)[index] * gimbalTypeCorrection;
csd->ffLastAttitude[index] = *attitude; csd->ffLastAttitude[index] = *attitude;
*attitude += accumulator; *attitude += accumulator;
float filter = (float)((accumulator > 0.0f) ? cast_struct_to_array(cameraStab->AccelTime, cameraStab->AccelTime.Roll)[index] : float filter = (float)((accumulator > 0.0f) ? CameraStabSettingsAccelTimeToArray(cameraStab->AccelTime)[index] :
cast_struct_to_array(cameraStab->DecelTime, cameraStab->DecelTime.Roll)[index]) / dT_millis; CameraStabSettingsDecelTimeToArray(cameraStab->DecelTime)[index]) / dT_millis;
if (filter < 1.0f) { if (filter < 1.0f) {
filter = 1.0f; filter = 1.0f;
} }

View File

@ -29,7 +29,6 @@
*/ */
#include "inc/manualcontrol.h" #include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <sanitycheck.h> #include <sanitycheck.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
#include <accessorydesired.h> #include <accessorydesired.h>
@ -259,7 +258,7 @@ static bool okToArm(void)
// Check each alarm // Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set if (SystemAlarmsAlarmToArray(alarms.Alarm)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue; continue;
} }

View File

@ -29,7 +29,6 @@
*/ */
#include "inc/manualcontrol.h" #include "inc/manualcontrol.h"
#include <pios_struct_helper.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
#include <stabilizationdesired.h> #include <stabilizationdesired.h>
#include <flightmodesettings.h> #include <flightmodesettings.h>
@ -70,27 +69,27 @@ void stabilizedHandler(bool newinit)
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll); stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll); stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll); stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = cast_struct_to_array(settings.Stabilization4Settings, settings.Stabilization4Settings.Roll); stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = cast_struct_to_array(settings.Stabilization5Settings, settings.Stabilization5Settings.Roll); stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = cast_struct_to_array(settings.Stabilization6Settings, settings.Stabilization6Settings.Roll); stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
break; break;
default: default:
// Major error, this should not occur because only enter this block when one of these is true // Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll); stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
return; return;
} }

View File

@ -51,7 +51,6 @@
#include <math.h> #include <math.h>
#include <pid.h> #include <pid.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_struct_helper.h>
#include <sin_lookup.h> #include <sin_lookup.h>
#include <pathdesired.h> #include <pathdesired.h>
#include <paths.h> #include <paths.h>

View File

@ -43,7 +43,6 @@
#include "waypoint.h" #include "waypoint.h"
#include "waypointactive.h" #include "waypointactive.h"
#include "flightmodesettings.h" #include "flightmodesettings.h"
#include <pios_struct_helper.h>
#include "paths.h" #include "paths.h"
#include "plans.h" #include "plans.h"

View File

@ -33,7 +33,6 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include <accessorydesired.h> #include <accessorydesired.h>
#include <manualcontrolsettings.h> #include <manualcontrolsettings.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
@ -210,12 +209,12 @@ static void receiverTask(__attribute__((unused)) void *parameters)
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
extern uint32_t pios_rcvr_group_map[]; extern uint32_t pios_rcvr_group_map[];
if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID; cmd.Channel[n] = PIOS_RCVR_INVALID;
} else { } else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[
cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], ManualControlSettingsChannelGroupsToArray(settings.ChannelGroups)[n]],
cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); ManualControlSettingsChannelNumberToArray(settings.ChannelNumber)[n]);
} }
// If a channel has timed out this is not valid data and we shouldn't update anything // If a channel has timed out this is not valid data and we shouldn't update anything
@ -224,9 +223,9 @@ static void receiverTask(__attribute__((unused)) void *parameters)
valid_input_detected = false; valid_input_detected = false;
} else { } else {
scaledChannel[n] = scaleChannel(cmd.Channel[n], scaledChannel[n] = scaleChannel(cmd.Channel[n],
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], ManualControlSettingsChannelMaxToArray(settings.ChannelMax)[n],
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], ManualControlSettingsChannelMinToArray(settings.ChannelMin)[n],
cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); ManualControlSettingsChannelNeutralToArray(settings.ChannelNeutral)[n]);
} }
} }
@ -445,8 +444,8 @@ static void receiverTask(__attribute__((unused)) void *parameters)
if (pios_usb_rctx_id) { if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id, PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel, cmd.Channel,
cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), ManualControlSettingsChannelMinToArray(settings.ChannelMin),
cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), ManualControlSettingsChannelMaxToArray(settings.ChannelMax),
NELEMENTS(cmd.Channel)); NELEMENTS(cmd.Channel));
} }
#endif /* PIOS_INCLUDE_USB_RCTX */ #endif /* PIOS_INCLUDE_USB_RCTX */
@ -661,8 +660,8 @@ static void applyDeadband(float *value, float deadband)
*/ */
static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT)
{ {
if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) { if (ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel]) {
float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]; float rt = (float)ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel];
inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT);
*value = inputFiltered[channel]; *value = inputFiltered[channel];
} }

View File

@ -62,7 +62,6 @@
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_board_info.h> #include <pios_board_info.h>
#include <pios_struct_helper.h>
// Private constants // Private constants
#define STACK_SIZE_BYTES 1000 #define STACK_SIZE_BYTES 1000
@ -495,7 +494,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
} else { } else {
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
} }
matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform); matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
} }
/** /**
* @} * @}

View File

@ -32,7 +32,6 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h> #include <pid.h>
#include <callbackinfo.h> #include <callbackinfo.h>
#include <ratedesired.h> #include <ratedesired.h>
@ -165,21 +164,21 @@ static void stabilizationInnerloopTask()
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); bool reinit = (StabilizationStatusInnerLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; previous_mode[t] = StabilizationStatusInnerLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) { if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) { if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0; stabSettings.innerPids[t].iAccumulator = 0;
} }
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR: case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings); stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break; break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING: case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t], rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t], -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t] StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
); );
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit); stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break; break;
@ -198,8 +197,8 @@ static void stabilizationInnerloopTask()
case STABILIZATIONSTATUS_INNERLOOP_RATE: case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop) // limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t], rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t], -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t] StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t]
); );
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT); actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break; break;
@ -209,7 +208,7 @@ static void stabilizationInnerloopTask()
break; break;
} }
} else { } else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { switch (StabilizationStatusInnerLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL: case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]); actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break; break;

View File

@ -32,7 +32,6 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h> #include <pid.h>
#include <callbackinfo.h> #include <callbackinfo.h>
#include <ratedesired.h> #include <ratedesired.h>
@ -115,7 +114,7 @@ static void stabilizationOuterloopTask()
float q_error[4]; float q_error[4];
for (t = 0; t < 3; t++) { for (t = 0; t < 3; t++) {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
@ -150,14 +149,14 @@ static void stabilizationOuterloopTask()
#endif /* if defined(PIOS_QUATERNION_STABILIZATION) */ #endif /* if defined(PIOS_QUATERNION_STABILIZATION) */
} }
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]); bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t]; previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) { if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0; stabSettings.outerPids[t].iAccumulator = 0;
} }
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break; break;
@ -167,11 +166,11 @@ static void stabilizationOuterloopTask()
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]; float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t], -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t] StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
); );
// Compute the weighted average rate desired // Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed; // Using max() rather than sqrt() for cpu speed;
@ -232,7 +231,7 @@ static void stabilizationOuterloopTask()
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * cast_struct_to_array(stabSettings.stabBank.ManualRate, stabSettings.stabBank.ManualRate.Roll)[t]; float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
@ -246,7 +245,7 @@ static void stabilizationOuterloopTask()
break; break;
} }
} else { } else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) { switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
#ifdef REVOLUTION #ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit); rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);

View File

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

View File

@ -33,7 +33,6 @@
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h> #include <pid.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
#include <flightmodesettings.h> #include <flightmodesettings.h>
@ -134,54 +133,54 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
StabilizationDesiredStabilizationModeGet(&mode); StabilizationDesiredStabilizationModeGet(&mode);
for (t = 0; t < AXES; t++) { for (t = 0; t < AXES; t++) {
switch (cast_struct_to_array(mode, mode.Roll)[t]) { switch (StabilizationDesiredStabilizationModeToArray(mode)[t]) {
case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL: case STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_DIRECT;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_AXISLOCK;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RATE;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD: case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO: case STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break; break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL: case STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL:
cast_struct_to_array(status.OuterLoop, status.OuterLoop.Roll)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT; StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
cast_struct_to_array(status.InnerLoop, status.InnerLoop.Roll)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL; StabilizationStatusInnerLoopToArray(status.InnerLoop)[t] = STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL;
break; break;
} }
} }

View File

@ -32,7 +32,6 @@
#include "openpilot.h" #include "openpilot.h"
#include <pios_math.h> #include <pios_math.h>
#include <pios_struct_helper.h>
#include "stabilization.h" #include "stabilization.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
@ -78,7 +77,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
} }
// Command signal is composed of stick input added to the gyro and virtual flybar // Command signal is composed of stick input added to the gyro and virtual flybar
*output = command * cast_struct_to_array(settings->VbarSensitivity, settings->VbarSensitivity.Roll)[axis] - *output = command * StabilizationSettingsVbarSensitivityToArray(settings->VbarSensitivity)[axis] -
gyro_gain * (vbar_integral[axis] * ki + gyro * kp); gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
return 0; return 0;

View File

@ -31,7 +31,6 @@
*/ */
#include "inc/stateestimation.h" #include "inc/stateestimation.h"
#include <pios_struct_helper.h>
#include <ekfconfiguration.h> #include <ekfconfiguration.h>
#include <ekfstatevariance.h> #include <ekfstatevariance.h>
@ -165,17 +164,17 @@ static int32_t maininit(stateFilter *self)
int t; int t;
// plausibility check // plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)[t])) { if (invalid_var(EKFConfigurationPToArray(this->ekfConfiguration.P)[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.Q, this->ekfConfiguration.Q.AccelX)[t])) { if (invalid_var(EKFConfigurationQToArray(this->ekfConfiguration.Q)[t])) {
return 2; return 2;
} }
} }
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(this->ekfConfiguration.R, this->ekfConfiguration.R.BaroZ)[t])) { if (invalid_var(EKFConfigurationRToArray(this->ekfConfiguration.R)[t])) {
return 2; return 2;
} }
} }
@ -295,7 +294,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros); INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)); INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
} else { } else {
// Run prediction a bit before any corrections // Run prediction a bit before any corrections
@ -422,12 +421,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
EKFStateVarianceData vardata; EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata); EKFStateVarianceGet(&vardata);
INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); INSGetP(EKFStateVariancePToArray(vardata.P));
EKFStateVarianceSet(&vardata); EKFStateVarianceSet(&vardata);
int t; int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) { for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (!IS_REAL(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t]) || cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)[t] <= 0.0f) { if (!IS_REAL(EKFStateVariancePToArray(vardata.P)[t]) || EKFStateVariancePToArray(vardata.P)[t] <= 0.0f) {
INSResetP(cast_struct_to_array(this->ekfConfiguration.P, this->ekfConfiguration.P.AttitudeQ1)); INSResetP(EKFConfigurationPToArray(this->ekfConfiguration.P));
this->init_stage = -1; this->init_stage = -1;
break; break;
} }

View File

@ -39,7 +39,6 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_struct_helper.h>
// private includes // private includes
#include "inc/systemmod.h" #include "inc/systemmod.h"
#include "notification.h" #include "notification.h"
@ -420,9 +419,9 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
// By convention, there is a direct mapping between task monitor task_id's and members // By convention, there is a direct mapping between task monitor task_id's and members
// of the TaskInfoXXXXElem enums // of the TaskInfoXXXXElem enums
PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM); PIOS_DEBUG_Assert(task_id < TASKINFO_RUNNING_NUMELEM);
cast_struct_to_array(taskData->Running, taskData->Running.System)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE; TaskInfoRunningToArray(taskData->Running)[task_id] = task_info->is_running ? TASKINFO_RUNNING_TRUE : TASKINFO_RUNNING_FALSE;
((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining; ((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage; ((uint8_t *)&taskData->RunningTime)[task_id] = task_info->running_time_percentage;
} }
static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context) static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)

View File

@ -51,7 +51,6 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <pios_struct_helper.h>
#include "txpidsettings.h" #include "txpidsettings.h"
#include "accessorydesired.h" #include "accessorydesired.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
@ -195,26 +194,26 @@ static void updatePIDs(UAVObjEvent *ev)
// Loop through every enabled instance // Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) { if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value; float value;
if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value); ManualControlCommandThrottleGet(&value);
value = scale(value, value = scale(value,
inst.ThrottleRange.Min, inst.ThrottleRange.Min,
inst.ThrottleRange.Max, inst.ThrottleRange.Max,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i], TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]); TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else if (AccessoryDesiredInstGet( } else if (AccessoryDesiredInstGet(
cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) { &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f, value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i], TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]); TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else { } else {
continue; continue;
} }
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) { switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP: case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value); needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break; break;

View File

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

View File

@ -51,6 +51,11 @@ typedef void *UAVObjHandle;
#define MetaObjectId(id) ((id) + 1) #define MetaObjectId(id) ((id) + 1)
#define UAVObjectFieldToArray(type, var) \
({ type *const dummy = &(var); \
(((type##Array *)dummy)->array); } \
)
/** /**
* Object update mode, used by multiple modules (e.g. telemetry and logger) * Object update mode, used by multiple modules (e.g. telemetry and logger)
*/ */

View File

@ -131,7 +131,11 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
for (int f = 0; f < info->fields[n]->elementNames.count(); f++) { 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 %2;\n").arg(type).arg(info->fields[n]->elementNames[f]));
} }
structType.append(QString("} %1 ;\n\n").arg(structTypeName)); structType.append(QString("} %1 ;\n").arg(structTypeName));
structType.append(QString("typedef struct __attribute__ ((__packed__)) {\n"));
structType.append(QString(" %1 array[%2];\n").arg(type).arg(info->fields[n]->elementNames.count()));
structType.append(QString("} %1Array ;\n").arg(structTypeName));
structType.append(QString("#define %1%2ToArray( var ) UAVObjectFieldToArray( %3, var )\n\n").arg(info->name).arg(info->fields[n]->name).arg(structTypeName));
dataStructures.append(structType); dataStructures.append(structType);