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