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

OP-1658 - Add support for barometer in sensor module

Browse Source
This commit is contained in:
Alessio Morale 2014-12-29 12:28:42 +01:00
parent 6904996aba
commit ba0e486004
2 changed files with 110 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,15 +49,22 @@
#include <openpilot.h> #include <openpilot.h>
#include <pios_sensors.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 <barosensor.h>
#include <flightstatus.h>
#include <attitudesettings.h> #include <attitudesettings.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <accelgyrosettings.h> #include <accelgyrosettings.h>
#include <flightstatus.h> #include <revosettings.h>
#include <mathmisc.h>
#include <taskinfo.h> #include <taskinfo.h>
#include <pios_math.h> #include <pios_math.h>
#include <pios_constants.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_board_info.h> #include <pios_board_info.h>
#include <string.h> #include <string.h>
@ -74,16 +81,24 @@
#define REGISTER_WDG() #define REGISTER_WDG()
#endif #endif
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); static const TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
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
// LPF // LPF
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE) #define TEMP_DT_GYRO_ACCEL (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f #define TEMP_LPF_FC_GYRO_ACCEL 5.0f
static const float temp_alpha = LPF_ALPHA(TEMP_DT, TEMP_LPF_FC); static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
// Interval in number of sample to recalculate temp bias
#define BARO_TEMP_CALIB_INTERVAL 10
// LPF
#define TEMP_DT_BARO (1.0f / 120.0f)
#define TEMP_LPF_FC_BARO 5.0f
static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
#define ZERO_ROT_ANGLE 0.00001f #define ZERO_ROT_ANGLE 0.00001f
// Private types // Private types
@ -114,15 +129,19 @@ 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 accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor); static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor);
static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor);
static void clearContext(sensor_fetch_context *sensor_context); static void clearContext(sensor_fetch_context *sensor_context);
static void handleAccel(float *samples, float temperature); static void handleAccel(float *samples, float temperature);
static void handleGyro(float *samples, float temperature); static void handleGyro(float *samples, float temperature);
static void handleMag(float *samples, float temperature); static void handleMag(float *samples, float temperature);
static void handleBaro(float sample, float temperature);
static void updateAccelTempBias(float temperature); static void updateAccelTempBias(float temperature);
static void updateGyroTempBias(float temperature); static void updateGyroTempBias(float temperature);
static void updateBaroTempBias(float temperature);
// Private variables // Private variables
static sensor_data *source_data; static sensor_data *source_data;
@ -137,7 +156,7 @@ static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
}; };
// Variables used to handle temperature bias // Variables used to handle accel/gyro 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;
@ -151,6 +170,14 @@ static uint8_t gyro_temp_calibration_count = 0;
static float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
}; };
// Variables used to handle baro temperature bias
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool baro_temp_correction_enabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t baro_temp_calibration_count = 0;
static int8_t rotate = 0; static int8_t rotate = 0;
/** /**
@ -163,15 +190,17 @@ int32_t SensorsInitialize(void)
GyroSensorInitialize(); GyroSensorInitialize();
AccelSensorInitialize(); AccelSensorInitialize();
MagSensorInitialize(); MagSensorInitialize();
BaroSensorInitialize();
RevoCalibrationInitialize(); RevoCalibrationInitialize();
RevoSettingsInitialize();
AttitudeSettingsInitialize(); AttitudeSettingsInitialize();
AccelGyroSettingsInitialize(); AccelGyroSettingsInitialize();
rotate = 0; rotate = 0;
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -260,7 +289,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
clearContext(&sensor_context); clearContext(&sensor_context);
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) { 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) // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
bool is_primary = (sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL); bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
if (!sensor->driver->is_polled) { if (!sensor->driver->is_polled) {
const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor); const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
@ -270,7 +299,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accumulateSamples(&sensor_context, source_data); accumulateSamples(&sensor_context, source_data);
} }
if (sensor_context.count) { if (sensor_context.count) {
processSamples(&sensor_context, sensor); processSamples3d(&sensor_context, sensor);
clearContext(&sensor_context); clearContext(&sensor_context);
} else if (is_primary) { } else if (is_primary) {
error = true; error = true;
@ -278,8 +307,12 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
} else { } else {
if (PIOS_SENSORS_Poll(sensor)) { if (PIOS_SENSORS_Poll(sensor)) {
PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE); PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
if (sensor->type & PIOS_SENSORS_TYPE_3D) {
accumulateSamples(&sensor_context, source_data); accumulateSamples(&sensor_context, source_data);
processSamples(&sensor_context, sensor); processSamples3d(&sensor_context, sensor);
} else {
processSamples1d(&source_data->sensorSample1Axis, sensor);
}
clearContext(&sensor_context); clearContext(&sensor_context);
} }
} }
@ -313,7 +346,7 @@ static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data
sensor_context->count++; sensor_context->count++;
} }
static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor) static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SENSORS_Instance *sensor)
{ {
float samples[3]; float samples[3];
float temperature; float temperature;
@ -321,7 +354,7 @@ static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENS
PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE); PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
float inv_count = 1.0f / (float)sensor_context->count; float inv_count = 1.0f / (float)sensor_context->count;
if ((sensor->type && PIOS_SENSORS_TYPE_3AXIS_ACCEL) || if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) { (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)) {
float t = inv_count * scales[0]; float t = inv_count * scales[0];
samples[0] = ((float)sensor_context->accum[0].x * t); samples[0] = ((float)sensor_context->accum[0].x * t);
@ -339,7 +372,7 @@ static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENS
} }
} }
if (sensor->type && PIOS_SENSORS_TYPE_3AXIS_GYRO) { if (sensor->type & PIOS_SENSORS_TYPE_3AXIS_GYRO) {
uint8_t index = 0; uint8_t index = 0;
if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) { if (sensor->type == PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL) {
index = 1; index = 1;
@ -352,14 +385,22 @@ static void processSamples(sensor_fetch_context *sensor_context, const PIOS_SENS
handleGyro(samples, temperature); handleGyro(samples, temperature);
return; return;
} }
}
if (sensor->type == PIOS_SENSORS_TYPE_1AXIS_BARO) { static void processSamples1d(PIOS_SENSORS_1Axis_SensorsWithTemp *sample, const PIOS_SENSORS_Instance *sensor)
{
switch (sensor->type) {
case PIOS_SENSORS_TYPE_1AXIS_BARO:
PERF_MEASURE_PERIOD(counterBaroPeriod); PERF_MEASURE_PERIOD(counterBaroPeriod);
PIOS_Assert(0); // not yet implemented handleBaro(sample->sample, sample->temperature);
return;
default:
PIOS_Assert(0);
} }
} }
void handleAccel(float *samples, float temperature) static void handleAccel(float *samples, float temperature)
{ {
AccelSensorData accelSensorData; AccelSensorData accelSensorData;
@ -372,10 +413,11 @@ void handleAccel(float *samples, float temperature)
accelSensorData.x = samples[0]; accelSensorData.x = samples[0];
accelSensorData.y = samples[1]; accelSensorData.y = samples[1];
accelSensorData.z = samples[2]; accelSensorData.z = samples[2];
accelSensorData.temperature = temperature;
AccelSensorSet(&accelSensorData); AccelSensorSet(&accelSensorData);
} }
void handleGyro(float *samples, float temperature)
static void handleGyro(float *samples, float temperature)
{ {
GyroSensorData gyroSensorData; GyroSensorData gyroSensorData;
@ -393,7 +435,7 @@ void handleGyro(float *samples, float temperature)
GyroSensorSet(&gyroSensorData); GyroSensorSet(&gyroSensorData);
} }
void handleMag(float *samples, float temperature) static void handleMag(float *samples, float temperature)
{ {
MagSensorData mag; MagSensorData mag;
float mags[3] = { (float)samples[1] - mag_bias[0], float mags[3] = { (float)samples[1] - mag_bias[0],
@ -410,12 +452,29 @@ void handleMag(float *samples, float temperature)
MagSensorSet(&mag); MagSensorSet(&mag);
} }
static void handleBaro(float sample, float temperature)
{
updateBaroTempBias(temperature);
sample -= baro_temp_bias;
float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
if (!isnan(altitude)) {
BaroSensorData data;
data.Altitude = altitude;
data.Temperature = temperature;
data.Pressure = sample;
// Update the BasoSensor UAVObject
BaroSensorSet(&data);
}
}
static void updateAccelTempBias(float temperature) static void updateAccelTempBias(float temperature)
{ {
if (isnan(accel_temperature)) { if (isnan(accel_temperature)) {
accel_temperature = temperature; accel_temperature = temperature;
} }
accel_temperature = temp_alpha * (temperature - accel_temperature) + accel_temperature; accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
if ((accel_temp_calibrated) && !accel_temp_calibration_count) { if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
accel_temp_calibration_count = TEMP_CALIB_INTERVAL; accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
@ -428,13 +487,14 @@ static void updateAccelTempBias(float temperature)
} }
accel_temp_calibration_count--; accel_temp_calibration_count--;
} }
static void updateGyroTempBias(float temperature) static void updateGyroTempBias(float temperature)
{ {
if (isnan(gyro_temperature)) { if (isnan(gyro_temperature)) {
gyro_temperature = temperature; gyro_temperature = temperature;
} }
gyro_temperature = temp_alpha * (temperature - gyro_temperature) + gyro_temperature; gyro_temperature = temp_alpha_gyro_accel * (temperature - gyro_temperature) + gyro_temperature;
if (gyro_temp_calibrated && !gyro_temp_calibration_count) { if (gyro_temp_calibrated && !gyro_temp_calibration_count) {
gyro_temp_calibration_count = TEMP_CALIB_INTERVAL; gyro_temp_calibration_count = TEMP_CALIB_INTERVAL;
@ -449,6 +509,22 @@ static void updateGyroTempBias(float temperature)
gyro_temp_calibration_count--; gyro_temp_calibration_count--;
} }
static void updateBaroTempBias(float temperature)
{
baro_temperature = temp_alpha_baro * (temperature - baro_temperature) + baro_temperature;
baro_temp_calibration_count--;
if (isnan(baro_temperature)) {
baro_temperature = temperature;
}
if (baro_temp_correction_enabled && !baro_temp_calibration_count) {
baro_temp_calibration_count = BARO_TEMP_CALIB_INTERVAL;
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
}
/** /**
* Locally cache some variables from the AtttitudeSettings object * Locally cache some variables from the AtttitudeSettings object
*/ */
@ -502,6 +578,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
} }
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform); matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
} }
/** /**
* @} * @}

3
flight/pios/inc/pios_sensors.h
View File

@ -61,6 +61,9 @@ typedef enum PIOS_SENSORS_TYPE {
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08, PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08,
} PIOS_SENSORS_TYPE; } PIOS_SENSORS_TYPE;
#define PIOS_SENSORS_TYPE_1D (PIOS_SENSORS_TYPE_1AXIS_BARO)
#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG)
typedef struct PIOS_SENSORS_Instance { typedef struct PIOS_SENSORS_Instance {
const PIOS_SENSORS_Driver *driver; const PIOS_SENSORS_Driver *driver;
uintptr_t context; uintptr_t context;