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

OP-1658 - Implementation of Sensor Module using PiOS Sensors Framework

Browse Source
This commit is contained in:
Alessio Morale 2014-12-23 19:14:31 +01:00
parent 84f577029e
commit cf791db71e
3 changed files with 249 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

536
flight/modules/Sensors/sensors.c
View File

@ -47,12 +47,11 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_sensors.h>
#include <homelocation.h> #include <homelocation.h>
#include <magsensor.h> #include <magsensor.h>
#include <accelsensor.h> #include <accelsensor.h>
#include <gyrosensor.h> #include <gyrosensor.h>
#include <attitudestate.h>
#include <attitudesettings.h> #include <attitudesettings.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <accelgyrosettings.h> #include <accelgyrosettings.h>
@ -60,14 +59,23 @@
#include <taskinfo.h> #include <taskinfo.h>
#include <pios_math.h> #include <pios_math.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_board_info.h> #include <pios_board_info.h>
#include <string.h>
// Private constants // Private constants
#define STACK_SIZE_BYTES 1000 #define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); #define MAX_SENSORS_PER_INSTANCE 2
#ifdef PIOS_INCLUDE_WDG
#define RELOAD_WDG() PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS)
#define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS)
#else
#define RELOAD_WDG()
#define REGISTER_WDG()
#endif
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
static const uint32_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
// Interval in number of sample to recalculate temp bias // Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30 #define TEMP_CALIB_INTERVAL 30
@ -75,41 +83,55 @@ static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
// LPF // LPF
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE) #define TEMP_DT (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f #define TEMP_LPF_FC 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); static const float temp_alpha = LPF_ALPHA(TEMP_DT, TEMP_LPF_FC);
#define ZERO_ROT_ANGLE 0.00001f #define ZERO_ROT_ANGLE 0.00001f
// Private types // Private types
typedef struct {
// used to accumulate all samples in a task iteration
Vector3i32 accum[2];
int32_t temperature;
uint32_t count;
} sensor_fetch_context;
#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
typedef union {
PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis;
PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis;
} sensor_data;
#define PIOS_INSTRUMENT_MODULE #define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h> #include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples);
PERF_DEFINE_COUNTER(counterSensorPeriod);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
// Private functions // Private functions
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv); static void settingsUpdatedCb(UAVObjEvent *objEv);
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor);
static void clearContext(sensor_fetch_context *sensor_context);
static void handleAccel(float *samples, float temperature);
static void handleGyro(float *samples, float temperature);
static void handleMag(float *samples, float temperature);
static void updateAccelTempBias(float temperature);
static void updateGyroTempBias(float temperature);
// Private variables // Private variables
static sensor_data *source_data;
static xTaskHandle sensorsTaskHandle; static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal; RevoCalibrationData cal;
AccelGyroSettingsData agcal; AccelGyroSettingsData agcal;
#ifdef PIOS_INCLUDE_HMC5X83
#include <pios_hmc5x83.h>
extern pios_hmc5x83_dev_t onboard_mag;
#endif
// These values are initialized by settings but can be updated by the attitude algorithm // These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 }; static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = { static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
}; };
// temp coefficient to calculate gyro bias
// Variables used to handle temperature bias
static volatile bool gyro_temp_calibrated = false; static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false; static volatile bool accel_temp_calibrated = false;
@ -117,29 +139,21 @@ static float accel_temperature = NAN;
static float gyro_temperature = NAN; static float gyro_temperature = NAN;
static float accel_temp_bias[3] = { 0 }; static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0; static uint8_t accel_temp_calibration_count = 0;
static uint8_t gyro_temp_calibration_count = 0;
static float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
}; };
static int8_t rotate = 0; static int8_t rotate = 0;
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
* Stores all the queues the algorithm will pull data from
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
* Update() -- queries queues and updates the attitude estiamte
*/
/** /**
* Initialise the module. Called before the start function * Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed * \returns 0 on success or -1 if initialisation failed
*/ */
int32_t SensorsInitialize(void) int32_t SensorsInitialize(void)
{ {
source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
GyroSensorInitialize(); GyroSensorInitialize();
AccelSensorInitialize(); AccelSensorInitialize();
MagSensorInitialize(); MagSensorInitialize();
@ -165,10 +179,7 @@ int32_t SensorsStart(void)
// Start main task // Start main task
xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
#ifdef PIOS_INCLUDE_WDG REGISTER_WDG();
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
#endif
return 0; return 0;
} }
@ -184,82 +195,47 @@ int32_t mag_test;
* The sensor task. This polls the gyros at 500 Hz and pumps that data to * The sensor task. This polls the gyros at 500 Hz and pumps that data to
* stabilization and to the attitude loop * stabilization and to the attitude loop
* *
* This function has a lot of if/defs right now to allow these configurations:
* 1. BMA180 accel and MPU6000 gyro
* 2. MPU6000 gyro and accel
* 3. BMA180 accel and L3GD20 gyro
*/ */
uint32_t sensor_dt_us; uint32_t sensor_dt_us;
static void SensorsTask(__attribute__((unused)) void *parameters) static void SensorsTask(__attribute__((unused)) void *parameters)
{ {
portTickType lastSysTime; portTickType lastSysTime;
uint32_t accel_samples = 0; sensor_fetch_context sensor_context;
uint32_t gyro_samples = 0; bool error = false;
int32_t accel_accum[3] = { 0, 0, 0 }; const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
int32_t gyro_accum[3] = { 0, 0, 0 }; PIOS_SENSORS_Instance *sensor;
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
settingsUpdatedCb(NULL);
UAVObjEvent ev; // Test sensors
settingsUpdatedCb(&ev); bool sensors_test = true;
uint8_t count = 0;
const struct pios_board_info *bdinfo = &pios_board_info_blob; LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
sensors_test &= PIOS_SENSORS_Test(sensor);
switch (bdinfo->board_rev) { count++;
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
} }
#if defined(PIOS_INCLUDE_HMC5X83) PIOS_Assert(count);
mag_test = PIOS_HMC5x83_Test(onboard_mag); RELOAD_WDG();
#else if (!sensors_test) {
mag_test = 0;
#endif
if (accel_test < 0 || gyro_test < 0 || mag_test < 0) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while (1) { while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelay(10); vTaskDelay(10);
} }
} }
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
bool error = false;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1) { while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail // TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) { if (error) {
#ifdef PIOS_INCLUDE_WDG RELOAD_WDG();
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false; error = false;
} else { } else {
@ -267,219 +243,195 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
} }
for (int i = 0; i < 3; i++) { // reset the fetch context
accel_accum[i] = 0; clearContext(&sensor_context);
gyro_accum[i] = 0; LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
} // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
accel_samples = 0; bool is_primary = (sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL);
gyro_samples = 0;
AccelSensorData accelSensorData; if (!sensor->driver->is_polled) {
GyroSensorData gyroSensorData; const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
while (xQueueReceive(queue,
switch (bdinfo->board_rev) { (void *)source_data,
case 0x01: // L3GD20 + BMA180 board (is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
#if defined(PIOS_INCLUDE_BMA180) accumulateSamples(&sensor_context, source_data);
{
struct pios_bma180_data accel;
int32_t read_good;
int32_t count;
count = 0;
while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
} }
if (error) { if (sensor_context.count) {
// Unfortunately if the BMA180 ever misses getting read, then it will not processSamples(&sensor_context, sensor);
// trigger more interrupts. In this case we must force a read to kickstarts clearContext(&sensor_context);
// it. } else if (is_primary) {
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data);
continue;
}
while (read_good == 0) {
count++;
accel_accum[1] += accel.x;
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z;
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
// Get temp from last reading
accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f;
}
#endif /* if defined(PIOS_INCLUDE_BMA180) */
#if defined(PIOS_INCLUDE_L3GD20)
{
struct pios_l3gd20_data gyro;
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) {
error = true; error = true;
continue;
} }
} else {
gyro_samples = 1; if (PIOS_SENSORS_Poll(sensor)) {
gyro_accum[1] += gyro.gyro_x; PIOS_SENSOR_Fetch(sensor, (void *)&source_data, MAX_SENSORS_PER_INSTANCE);
gyro_accum[0] += gyro.gyro_y; accumulateSamples(&sensor_context, source_data);
gyro_accum[2] -= gyro.gyro_z; processSamples(&sensor_context, sensor);
clearContext(&sensor_context);
gyro_scaling = PIOS_L3GD20_GetScale();
// Get temp from last reading
gyroSensorData.temperature = gyro.temperature;
}
#endif /* if defined(PIOS_INCLUDE_L3GD20) */
break;
case 0x02: // MPU6000 board
case 0x03: // MPU6000 board
#if defined(PIOS_INCLUDE_MPU6000)
{
struct pios_mpu6000_data mpu6000_data;
xQueueHandle queue = PIOS_MPU6000_GetQueue();
while (xQueueReceive(queue, (void *)&mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) {
gyro_accum[0] += mpu6000_data.gyro_x;
gyro_accum[1] += mpu6000_data.gyro_y;
gyro_accum[2] += mpu6000_data.gyro_z;
accel_accum[0] += mpu6000_data.accel_x;
accel_accum[1] += mpu6000_data.accel_y;
accel_accum[2] += mpu6000_data.accel_z;
gyro_samples++;
accel_samples++;
} }
PERF_MEASURE_PERIOD(counterSensorPeriod);
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true;
continue;
}
gyro_scaling = PIOS_MPU6000_GetScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
gyroSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
accelSensorData.temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
}
#endif /* PIOS_INCLUDE_MPU6000 */
break;
default:
PIOS_DEBUG_Assert(0);
}
if (isnan(accel_temperature)) {
accel_temperature = accelSensorData.temperature;
gyro_temperature = gyroSensorData.temperature;
}
accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature;
gyro_temperature = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature;
if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
}
if (gyro_temp_calibrated) {
float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
} }
} }
temp_calibration_count--;
// Scale the accels
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
RELOAD_WDG();
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0], vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
accelSensorData.y = accels[1];
accelSensorData.z = accels[2];
} else {
accelSensorData.x = accels_out[0];
accelSensorData.y = accels_out[1];
accelSensorData.z = accels_out[2];
}
AccelSensorSet(&accelSensorData);
// Scale the gyros
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
gyroSensorData.y = gyros[1];
gyroSensorData.z = gyros[2];
} else {
gyroSensorData.x = gyros_out[0];
gyroSensorData.y = gyros_out[1];
gyroSensorData.z = gyros_out[2];
}
GyroSensorSet(&gyroSensorData);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5x83_ReadMag(onboard_mag, values);
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
} }
} }
static void clearContext(sensor_fetch_context *sensor_context)
{
// clear the context once it has finished
for (uint32_t i = 0; i < MAX_SENSORS_PER_INSTANCE; i++) {
sensor_context->accum[i].x = 0;
sensor_context->accum[i].y = 0;
sensor_context->accum[i].z = 0;
}
sensor_context->temperature = 0;
sensor_context->count = 0;
}
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample)
{
for (uint32_t i = 0; (i < MAX_SENSORS_PER_INSTANCE) && (i < sample->sensorSample3Axis.count); i++) {
sensor_context->accum[i].x += sample->sensorSample3Axis.sample[i].x;
sensor_context->accum[i].y += sample->sensorSample3Axis.sample[i].y;
sensor_context->accum[i].z += sample->sensorSample3Axis.sample[i].z;
}
sensor_context->temperature += sample->sensorSample3Axis.temperature;
sensor_context->count++;
}
static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor)
{
float samples[3];
float temperature;
float scales[MAX_SENSORS_PER_INSTANCE];
PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
float inv_count = 1.0f / (float)sensor_context->count;
if ((sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) {
float t = inv_count * scales[0];
samples[0] = ((float)sensor_context->accum[0].x * t);
samples[1] = ((float)sensor_context->accum[0].y * t);
samples[2] = ((float)sensor_context->accum[0].z * t);
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) {
handleMag(samples, temperature);
return;
} else {
handleAccel(samples, temperature);
}
}
if (sensor->type && PIOS_SENSORS_TYPE_3AXIS_GYRO) {
uint8_t index = 0;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
index = 1;
}
float t = inv_count * scales[index];
samples[0] = ((float)sensor_context->accum[index].x * t);
samples[1] = ((float)sensor_context->accum[index].y * t);
samples[2] = ((float)sensor_context->accum[index].z * t);
temperature = (float)sensor_context->temperature * inv_count * 0.01f;
handleGyro(samples, temperature);
return;
}
if (sensor->type == PIOS_SENSORS_TYPE_1AXIS_BARO) {
PIOS_Assert(0); // not yet implemented
}
}
void handleAccel(float *samples, float temperature)
{
AccelSensorData accelSensorData;
updateAccelTempBias(temperature);
float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
rot_mult(R, accels_out, samples);
accelSensorData.x = samples[0];
accelSensorData.y = samples[1];
accelSensorData.z = samples[2];
AccelSensorSet(&accelSensorData);
}
void handleGyro(float *samples, float temperature)
{
GyroSensorData gyroSensorData;
updateGyroTempBias(temperature);
float gyros_out[3] = { samples[0] * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
samples[1] * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
samples[2] * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
rot_mult(R, gyros_out, samples);
gyroSensorData.temperature = temperature;
gyroSensorData.x = samples[0];
gyroSensorData.y = samples[1];
gyroSensorData.z = samples[2];
GyroSensorSet(&gyroSensorData);
}
void handleMag(float *samples, float temperature)
{
MagSensorData mag;
float mags[3] = { (float)samples[1] - mag_bias[0],
(float)samples[0] - mag_bias[1],
(float)samples[2] - mag_bias[2] };
rot_mult(mag_transform, mags, samples);
mag.x = samples[0];
mag.y = samples[1];
mag.z = samples[2];
mag.temperature = temperature;
MagSensorSet(&mag);
}
static void updateAccelTempBias(float temperature)
{
if (isnan(accel_temperature)) {
accel_temperature = temperature;
}
accel_temperature = temp_alpha * (temperature - accel_temperature) + accel_temperature;
if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
}
}
accel_temp_calibration_count--;
}
static void updateGyroTempBias(float temperature)
{
if (isnan(gyro_temperature)) {
gyro_temperature = temperature;
}
gyro_temperature = temp_alpha * (temperature - gyro_temperature) + gyro_temperature;
if (gyro_temp_calibrated && !gyro_temp_calibration_count) {
gyro_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (gyro_temp_calibrated) {
float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
}
gyro_temp_calibration_count--;
}
/** /**
* Locally cache some variables from the AtttitudeSettings object * Locally cache some variables from the AtttitudeSettings object
*/ */

2
flight/pios/inc/pios_math.h
View File

@ -73,7 +73,7 @@
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) #define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
// helper macros for LPFs // helper macros for LPFs
#define LPF_ALPHA(dt,fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc))) #define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
// Useful math macros // Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MAX(a, b) ((a) > (b) ? (a) : (b))

7
shared/uavobjectdefinition/magsensor.xml
View File

@ -1,9 +1,10 @@
<xml> <xml>
<object name="MagSensor" singleinstance="true" settings="false" category="Sensors"> <object name="MagSensor" singleinstance="true" settings="false" category="Sensors">
<description>Calibrated sensor data from 3 axis magnetometer in MilliGauss.</description> <description>Calibrated sensor data from 3 axis magnetometer in MilliGauss.</description>
<field name="x" units="mGa" type="float" elements="1"/> <field name="x" units="mGa" type="float" elements="1"/>
<field name="y" units="mGa" type="float" elements="1"/> <field name="y" units="mGa" type="float" elements="1"/>
<field name="z" units="mGa" type="float" elements="1"/> <field name="z" units="mGa" type="float" elements="1"/>
<field name="temperature" units="deg C" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/> <telemetryflight acked="false" updatemode="periodic" period="10000"/>