flight/modules/Sensors/sensors.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup Sensors
   6  * @brief Acquires sensor data
   7  * @{
   8  *
   9  * @file       sensors.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  12  * @brief      Module to handle fetch and preprocessing of sensor data
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  ******************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 /**
  34  * Input objects: None, takes sensor data via pios
  35  * Output objects: @ref GyroSensor @ref AccelSensor @ref MagSensor
  36  *
  37  * The module executes in its own thread.
  38  *
  39  * UAVObjects are automatically generated by the UAVObjectGenerator from
  40  * the object definition XML file.
  41  *
  42  * Modules have no API, all communication to other modules is done through UAVObjects.
  43  * However modules may use the API exposed by shared libraries.
  44  * See the OpenPilot wiki for more details.
  45  * http://www.openpilot.org/OpenPilot_Application_Architecture
  46  *
  47  */
  48
  49 #include <openpilot.h>
  50 #include <pios_sensors.h>
  51 #include <homelocation.h>
  52
  53 #include <magsensor.h>
  54 #include <accelsensor.h>
  55 #include <gyrosensor.h>
  56 #include <barosensor.h>
  57 #include <flightstatus.h>
  58
  59 #include <attitudesettings.h>
  60 #include <revocalibration.h>
  61 #include <auxmagsettings.h>
  62 #include <auxmagsensor.h>
  63 #include <auxmagsupport.h>
  64 #include <accelgyrosettings.h>
  65 #include <revosettings.h>
  66 #include <UBX.h>
  67
  68 #include <mathmisc.h>
  69 #include <taskinfo.h>
  70 #include <pios_math.h>
  71 #include <pios_constants.h>
  72 #include <CoordinateConversions.h>
  73 #include <pios_board_info.h>
  74 #include <string.h>
  75
  76 // Private constants
  77 #define STACK_SIZE_BYTES         1000
  78 #define TASK_PRIORITY            (tskIDLE_PRIORITY + 3)
  79
  80 #define MAX_SENSORS_PER_INSTANCE 2
  81 #ifdef PIOS_INCLUDE_WDG
  82 #define RELOAD_WDG()   PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS)
  83 #define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS)
  84 #else
  85 #define RELOAD_WDG()
  86 #define REGISTER_WDG()
  87 #endif
  88
  89 static const TickType_t sensor_period_ticks = ((uint32_t)(1000.0f / PIOS_SENSOR_RATE / (float)portTICK_RATE_MS));
  90
  91 // Interval in number of sample to recalculate temp bias
  92 #define TEMP_CALIB_INTERVAL      30
  93
  94 // LPF
  95 #define TEMP_DT_GYRO_ACCEL       (1.0f / PIOS_SENSOR_RATE)
  96 #define TEMP_LPF_FC_GYRO_ACCEL   5.0f
  97 static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
  98
  99 // Interval in number of sample to recalculate temp bias
 100 #define BARO_TEMP_CALIB_INTERVAL 10
 101
 102 // LPF
 103 #define TEMP_DT_BARO             (1.0f / 120.0f)
 104 #define TEMP_LPF_FC_BARO         5.0f
 105 static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
 106
 107 #define ZERO_ROT_ANGLE           0.00001f
 108
 109 // Private types
 110 typedef struct {
 111     // used to accumulate all samples in a task iteration
 112     uint64_t   timestamp;      // sum of "PIOS_DELAY_GetRaw() times of sensor read" in this averaged set
 113     Vector3i32 accum[2]; // summed 16 bit sensor values in this averaged set
 114     int32_t    temperature;    // sum of 16 bit temperatures in this averaged set
 115     uint32_t   prev_timestamp; // to detect timer wrap around
 116     uint16_t   count;          // number of sensor reads in this averaged set
 117 } sensor_fetch_context;
 118
 119 #define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
 120 typedef union {
 121     PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis;
 122     PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis;
 123 } sensor_data;
 124
 125 #define PIOS_INSTRUMENT_MODULE
 126 #include <pios_instrumentation_helper.h>
 127
 128 PERF_DEFINE_COUNTER(counterAccelSamples);
 129 PERF_DEFINE_COUNTER(counterAccelPeriod);
 130 PERF_DEFINE_COUNTER(counterMagPeriod);
 131 PERF_DEFINE_COUNTER(counterBaroPeriod);
 132 PERF_DEFINE_COUNTER(counterSensorPeriod);
 133 PERF_DEFINE_COUNTER(counterSensorResets);
 134
 135 #if defined(PIOS_INCLUDE_HMC5X83)
 136 void aux_hmc5x83_load_settings();
 137 #endif
 138
 139 // Private functions
 140 static void SensorsTask(void *parameters);
 141 static void settingsUpdatedCb(UAVObjEvent *objEv);
 142
 143 static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
 144 static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor);
 145 static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor);
 146
 147 static void clearContext(sensor_fetch_context *sensor_context);
 148
 149 static void handleAccel(float *samples, float temperature);
 150 static void handleGyro(float *samples, float temperature, uint32_t timestamp);
 151 static void handleMag(float *samples, float temperature);
 152 #if defined(PIOS_INCLUDE_HMC5X83)
 153 static void handleAuxMag(float *samples);
 154 #endif
 155 static void handleBaro(float sample, float temperature);
 156
 157 static void updateAccelTempBias(float temperature);
 158 static void updateGyroTempBias(float temperature);
 159 static void updateBaroTempBias(float temperature);
 160
 161 // Private variables
 162 static sensor_data *source_data;
 163 static xTaskHandle sensorsTaskHandle;
 164 RevoCalibrationData cal;
 165 AccelGyroSettingsData agcal;
 166
 167 // These values are initialized by settings but can be updated by the attitude algorithm
 168 static float mag_bias[3] = { 0, 0, 0 };
 169 static float mag_transform[3][3] = {
 170     { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
 171 };
 172
 173 // Variables used to handle accel/gyro temperature bias
 174 static volatile bool gyro_temp_calibrated  = false;
 175 static volatile bool accel_temp_calibrated = false;
 176
 177 static float accel_temperature  = NAN;
 178 static float gyro_temperature   = NAN;
 179 static float accel_temp_bias[3] = { 0 };
 180 static float gyro_temp_bias[3] = { 0 };
 181 static uint8_t accel_temp_calibration_count = 0;
 182 static uint8_t gyro_temp_calibration_count  = 0;
 183
 184 // The user specified "Rotate virtual attitude relative to board"
 185 static float R[3][3] = {
 186     { 0 }
 187 };
 188
 189 // Variables used to handle baro temperature bias
 190 static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
 191 static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
 192 static volatile bool baro_temp_correction_enabled;
 193 static float baro_temp_bias   = 0;
 194 static float baro_temperature = NAN;
 195 static uint8_t baro_temp_calibration_count = 0;
 196
 197 #if defined(PIOS_INCLUDE_HMC5X83)
 198 // Allow AuxMag to be disabled without reboot
 199 // because the other mags are that way
 200 static bool useAuxMag = false;
 201 #endif
 202
 203 /**
 204  * Initialise the module.  Called before the start function
 205  * \returns 0 on success or -1 if initialisation failed
 206  */
 207 int32_t SensorsInitialize(void)
 208 {
 209     source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
 210     GyroSensorInitialize();
 211     AccelSensorInitialize();
 212     MagSensorInitialize();
 213     BaroSensorInitialize();
 214     RevoCalibrationInitialize();
 215     RevoSettingsInitialize();
 216     AttitudeSettingsInitialize();
 217     AccelGyroSettingsInitialize();
 218
 219 #if defined(PIOS_INCLUDE_HMC5X83)
 220     // for auxmagsupport.c helpers
 221     AuxMagSettingsInitialize();
 222     AuxMagSensorInitialize();
 223 #endif
 224
 225     RevoSettingsConnectCallback(&settingsUpdatedCb);
 226     RevoCalibrationConnectCallback(&settingsUpdatedCb);
 227     AttitudeSettingsConnectCallback(&settingsUpdatedCb);
 228     AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
 229
 230     return 0;
 231 }
 232
 233 /**
 234  * Start the task.  Expects all objects to be initialized by this point.
 235  * \returns 0 on success or -1 if initialisation failed
 236  */
 237 int32_t SensorsStart(void)
 238 {
 239     // Start main task
 240     xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
 241     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
 242     REGISTER_WDG();
 243     return 0;
 244 }
 245
 246 MODULE_INITCALL(SensorsInitialize, SensorsStart);
 247
 248 int32_t accel_test;
 249 int32_t gyro_test;
 250 int32_t mag_test;
 251 // int32_t pressure_test;
 252
 253
 254 /**
 255  * The sensor task.  This polls the gyros at 500 Hz and pumps that data to
 256  * stabilization and to the attitude loop
 257  *
 258  */
 259 static void SensorsTask(__attribute__((unused)) void *parameters)
 260 {
 261     portTickType lastSysTime;
 262     sensor_fetch_context sensor_context;
 263     bool error = false;
 264     const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
 265     PIOS_SENSORS_Instance *sensor;
 266
 267     AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
 268     settingsUpdatedCb(NULL);
 269
 270     // Performance counters
 271     PERF_INIT_COUNTER(counterAccelSamples, 0x53000001);
 272     PERF_INIT_COUNTER(counterAccelPeriod, 0x53000002);
 273     PERF_INIT_COUNTER(counterMagPeriod, 0x53000003);
 274     PERF_INIT_COUNTER(counterBaroPeriod, 0x53000004);
 275     PERF_INIT_COUNTER(counterSensorPeriod, 0x53000005);
 276     PERF_INIT_COUNTER(counterSensorResets, 0x53000006);
 277
 278     // Test sensors
 279     bool sensors_test = true;
 280     uint8_t count     = 0;
 281     LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
 282         RELOAD_WDG(); // mag tests on I2C have 200+(7x10)ms delay calls in them
 283         sensors_test &= PIOS_SENSORS_Test(sensor);
 284         count++;
 285     }
 286
 287     PIOS_Assert(count);
 288     RELOAD_WDG();
 289     if (!sensors_test) {
 290         AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
 291         while (1) {
 292             vTaskDelay(10);
 293         }
 294     }
 295
 296     // Main task loop
 297     lastSysTime = xTaskGetTickCount();
 298     uint32_t reset_counter = 0;
 299
 300     while (1) {
 301         // TODO: add timeouts to the sensor reads and set an error if the fail
 302         if (error) {
 303             RELOAD_WDG();
 304             lastSysTime = xTaskGetTickCount();
 305             vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
 306             AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
 307             error = false;
 308         } else {
 309             AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
 310         }
 311
 312
 313         // reset the fetch context
 314         clearContext(&sensor_context);
 315         LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
 316             // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
 317             bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
 318
 319             if (!sensor->driver->is_polled) {
 320                 const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
 321                 while (xQueueReceive(queue,
 322                                      (void *)source_data,
 323                                      (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
 324                     accumulateSamples(&sensor_context, source_data);
 325                 }
 326                 if (sensor_context.count) {
 327                     processSamples3d(&sensor_context, sensor);
 328                     clearContext(&sensor_context);
 329                 } else if (is_primary) {
 330                     PIOS_SENSOR_Reset(sensor);
 331                     reset_counter++;
 332                     PERF_TRACK_VALUE(counterSensorResets, reset_counter);
 333                     error = true;
 334                 }
 335             } else {
 336                 if (PIOS_SENSORS_Poll(sensor)) {
 337                     PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
 338                     if (sensor->type & PIOS_SENSORS_TYPE_3D) {
 339                         accumulateSamples(&sensor_context, source_data);
 340                         processSamples3d(&sensor_context, sensor);
 341                     } else {
 342                         processSamples1d(&source_data->sensorSample1Axis, sensor);
 343                     }
 344                     clearContext(&sensor_context);
 345                 }
 346             }
 347         }
 348         PERF_MEASURE_PERIOD(counterSensorPeriod);
 349         RELOAD_WDG();
 350         vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
 351     }
 352 }
 353
 354 static void clearContext(sensor_fetch_context *sensor_context)
 355 {
 356     // clear the context once it has finished
 357     for (uint32_t i = 0; i < MAX_SENSORS_PER_INSTANCE; i++) {
 358         sensor_context->accum[i].x = 0;
 359         sensor_context->accum[i].y = 0;
 360         sensor_context->accum[i].z = 0;
 361     }
 362     sensor_context->temperature    = 0;
 363     sensor_context->prev_timestamp = 0;
 364     sensor_context->timestamp = 0LL;
 365     sensor_context->count     = 0;
 366 }
 367
 368 static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample)
 369 {
 370     for (uint32_t i = 0; (i < MAX_SENSORS_PER_INSTANCE) && (i < sample->sensorSample3Axis.count); i++) {
 371         sensor_context->accum[i].x += sample->sensorSample3Axis.sample[i].x;
 372         sensor_context->accum[i].y += sample->sensorSample3Axis.sample[i].y;
 373         sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z;
 374     }
 375     sensor_context->temperature += sample->sensorSample3Axis.temperature;
 376     sensor_context->timestamp   += sample->sensorSample3Axis.timestamp;
 377     if (sensor_context->prev_timestamp > sample->sensorSample3Axis.timestamp) {
 378         // we've wrapped so add the dropped top bit
 379         // this makes the average come out correct instead of (0xfd+0x01)/2 = 0x7f or such
 380         sensor_context->timestamp += 0x100000000LL;
 381     } else {
 382         sensor_context->prev_timestamp = sample->sensorSample3Axis.timestamp;
 383     }
 384     sensor_context->count++;
 385 }
 386
 387 static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor)
 388 {
 389     float samples[3];
 390     float temperature;
 391     float scales[MAX_SENSORS_PER_INSTANCE];
 392
 393     PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
 394     float inv_count = 1.0f / (float)sensor_context->count;
 395     if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL)
 396         || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)
 397 #if defined(PIOS_INCLUDE_HMC5X83)
 398         || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)
 399 #endif
 400         ) {
 401         float t = inv_count * scales[0];
 402         samples[0]  = ((float)sensor_context->accum[0].x * t);
 403         samples[1]  = ((float)sensor_context->accum[0].y * t);
 404         samples[2]  = ((float)sensor_context->accum[0].z * t);
 405         temperature = (float)sensor_context->temperature * inv_count * 0.01f;
 406         switch (sensor->type) {
 407         case PIOS_SENSORS_TYPE_3AXIS_MAG:
 408             handleMag(samples, temperature);
 409             PERF_MEASURE_PERIOD(counterMagPeriod);
 410             return;
 411
 412 #if defined(PIOS_INCLUDE_HMC5X83)
 413         case PIOS_SENSORS_TYPE_3AXIS_AUXMAG:
 414             handleAuxMag(samples);
 415             PERF_MEASURE_PERIOD(counterMagPeriod);
 416             return;
 417
 418 #endif
 419         default:
 420             PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
 421             PERF_MEASURE_PERIOD(counterAccelPeriod);
 422             handleAccel(samples, temperature);
 423             break;
 424         }
 425     }
 426
 427     if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) {
 428         uint8_t index = 0;
 429         uint32_t timestamp;
 430         if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
 431             index = 1;
 432         }
 433         float t = inv_count * scales[index];
 434         samples[0]  = ((float)sensor_context->accum[index].x * t);
 435         samples[1]  = ((float)sensor_context->accum[index].y * t);
 436         samples[2]  = ((float)sensor_context->accum[index].z * t);
 437         temperature = (float)sensor_context->temperature * inv_count * 0.01f;
 438         timestamp   = (uint32_t)(sensor_context->timestamp / sensor_context->count);
 439         handleGyro(samples, temperature, timestamp);
 440         return;
 441     }
 442 }
 443
 444 static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor)
 445 {
 446     switch (sensor->type) {
 447     case PIOS_SENSORS_TYPE_1AXIS_BARO:
 448         PERF_MEASURE_PERIOD(counterBaroPeriod);
 449         handleBaro(sample->sample, sample->temperature);
 450         return;
 451
 452     default:
 453         PIOS_Assert(0);
 454     }
 455 }
 456
 457 static void handleAccel(float *samples, float temperature)
 458 {
 459     AccelSensorData accelSensorData;
 460
 461     updateAccelTempBias(temperature);
 462     float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0],
 463                             (samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1],
 464                             (samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] };
 465
 466     rot_mult(R, accels_out, samples);
 467     accelSensorData.x = samples[0];
 468     accelSensorData.y = samples[1];
 469     accelSensorData.z = samples[2];
 470     accelSensorData.temperature = temperature;
 471
 472     AccelSensorSet(&accelSensorData);
 473 }
 474
 475 static void handleGyro(float *samples, float temperature, uint32_t timestamp)
 476 {
 477     GyroSensorData gyroSensorData;
 478
 479     updateGyroTempBias(temperature);
 480     float gyros_out[3] = { samples[0] * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
 481                            samples[1] * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
 482                            samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
 483
 484     rot_mult(R, gyros_out, samples);
 485     gyroSensorData.x = samples[0];
 486     gyroSensorData.y = samples[1];
 487     gyroSensorData.z = samples[2];
 488     gyroSensorData.temperature = temperature;
 489     gyroSensorData.SensorReadTimestamp = timestamp;
 490
 491     GyroSensorSet(&gyroSensorData);
 492 }
 493
 494 static void handleMag(float *samples, float temperature)
 495 {
 496     MagSensorData mag;
 497     float mags[3] = { (float)samples[0] - mag_bias[0],
 498                       (float)samples[1] - mag_bias[1],
 499                       (float)samples[2] - mag_bias[2] };
 500
 501     rot_mult(mag_transform, mags, samples);
 502
 503     mag.x = samples[0];
 504     mag.y = samples[1];
 505     mag.z = samples[2];
 506     mag.temperature = temperature;
 507
 508     MagSensorSet(&mag);
 509 }
 510
 511 #if defined(PIOS_INCLUDE_HMC5X83)
 512 static void handleAuxMag(float *samples)
 513 {
 514     if (useAuxMag) {
 515         auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
 516     }
 517 }
 518 #endif
 519
 520 static void handleBaro(float sample, float temperature)
 521 {
 522     updateBaroTempBias(temperature);
 523     sample -= baro_temp_bias;
 524
 525     float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
 526
 527     if (!isnan(altitude)) {
 528         BaroSensorData data;
 529         data.Altitude    = altitude;
 530         data.Temperature = temperature;
 531         data.Pressure    = sample;
 532         // Update the BasoSensor UAVObject
 533         BaroSensorSet(&data);
 534     }
 535 }
 536
 537 static void updateAccelTempBias(float temperature)
 538 {
 539     if (isnan(accel_temperature)) {
 540         accel_temperature = temperature;
 541     }
 542     accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
 543
 544     if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
 545         accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
 546         if (accel_temp_calibrated) {
 547             float ctemp = boundf(accel_temperature,
 548                                  agcal.temp_calibrated_extent.max,
 549                                  agcal.temp_calibrated_extent.min);
 550
 551             accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
 552             accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
 553             accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
 554         }
 555     }
 556     accel_temp_calibration_count--;
 557 }
 558
 559 static void updateGyroTempBias(float temperature)
 560 {
 561     if (isnan(gyro_temperature)) {
 562         gyro_temperature = temperature;
 563     }
 564
 565     gyro_temperature = temp_alpha_gyro_accel * (temperature - gyro_temperature) + gyro_temperature;
 566
 567     if (gyro_temp_calibrated && !gyro_temp_calibration_count) {
 568         gyro_temp_calibration_count = TEMP_CALIB_INTERVAL;
 569
 570         if (gyro_temp_calibrated) {
 571             float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
 572             gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
 573             gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
 574             gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
 575         }
 576     }
 577     gyro_temp_calibration_count--;
 578 }
 579
 580 static void updateBaroTempBias(float temperature)
 581 {
 582     if (isnan(baro_temperature)) {
 583         baro_temperature = temperature;
 584     }
 585
 586     baro_temperature = temp_alpha_baro * (temperature - baro_temperature) + baro_temperature;
 587
 588     if (baro_temp_correction_enabled && !baro_temp_calibration_count) {
 589         baro_temp_calibration_count = BARO_TEMP_CALIB_INTERVAL;
 590         // pressure bias = A + B*t + C*t^2 + D * t^3
 591         // in case the temperature is outside of the calibrated range, uses the nearest extremes
 592         float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
 593         baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
 594     }
 595     baro_temp_calibration_count--;
 596 }
 597
 598 /**
 599  * Locally cache some variables from the AttitudeSettings object
 600  */
 601 static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 602 {
 603     RevoCalibrationGet(&cal);
 604     mag_bias[0] = cal.mag_bias.X;
 605     mag_bias[1] = cal.mag_bias.Y;
 606     mag_bias[2] = cal.mag_bias.Z;
 607
 608     AccelGyroSettingsGet(&agcal);
 609     accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
 610                             (fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
 611     gyro_temp_calibrated  = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
 612                             (fabsf(agcal.gyro_temp_coeff.X) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Y) > 1e-9f ||
 613                             fabsf(agcal.gyro_temp_coeff.Z) > 1e-9f || fabsf(agcal.gyro_temp_coeff.Z2) > 1e-9f);
 614
 615     // convert BoardRotation ("rotate virtual") into a quaternion
 616     AttitudeSettingsData attitudeSettings;
 617     AttitudeSettingsGet(&attitudeSettings);
 618     const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
 619                            attitudeSettings.BoardRotation.Pitch,
 620                            attitudeSettings.BoardRotation.Yaw };
 621     float rotationQuat[4];
 622     RPY2Quaternion(rpy, rotationQuat);
 623
 624     // convert BoardLevelTrim ("board level calibration") into a quaternion
 625     float trimQuat[4];
 626     float sumQuat[4];
 627     const float trimRpy[3] = { attitudeSettings.BoardLevelTrim.Roll, attitudeSettings.BoardLevelTrim.Pitch, 0.0f };
 628     // do we actually want to include BoardLevelTrim in the mag rotation?  BoardRotation yes, but BoardLevelTrim?
 629     // and is BoardLevelTrim done at the correct point in the sequence of rotations?
 630     RPY2Quaternion(trimRpy, trimQuat);
 631
 632     // add the boardrotation and boardtrim rotations and put them into a rotation matrix
 633     quat_mult(rotationQuat, trimQuat, sumQuat);
 634     Quaternion2R(sumQuat, R);
 635
 636     // mag_transform is only a scaling
 637     // so add the scaling, and store the result in mag_transform for run time use
 638     matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
 639
 640     RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
 641     RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
 642     baro_temp_correction_enabled =
 643         (baroCorrectionExtent.max - baroCorrectionExtent.min > 0.1f &&
 644          (fabsf(baroCorrection.a) > 1e-9f ||
 645           fabsf(baroCorrection.b) > 1e-9f ||
 646           fabsf(baroCorrection.c) > 1e-9f ||
 647           fabsf(baroCorrection.d) > 1e-9f));
 648 }
 649
 650 #if defined(PIOS_INCLUDE_HMC5X83)
 651 void aux_hmc5x83_load_mag_settings()
 652 {
 653     uint8_t magType = auxmagsupport_get_type();
 654
 655     if (magType == AUXMAGSETTINGS_TYPE_I2C || magType == AUXMAGSETTINGS_TYPE_FLEXI) {
 656         useAuxMag = true;
 657     } else {
 658         useAuxMag = false;
 659     }
 660 }
 661 #endif
 662 /**
 663  * @}
 664  * @}
 665  */