From f64a1ba822fe3826c5bb6c843a16f6482e84c08d Mon Sep 17 00:00:00 2001 From: Vladimir Zidar Date: Fri, 23 Feb 2018 13:16:20 +0100 Subject: [PATCH] LP-580 Change PIOS_INCLUDE_HMC5X83 to PIOS_INCLUDE_SENSORS_AUXMAG where it applies to general auxmag support. --- flight/modules/GPS/GPS.c | 9 ++++++--- flight/modules/GPS/inc/UBX.h | 1 - flight/modules/Sensors/inc/sensors.h | 1 + flight/modules/Sensors/sensors.c | 21 +++++++++------------ flight/modules/StateEstimation/filtercf.c | 6 ------ flight/pios/common/pios_board_sensors.c | 4 ++-- flight/pios/pios.h | 4 ++++ 7 files changed, 22 insertions(+), 24 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index fd7eb9a05..e19d1e01b 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -51,6 +51,9 @@ #include "NMEA.h" #include "UBX.h" #include "DJI.h" +#ifdef PIOS_INCLUDE_SENSORS_AUXMAG +#include "sensors.h" +#endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #include "inc/ubx_autoconfig.h" #define FULL_UBX_PARSER @@ -72,7 +75,7 @@ PERF_DEFINE_COUNTER(counterParse); #if defined(ANY_GPS_PARSER) && !defined(PIOS_GPS_MINIMAL) #define ANY_FULL_GPS_PARSER #endif -#if (defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_SENSORS_AUXMAG) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #define ANY_FULL_MAG_PARSER #endif @@ -653,8 +656,8 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) #if defined(PIOS_INCLUDE_GPS_DJI_PARSER) dji_load_mag_settings(); #endif -#if defined(PIOS_INCLUDE_HMC5X83) - aux_hmc5x83_load_mag_settings(); +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) + sensors_auxmag_load_mag_settings(); #endif } #endif /* defined(ANY_FULL_MAG_PARSER) */ diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index f622ea87a..1583f188e 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -677,6 +677,5 @@ uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void op_gpsv9_load_mag_settings(); -void aux_hmc5x83_load_mag_settings(); #endif /* UBX_H */ diff --git a/flight/modules/Sensors/inc/sensors.h b/flight/modules/Sensors/inc/sensors.h index 4676ef8eb..1c98c2732 100644 --- a/flight/modules/Sensors/inc/sensors.h +++ b/flight/modules/Sensors/inc/sensors.h @@ -34,5 +34,6 @@ #include "openpilot.h" int32_t SensorsInitialize(void); +void sensors_auxmag_load_mag_settings(void); #endif // SENSORS_H diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 2d16307e4..76e2b895b 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -71,6 +71,7 @@ #include #include #include +#include "sensors.h" // Private constants #define STACK_SIZE_BYTES 1000 @@ -131,10 +132,6 @@ PERF_DEFINE_COUNTER(counterBaroPeriod); PERF_DEFINE_COUNTER(counterSensorPeriod); PERF_DEFINE_COUNTER(counterSensorResets); -#if defined(PIOS_INCLUDE_HMC5X83) -void aux_hmc5x83_load_settings(); -#endif - // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -148,7 +145,7 @@ static void clearContext(sensor_fetch_context *sensor_context); static void handleAccel(float *samples, float temperature); static void handleGyro(float *samples, float temperature, uint32_t timestamp); static void handleMag(float *samples, float temperature); -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) static void handleAuxMag(float *samples); #endif static void handleBaro(float sample, float temperature); @@ -193,7 +190,7 @@ static float baro_temp_bias = 0; static float baro_temperature = NAN; static uint8_t baro_temp_calibration_count = 0; -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) // Allow AuxMag to be disabled without reboot // because the other mags are that way static bool useAuxMag = false; @@ -211,7 +208,7 @@ int32_t SensorsInitialize(void) MagSensorInitialize(); BaroSensorInitialize(); -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) // for auxmagsupport.c helpers AuxMagSensorInitialize(); #endif @@ -388,7 +385,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE float inv_count = 1.0f / (float)sensor_context->count; if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG) #endif ) { @@ -403,7 +400,7 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE PERF_MEASURE_PERIOD(counterMagPeriod); return; -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) case PIOS_SENSORS_TYPE_3AXIS_AUXMAG: handleAuxMag(samples); PERF_MEASURE_PERIOD(counterMagPeriod); @@ -502,7 +499,7 @@ static void handleMag(float *samples, float temperature) MagSensorSet(&mag); } -#if defined(PIOS_INCLUDE_HMC5X83) +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) static void handleAuxMag(float *samples) { if (useAuxMag) { @@ -641,8 +638,8 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) fabsf(baroCorrection.d) > 1e-9f)); } -#if defined(PIOS_INCLUDE_HMC5X83) -void aux_hmc5x83_load_mag_settings() +#if defined(PIOS_INCLUDE_SENSORS_AUXMAG) +void sensors_auxmag_load_mag_settings() { uint8_t magType = auxmagsupport_get_type(); diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 1bad774d0..e4a65e1db 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -228,16 +228,10 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // During initialization and if (this->first_run) { -#if defined(PIOS_INCLUDE_HMC5X83) // wait until mags have been updated if (!this->magUpdated && this->useMag) { return FILTERRESULT_ERROR; } -#else - mag[0] = 100.0f; - mag[1] = 0.0f; - mag[2] = 0.0f; -#endif pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE); pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE); diff --git a/flight/pios/common/pios_board_sensors.c b/flight/pios/common/pios_board_sensors.c index 29b865cfa..43c7226e9 100644 --- a/flight/pios/common/pios_board_sensors.c +++ b/flight/pios/common/pios_board_sensors.c @@ -150,7 +150,7 @@ void PIOS_BOARD_Sensors_Configure() # endif /* PIOS_INCLUDE_HMC5X83_INTERNAL */ -# if defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_QMC5883L) +# ifdef PIOS_INCLUDE_SENSORS_AUXMAG AuxMagSettingsInitialize(); AuxMagSettingsTypeOptions option; @@ -197,7 +197,7 @@ void PIOS_BOARD_Sensors_Configure() AlarmsSet(SYSTEMALARMS_ALARM_I2C, (qmc5883l_dev) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING); # endif /* PIOS_INCLUDE_QMC5883L */ } -# endif /* PIOS_INCLUDE_HMC5X83 || PIOS_INCLUDE_QMC5883L */ +# endif /* PIOS_INCLUDE_SENSORS_AUXMAG */ // internal ms5611 baro #ifdef PIOS_INCLUDE_MS56XX diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 8d37b0852..83228b329 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -339,6 +339,10 @@ extern "C" { /* Performance counters */ /* #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 */ +#if defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_QMC5883L) +#define PIOS_INCLUDE_SENSORS_AUXMAG +#endif + #endif /* USE_SIM_POSIX */