mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Merge remote-tracking branch 'origin/amorale/OP-1658_sensor_overhaul_next' into next
This commit is contained in:
commit
ad42ca7f16
@ -1,14 +1,13 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @addtogroup OpenPilot Math Utilities
|
||||
* @{
|
||||
* @addtogroup AltitudeModule Altitude Module
|
||||
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
|
||||
* @addtogroup Reuseable vector data type and functions
|
||||
* @{
|
||||
*
|
||||
* @file altitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, reads temperature and pressure from BMP085
|
||||
* @file vectors.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Reuseable vector data type and functions
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -28,14 +27,37 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ALTITUDE_H
|
||||
#define ALTITUDE_H
|
||||
|
||||
int32_t AltitudeInitialize();
|
||||
#ifndef VECTORS_H_
|
||||
#define VECTORS_H_
|
||||
|
||||
#endif // ALTITUDE_H
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
#define DECLAREVECTOR3(suffix, datatype) \
|
||||
typedef struct Vector3##suffix##_t { \
|
||||
datatype x; \
|
||||
datatype y; \
|
||||
datatype z; \
|
||||
} Vector3##suffix
|
||||
|
||||
#define DECLAREVECTOR2(suffix, datatype) \
|
||||
typedef struct Vector2##suffix##_t { \
|
||||
datatype x; \
|
||||
datatype y; \
|
||||
} Vector2##suffix
|
||||
|
||||
|
||||
DECLAREVECTOR3(i16, int16_t);
|
||||
DECLAREVECTOR3(i32, int32_t);
|
||||
DECLAREVECTOR3(u16, uint16_t);
|
||||
DECLAREVECTOR3(u32, uint32_t);
|
||||
DECLAREVECTOR3(f, float);
|
||||
|
||||
DECLAREVECTOR2(i16, int16_t);
|
||||
DECLAREVECTOR2(i32, int32_t);
|
||||
DECLAREVECTOR2(u16, uint16_t);
|
||||
DECLAREVECTOR2(u32, uint32_t);
|
||||
DECLAREVECTOR2(f, float);
|
||||
|
||||
#endif /* VECTORS_H_ */
|
@ -1,221 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup AltitudeModule Altitude Module
|
||||
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
|
||||
* @{
|
||||
*
|
||||
* @file altitude.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, handles temperature and pressure readings from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Output object: BaroSensor
|
||||
*
|
||||
* This module will periodically update the value of the BaroSensor object.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <openpilot.h>
|
||||
|
||||
#include "altitude.h"
|
||||
#include "barosensor.h" // object that will be updated by the module
|
||||
#include "revosettings.h"
|
||||
#include <mathmisc.h>
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
#include "sonaraltitude.h" // object that will be updated by the module
|
||||
#endif
|
||||
#include "taskinfo.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 550
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
|
||||
|
||||
// Interval in number of sample to recalculate temp bias
|
||||
#define TEMP_CALIB_INTERVAL 10
|
||||
|
||||
// LPF
|
||||
#define TEMP_DT (1.0f / 120.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));
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
|
||||
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
|
||||
static volatile bool tempCorrectionEnabled;
|
||||
|
||||
static float baro_temp_bias = 0;
|
||||
static float baro_temperature = NAN;
|
||||
static uint8_t temp_calibration_count = 0;
|
||||
|
||||
// Private functions
|
||||
static void altitudeTask(void *parameters);
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeStart()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(altitudeTask, "Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeInitialize()
|
||||
{
|
||||
BaroSensorInitialize();
|
||||
RevoSettingsInitialize();
|
||||
RevoSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(NULL);
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeInitialize();
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void altitudeTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
BaroSensorData data;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeData sonardata;
|
||||
int32_t value = 0, timeout = 10, sample_rate = 0;
|
||||
float coeff = 0.25, height_out = 0, height_in = 0;
|
||||
PIOS_HCSR04_Trigger();
|
||||
#endif
|
||||
|
||||
// TODO: Check the pressure sensor and set a warning if it fails test
|
||||
|
||||
// Option to change the interleave between Temp and Pressure conversions
|
||||
// Undef for normal operation
|
||||
// #define PIOS_MS5611_SLOW_TEMP_RATE 20
|
||||
|
||||
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
|
||||
|
||||
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
|
||||
uint8_t temp_press_interleave_count = 1;
|
||||
#endif
|
||||
// Main task loop
|
||||
while (1) {
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
// Compute the current altitude
|
||||
// depends on baro samplerate
|
||||
if (!(sample_rate--)) {
|
||||
if (PIOS_HCSR04_Completed()) {
|
||||
value = PIOS_HCSR04_Get();
|
||||
// from 3.4cm to 5.1m
|
||||
if ((value > 100) && (value < 15000)) {
|
||||
height_in = value * 0.00034f / 2.0f;
|
||||
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
|
||||
sonardata.Altitude = height_out; // m/us
|
||||
}
|
||||
|
||||
// Update the SonarAltitude UAVObject
|
||||
SonarAltitudeSet(&sonardata);
|
||||
timeout = 10;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
if (!(timeout--)) {
|
||||
// retrigger
|
||||
timeout = 10;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
sample_rate = 25;
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_HCSR04) */
|
||||
float temp, press;
|
||||
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
|
||||
temp_press_interleave_count--;
|
||||
if (temp_press_interleave_count == 0) {
|
||||
#endif
|
||||
// Update the temperature data
|
||||
PIOS_MS5611_StartADC(TemperatureConv);
|
||||
vTaskDelay(PIOS_MS5611_GetDelay());
|
||||
PIOS_MS5611_ReadADC();
|
||||
|
||||
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
|
||||
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Update the pressure data
|
||||
PIOS_MS5611_StartADC(PressureConv);
|
||||
vTaskDelay(PIOS_MS5611_GetDelay());
|
||||
PIOS_MS5611_ReadADC();
|
||||
|
||||
temp = PIOS_MS5611_GetTemperature();
|
||||
press = PIOS_MS5611_GetPressure();
|
||||
|
||||
if (isnan(baro_temperature)) {
|
||||
baro_temperature = temp;
|
||||
}
|
||||
|
||||
baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
|
||||
|
||||
if (tempCorrectionEnabled && !temp_calibration_count) {
|
||||
temp_calibration_count = 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;
|
||||
}
|
||||
|
||||
press -= baro_temp_bias;
|
||||
|
||||
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
|
||||
|
||||
if (!isnan(altitude)) {
|
||||
data.Altitude = altitude;
|
||||
data.Temperature = temp;
|
||||
data.Pressure = press;
|
||||
// Update the BasoSensor UAVObject
|
||||
BaroSensorSet(&data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
|
||||
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
|
||||
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
|
||||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
@ -62,6 +62,10 @@
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "taskinfo.h"
|
||||
|
||||
#include <pios_sensors.h>
|
||||
#include <pios_adxl345.h>
|
||||
#include <pios_mpu6000.h>
|
||||
|
||||
#include "CoordinateConversions.h"
|
||||
#include <pios_notify.h>
|
||||
#include <mathmisc.h>
|
||||
@ -160,9 +164,11 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
|
||||
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
|
||||
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
|
||||
|
||||
static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
|
||||
|
||||
// Used to detect CC vs CC3D
|
||||
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
|
||||
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
@ -238,7 +244,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
if (cc3d) {
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
gyro_test = PIOS_MPU6000_Test();
|
||||
|
||||
gyro_test = PIOS_MPU6000_Driver.test(0);
|
||||
mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2);
|
||||
#endif
|
||||
} else {
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
@ -455,7 +463,6 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
|
||||
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
|
||||
* @return 0 if successfull, -1 if not
|
||||
*/
|
||||
static struct pios_mpu6000_data mpu6000_data;
|
||||
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
|
||||
{
|
||||
float accels[3] = { 0 };
|
||||
@ -465,18 +472,18 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
|
||||
xQueueHandle queue = PIOS_MPU6000_GetQueue();
|
||||
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
|
||||
xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
|
||||
BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
|
||||
while (ret == pdTRUE) {
|
||||
gyros[0] += mpu6000_data.gyro_x;
|
||||
gyros[1] += mpu6000_data.gyro_y;
|
||||
gyros[2] += mpu6000_data.gyro_z;
|
||||
gyros[0] += mpu6000_data->sample[1].x;
|
||||
gyros[1] += mpu6000_data->sample[1].y;
|
||||
gyros[2] += mpu6000_data->sample[1].z;
|
||||
|
||||
accels[0] += mpu6000_data.accel_x;
|
||||
accels[1] += mpu6000_data.accel_y;
|
||||
accels[2] += mpu6000_data.accel_z;
|
||||
accels[0] += mpu6000_data->sample[0].x;
|
||||
accels[1] += mpu6000_data->sample[0].y;
|
||||
accels[2] += mpu6000_data->sample[0].z;
|
||||
|
||||
temp += mpu6000_data.temperature;
|
||||
temp += mpu6000_data->temperature;
|
||||
|
||||
count++;
|
||||
// check if further samples are already in queue
|
||||
@ -751,17 +758,19 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
|
||||
|
||||
if (BOARDISCC3D) {
|
||||
float scales[2];
|
||||
PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
|
||||
accel_bias.X = accelGyroSettings.accel_bias.X;
|
||||
accel_bias.Y = accelGyroSettings.accel_bias.Y;
|
||||
accel_bias.Z = accelGyroSettings.accel_bias.Z;
|
||||
|
||||
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
|
||||
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
|
||||
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
|
||||
gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[0];
|
||||
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[0];
|
||||
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[0];
|
||||
|
||||
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
|
||||
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
|
||||
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
|
||||
accel_scale.X = accelGyroSettings.accel_scale.X * scales[1];
|
||||
accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[1];
|
||||
accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[1];
|
||||
} else {
|
||||
// Original CC with analog gyros and ADXL accel
|
||||
accel_bias.X = accelGyroSettings.accel_bias.X;
|
||||
|
@ -2,16 +2,17 @@
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup Sensors Sensors Module
|
||||
* @addtogroup Sensors
|
||||
* @brief Acquires sensor data
|
||||
* @{
|
||||
*
|
||||
* @file attitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
|
||||
* @brief Acquires sensor data and fuses it into attitude estimate for CC
|
||||
* @file sensors.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Module to handle fetch and preprocessing of sensor data
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
******************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
|
@ -4,12 +4,11 @@
|
||||
* @{
|
||||
* @addtogroup Sensors
|
||||
* @brief Acquires sensor data
|
||||
* Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
|
||||
* @{
|
||||
*
|
||||
* @file sensors.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Module to handle fetch and preprocessing of sensor data
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -47,69 +46,118 @@
|
||||
*/
|
||||
|
||||
#include <openpilot.h>
|
||||
|
||||
#include <pios_sensors.h>
|
||||
#include <homelocation.h>
|
||||
|
||||
#include <magsensor.h>
|
||||
#include <accelsensor.h>
|
||||
#include <gyrosensor.h>
|
||||
#include <attitudestate.h>
|
||||
#include <barosensor.h>
|
||||
#include <flightstatus.h>
|
||||
|
||||
#include <attitudesettings.h>
|
||||
#include <revocalibration.h>
|
||||
#include <accelgyrosettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <revosettings.h>
|
||||
|
||||
#include <mathmisc.h>
|
||||
#include <taskinfo.h>
|
||||
#include <pios_math.h>
|
||||
#include <pios_constants.h>
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
#include <pios_board_info.h>
|
||||
#include <string.h>
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1000
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define STACK_SIZE_BYTES 1000
|
||||
#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 TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
|
||||
|
||||
// Interval in number of sample to recalculate temp bias
|
||||
#define TEMP_CALIB_INTERVAL 30
|
||||
#define TEMP_CALIB_INTERVAL 30
|
||||
|
||||
// LPF
|
||||
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE)
|
||||
#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));
|
||||
#define TEMP_DT_GYRO_ACCEL (1.0f / PIOS_SENSOR_RATE)
|
||||
#define TEMP_LPF_FC_GYRO_ACCEL 5.0f
|
||||
static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
|
||||
|
||||
#define ZERO_ROT_ANGLE 0.00001f
|
||||
// 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
|
||||
// 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
|
||||
#include <pios_instrumentation_helper.h>
|
||||
|
||||
PERF_DEFINE_COUNTER(counterGyroSamples);
|
||||
PERF_DEFINE_COUNTER(counterAccelSamples);
|
||||
PERF_DEFINE_COUNTER(counterAccelPeriod);
|
||||
PERF_DEFINE_COUNTER(counterMagPeriod);
|
||||
PERF_DEFINE_COUNTER(counterBaroPeriod);
|
||||
PERF_DEFINE_COUNTER(counterSensorPeriod);
|
||||
|
||||
// Counters:
|
||||
// - 0x53000001 Sensor fetch rate(period)
|
||||
// - 0x53000002 number of gyro samples read for each loop
|
||||
PERF_DEFINE_COUNTER(counterSensorResets);
|
||||
|
||||
// Private functions
|
||||
static void SensorsTask(void *parameters);
|
||||
static void settingsUpdatedCb(UAVObjEvent *objEv);
|
||||
|
||||
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
|
||||
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 handleAccel(float *samples, float temperature);
|
||||
static void handleGyro(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 updateGyroTempBias(float temperature);
|
||||
static void updateBaroTempBias(float temperature);
|
||||
|
||||
// Private variables
|
||||
static sensor_data *source_data;
|
||||
static xTaskHandle sensorsTaskHandle;
|
||||
RevoCalibrationData cal;
|
||||
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
|
||||
|
||||
static float mag_bias[3] = { 0, 0, 0 };
|
||||
static float mag_transform[3][3] = {
|
||||
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
|
||||
};
|
||||
// temp coefficient to calculate gyro bias
|
||||
|
||||
// Variables used to handle accel/gyro temperature bias
|
||||
static volatile bool gyro_temp_calibrated = false;
|
||||
static volatile bool accel_temp_calibrated = false;
|
||||
|
||||
@ -117,42 +165,45 @@ static float accel_temperature = NAN;
|
||||
static float gyro_temperature = NAN;
|
||||
static float accel_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] = {
|
||||
{ 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;
|
||||
|
||||
/**
|
||||
* 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
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t SensorsInitialize(void)
|
||||
{
|
||||
source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
|
||||
GyroSensorInitialize();
|
||||
AccelSensorInitialize();
|
||||
MagSensorInitialize();
|
||||
BaroSensorInitialize();
|
||||
RevoCalibrationInitialize();
|
||||
RevoSettingsInitialize();
|
||||
AttitudeSettingsInitialize();
|
||||
AccelGyroSettingsInitialize();
|
||||
|
||||
rotate = 0;
|
||||
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -165,10 +216,7 @@ int32_t SensorsStart(void)
|
||||
// Start main task
|
||||
xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
|
||||
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
|
||||
#endif
|
||||
|
||||
REGISTER_WDG();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -184,82 +232,55 @@ int32_t mag_test;
|
||||
* The sensor task. This polls the gyros at 500 Hz and pumps that data to
|
||||
* 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;
|
||||
static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
portTickType lastSysTime;
|
||||
uint32_t accel_samples = 0;
|
||||
uint32_t gyro_samples = 0;
|
||||
int32_t accel_accum[3] = { 0, 0, 0 };
|
||||
int32_t gyro_accum[3] = { 0, 0, 0 };
|
||||
float gyro_scaling = 0;
|
||||
float accel_scaling = 0;
|
||||
static int32_t timeval;
|
||||
sensor_fetch_context sensor_context;
|
||||
bool error = false;
|
||||
const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
|
||||
PIOS_SENSORS_Instance *sensor;
|
||||
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
|
||||
settingsUpdatedCb(NULL);
|
||||
|
||||
UAVObjEvent ev;
|
||||
settingsUpdatedCb(&ev);
|
||||
// Performance counters
|
||||
PERF_INIT_COUNTER(counterAccelSamples, 0x53000001);
|
||||
PERF_INIT_COUNTER(counterAccelPeriod, 0x53000002);
|
||||
PERF_INIT_COUNTER(counterMagPeriod, 0x53000003);
|
||||
PERF_INIT_COUNTER(counterBaroPeriod, 0x53000004);
|
||||
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000005);
|
||||
PERF_INIT_COUNTER(counterSensorResets, 0x53000006);
|
||||
|
||||
const struct pios_board_info *bdinfo = &pios_board_info_blob;
|
||||
|
||||
switch (bdinfo->board_rev) {
|
||||
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);
|
||||
// Test sensors
|
||||
bool sensors_test = true;
|
||||
uint8_t count = 0;
|
||||
LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
|
||||
sensors_test &= PIOS_SENSORS_Test(sensor);
|
||||
count++;
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
mag_test = PIOS_HMC5x83_Test(onboard_mag);
|
||||
#else
|
||||
mag_test = 0;
|
||||
#endif
|
||||
|
||||
if (accel_test < 0 || gyro_test < 0 || mag_test < 0) {
|
||||
PIOS_Assert(count);
|
||||
RELOAD_WDG();
|
||||
if (!sensors_test) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
while (1) {
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
#endif
|
||||
vTaskDelay(10);
|
||||
}
|
||||
}
|
||||
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
|
||||
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
bool error = false;
|
||||
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
|
||||
uint32_t reset_counter = 0;
|
||||
|
||||
while (1) {
|
||||
// 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) {
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
#endif
|
||||
RELOAD_WDG();
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
|
||||
vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
error = false;
|
||||
} else {
|
||||
@ -267,219 +288,250 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_accum[i] = 0;
|
||||
gyro_accum[i] = 0;
|
||||
}
|
||||
accel_samples = 0;
|
||||
gyro_samples = 0;
|
||||
// reset the fetch context
|
||||
clearContext(&sensor_context);
|
||||
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)
|
||||
bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
|
||||
|
||||
AccelSensorData accelSensorData;
|
||||
GyroSensorData gyroSensorData;
|
||||
|
||||
switch (bdinfo->board_rev) {
|
||||
case 0x01: // L3GD20 + BMA180 board
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
{
|
||||
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 (!sensor->driver->is_polled) {
|
||||
const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
|
||||
while (xQueueReceive(queue,
|
||||
(void *)source_data,
|
||||
(is_primary && !sensor_context.count) ? sensor_period_ticks : 0) == pdTRUE) {
|
||||
accumulateSamples(&sensor_context, source_data);
|
||||
}
|
||||
if (error) {
|
||||
// Unfortunately if the BMA180 ever misses getting read, then it will not
|
||||
// trigger more interrupts. In this case we must force a read to kickstarts
|
||||
// it.
|
||||
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) {
|
||||
if (sensor_context.count) {
|
||||
processSamples3d(&sensor_context, sensor);
|
||||
clearContext(&sensor_context);
|
||||
} else if (is_primary) {
|
||||
PIOS_SENSOR_Reset(sensor);
|
||||
reset_counter++;
|
||||
PERF_TRACK_VALUE(counterSensorResets, reset_counter);
|
||||
error = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
gyro_samples = 1;
|
||||
gyro_accum[1] += gyro.gyro_x;
|
||||
gyro_accum[0] += gyro.gyro_y;
|
||||
gyro_accum[2] -= gyro.gyro_z;
|
||||
|
||||
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++;
|
||||
} else {
|
||||
if (PIOS_SENSORS_Poll(sensor)) {
|
||||
PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
|
||||
if (sensor->type & PIOS_SENSORS_TYPE_3D) {
|
||||
accumulateSamples(&sensor_context, source_data);
|
||||
processSamples3d(&sensor_context, sensor);
|
||||
} else {
|
||||
processSamples1d(&source_data->sensorSample1Axis, sensor);
|
||||
}
|
||||
clearContext(&sensor_context);
|
||||
}
|
||||
|
||||
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 };
|
||||
|
||||
|
||||
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
|
||||
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);
|
||||
PERF_MEASURE_PERIOD(counterSensorPeriod);
|
||||
RELOAD_WDG();
|
||||
vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
|
||||
}
|
||||
}
|
||||
|
||||
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 processSamples3d(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);
|
||||
PERF_MEASURE_PERIOD(counterMagPeriod);
|
||||
return;
|
||||
} else {
|
||||
PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
|
||||
PERF_MEASURE_PERIOD(counterAccelPeriod);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
handleBaro(sample->sample, sample->temperature);
|
||||
return;
|
||||
|
||||
default:
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
}
|
||||
|
||||
static 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];
|
||||
accelSensorData.temperature = temperature;
|
||||
AccelSensorSet(&accelSensorData);
|
||||
}
|
||||
|
||||
static 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);
|
||||
}
|
||||
|
||||
static 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 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)
|
||||
{
|
||||
if (isnan(accel_temperature)) {
|
||||
accel_temperature = temperature;
|
||||
}
|
||||
accel_temperature = temp_alpha_gyro_accel * (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_gyro_accel * (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--;
|
||||
}
|
||||
|
||||
static void updateBaroTempBias(float temperature)
|
||||
{
|
||||
if (isnan(baro_temperature)) {
|
||||
baro_temperature = temperature;
|
||||
}
|
||||
|
||||
baro_temperature = temp_alpha_baro * (temperature - baro_temperature) + baro_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;
|
||||
}
|
||||
baro_temp_calibration_count--;
|
||||
}
|
||||
/**
|
||||
* Locally cache some variables from the AtttitudeSettings object
|
||||
*/
|
||||
@ -533,6 +585,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
Quaternion2R(rotationQuat, R);
|
||||
}
|
||||
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));
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
|
@ -29,7 +29,7 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#include <pios_adxl345.h>
|
||||
#ifdef PIOS_INCLUDE_ADXL345
|
||||
|
||||
enum pios_adxl345_dev_magic {
|
||||
|
@ -30,7 +30,7 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#include <pios_bma180.h>
|
||||
#ifdef PIOS_INCLUDE_BMA180
|
||||
|
||||
#include "fifo_buffer.h"
|
||||
|
@ -29,7 +29,7 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#include <pios_bmp085.h>
|
||||
#ifdef PIOS_INCLUDE_BMP085
|
||||
|
||||
#ifndef PIOS_INCLUDE_EXTI
|
||||
|
@ -50,6 +50,22 @@ typedef struct {
|
||||
|
||||
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
|
||||
|
||||
// sensor driver interface
|
||||
bool PIOS_HMC5x83_driver_Test(uintptr_t context);
|
||||
void PIOS_HMC5x83_driver_Reset(uintptr_t context);
|
||||
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
|
||||
void PIOS_HMC5x83_driver_fetch(void *, uint8_t size, uintptr_t context);
|
||||
bool PIOS_HMC5x83_driver_poll(uintptr_t context);
|
||||
|
||||
const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
|
||||
.test = PIOS_HMC5x83_driver_Test,
|
||||
.poll = PIOS_HMC5x83_driver_poll,
|
||||
.fetch = PIOS_HMC5x83_driver_fetch,
|
||||
.reset = PIOS_HMC5x83_driver_Reset,
|
||||
.get_queue = NULL,
|
||||
.get_scale = PIOS_HMC5x83_driver_get_scale,
|
||||
.is_polled = true,
|
||||
};
|
||||
/**
|
||||
* Allocate the device setting structure
|
||||
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
|
||||
@ -99,6 +115,11 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
|
||||
return (pios_hmc5x83_dev_t)dev;
|
||||
}
|
||||
|
||||
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
|
||||
{
|
||||
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Initialize the HMC5x83 magnetometer sensor
|
||||
* \return none
|
||||
@ -549,6 +570,37 @@ int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_I2C */
|
||||
|
||||
/* PIOS sensor driver implementation */
|
||||
bool PIOS_HMC5x83_driver_Test(uintptr_t context)
|
||||
{
|
||||
return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
|
||||
}
|
||||
|
||||
void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
|
||||
|
||||
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
PIOS_Assert(size > 0);
|
||||
scales[0] = 1;
|
||||
}
|
||||
|
||||
void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
|
||||
{
|
||||
PIOS_Assert(size > 0);
|
||||
int16_t mag[3];
|
||||
PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
|
||||
PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
|
||||
tmp->count = 1;
|
||||
tmp->sample[0].x = mag[0];
|
||||
tmp->sample[0].y = mag[1];
|
||||
tmp->sample[0].z = mag[2];
|
||||
tmp->temperature = 0;
|
||||
}
|
||||
|
||||
bool PIOS_HMC5x83_driver_poll(uintptr_t context)
|
||||
{
|
||||
return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_HMC5x83 */
|
||||
|
||||
|
@ -52,8 +52,8 @@ pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id)
|
||||
if (!counter_handle) {
|
||||
pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
|
||||
newcounter->id = id;
|
||||
newcounter->max = INT32_MIN;
|
||||
newcounter->min = INT32_MAX;
|
||||
newcounter->max = INT32_MIN + 1;
|
||||
newcounter->min = INT32_MAX - 1;
|
||||
counter_handle = (pios_counter_t)newcounter;
|
||||
}
|
||||
return counter_handle;
|
||||
|
@ -30,7 +30,7 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#include <pios_l3gd20.h>
|
||||
#ifdef PIOS_INCLUDE_L3GD20
|
||||
|
||||
#include "fifo_buffer.h"
|
||||
|
@ -1,6 +1,6 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup PIOS PIOS Core hardware abstraction layer
|
||||
* @addtogroup PIOS PIOS Core haoftware; you can rnedtt
|
||||
* @{
|
||||
* @addtogroup PIOS_MPU6000 MPU6000 Functions
|
||||
* @brief Deals with the hardware interface to the 3-axis gyro
|
||||
@ -13,8 +13,8 @@
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
/*istribu
|
||||
* This program is free software; you can rnedtt ad/oe ir modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
@ -30,20 +30,40 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#include <pios_mpu6000.h>
|
||||
#ifdef PIOS_INCLUDE_MPU6000
|
||||
|
||||
#include <stdint.h>
|
||||
#include <pios_constants.h>
|
||||
#include <pios_sensors.h>
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
enum pios_mpu6000_dev_magic {
|
||||
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
|
||||
};
|
||||
|
||||
// sensor driver interface
|
||||
bool PIOS_MPU6000_driver_Test(uintptr_t context);
|
||||
void PIOS_MPU6000_driver_Reset(uintptr_t context);
|
||||
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
|
||||
QueueHandle_t PIOS_MPU6000_driver_get_queue(uintptr_t context);
|
||||
|
||||
const PIOS_SENSORS_Driver PIOS_MPU6000_Driver = {
|
||||
.test = PIOS_MPU6000_driver_Test,
|
||||
.poll = NULL,
|
||||
.fetch = NULL,
|
||||
.reset = PIOS_MPU6000_driver_Reset,
|
||||
.get_queue = PIOS_MPU6000_driver_get_queue,
|
||||
.get_scale = PIOS_MPU6000_driver_get_scale,
|
||||
.is_polled = false,
|
||||
};
|
||||
//
|
||||
|
||||
|
||||
struct mpu6000_dev {
|
||||
uint32_t spi_id;
|
||||
uint32_t slave_num;
|
||||
xQueueHandle queue;
|
||||
uint32_t spi_id;
|
||||
uint32_t slave_num;
|
||||
QueueHandle_t queue;
|
||||
const struct pios_mpu6000_cfg *cfg;
|
||||
enum pios_mpu6000_range gyro_range;
|
||||
enum pios_mpu6000_accel_range accel_range;
|
||||
@ -51,26 +71,19 @@ struct mpu6000_dev {
|
||||
enum pios_mpu6000_dev_magic magic;
|
||||
};
|
||||
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
#define PIOS_MPU6000_SAMPLES_BYTES 14
|
||||
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB
|
||||
#else
|
||||
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_TEMP_OUT_MSB
|
||||
#define PIOS_MPU6000_SAMPLES_BYTES 8
|
||||
#endif
|
||||
|
||||
typedef union {
|
||||
uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
|
||||
struct {
|
||||
uint8_t dummy;
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
uint8_t Accel_X_h;
|
||||
uint8_t Accel_X_l;
|
||||
uint8_t Accel_Y_h;
|
||||
uint8_t Accel_Y_l;
|
||||
uint8_t Accel_Z_h;
|
||||
uint8_t Accel_Z_l;
|
||||
#endif
|
||||
uint8_t Temperature_h;
|
||||
uint8_t Temperature_l;
|
||||
uint8_t Gyro_X_h;
|
||||
@ -88,7 +101,9 @@ typedef union {
|
||||
static struct mpu6000_dev *dev;
|
||||
volatile bool mpu6000_configured = false;
|
||||
static mpu6000_data_t mpu6000_data;
|
||||
|
||||
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
|
||||
#define SENSOR_COUNT 2
|
||||
#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
|
||||
// ! Private functions
|
||||
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
|
||||
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
|
||||
@ -97,8 +112,14 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
|
||||
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
|
||||
static void PIOS_MPU6000_SetSpeed(const bool fast);
|
||||
static bool PIOS_MPU6000_HandleData();
|
||||
static bool PIOS_MPU6000_ReadFifo(bool *woken);
|
||||
static bool PIOS_MPU6000_ReadSensor(bool *woken);
|
||||
|
||||
static int32_t PIOS_MPU6000_Test(void);
|
||||
|
||||
void PIOS_MPU6000_Register()
|
||||
{
|
||||
PIOS_SENSORS_Register(&PIOS_MPU6000_Driver, PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL, 0);
|
||||
}
|
||||
/**
|
||||
* @brief Allocate a new device
|
||||
*/
|
||||
@ -107,18 +128,16 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg
|
||||
struct mpu6000_dev *mpu6000_dev;
|
||||
|
||||
mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev));
|
||||
if (!mpu6000_dev) {
|
||||
return NULL;
|
||||
}
|
||||
PIOS_Assert(mpu6000_dev);
|
||||
|
||||
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
|
||||
|
||||
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
|
||||
if (mpu6000_dev->queue == NULL) {
|
||||
vPortFree(mpu6000_dev);
|
||||
return NULL;
|
||||
}
|
||||
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
|
||||
PIOS_Assert(mpu6000_dev->queue);
|
||||
|
||||
queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
|
||||
PIOS_Assert(queue_data);
|
||||
queue_data->count = SENSOR_COUNT;
|
||||
return mpu6000_dev;
|
||||
}
|
||||
|
||||
@ -211,15 +230,9 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
|
||||
}
|
||||
|
||||
// FIFO storage
|
||||
#if defined(PIOS_MPU6000_ACCEL)
|
||||
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) {
|
||||
;
|
||||
}
|
||||
#else
|
||||
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) {
|
||||
;
|
||||
}
|
||||
#endif
|
||||
PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
|
||||
// Interrupt configuration
|
||||
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) {
|
||||
@ -279,14 +292,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
|
||||
}
|
||||
|
||||
dev->gyro_range = gyroRange;
|
||||
#if defined(PIOS_MPU6000_ACCEL)
|
||||
// Set the accel range
|
||||
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
|
||||
;
|
||||
}
|
||||
|
||||
dev->accel_range = accelRange;
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -414,11 +425,10 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read current X, Z, Y values (in that order)
|
||||
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
|
||||
* @brief Perform a dummy read in order to restart interrupt generation
|
||||
* \returns 0 if succesful
|
||||
*/
|
||||
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
|
||||
int32_t PIOS_MPU6000_DummyReadGyros()
|
||||
{
|
||||
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
|
||||
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
|
||||
@ -434,10 +444,6 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
|
||||
|
||||
PIOS_MPU6000_ReleaseBus();
|
||||
|
||||
data->gyro_x = rec[1] << 8 | rec[2];
|
||||
data->gyro_y = rec[3] << 8 | rec[4];
|
||||
data->gyro_z = rec[5] << 8 | rec[6];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -469,7 +475,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
|
||||
}
|
||||
|
||||
|
||||
float PIOS_MPU6000_GetScale()
|
||||
static float PIOS_MPU6000_GetScale()
|
||||
{
|
||||
switch (dev->gyro_range) {
|
||||
case PIOS_MPU6000_SCALE_250_DEG:
|
||||
@ -487,7 +493,7 @@ float PIOS_MPU6000_GetScale()
|
||||
return 0;
|
||||
}
|
||||
|
||||
float PIOS_MPU6000_GetAccelScale()
|
||||
static float PIOS_MPU6000_GetAccelScale()
|
||||
{
|
||||
switch (dev->accel_range) {
|
||||
case PIOS_MPU6000_ACCEL_2G:
|
||||
@ -510,7 +516,7 @@ float PIOS_MPU6000_GetAccelScale()
|
||||
* \return 0 if test succeeded
|
||||
* \return non-zero value if test succeeded
|
||||
*/
|
||||
int32_t PIOS_MPU6000_Test(void)
|
||||
static int32_t PIOS_MPU6000_Test(void)
|
||||
{
|
||||
/* Verify that ID matches (MPU6000 ID is 0x69) */
|
||||
int32_t mpu6000_id = PIOS_MPU6000_ReadID();
|
||||
@ -526,172 +532,76 @@ int32_t PIOS_MPU6000_Test(void)
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR
|
||||
* @return The register value or -1 on failure to claim the bus
|
||||
*/
|
||||
static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
|
||||
{
|
||||
/* Interrupt Status register can be read at high SPI clock speed */
|
||||
uint8_t data;
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
|
||||
data = PIOS_SPI_TransferByte(dev->spi_id, 0);
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
return data;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Resets the MPU6000 FIFO from an ISR
|
||||
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
|
||||
* task has is now eligible to run, else unchanged
|
||||
* @return 0 if operation was successful
|
||||
* @return -1 if unable to claim SPI bus
|
||||
* @return -2 if write to the device failed
|
||||
*/
|
||||
static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
|
||||
{
|
||||
int32_t result = 0;
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
/* Reset FIFO. */
|
||||
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
|
||||
result = -2;
|
||||
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
|
||||
result = -2;
|
||||
}
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Obtains the number of bytes in the FIFO. Call from ISR only.
|
||||
* @return the number of bytes in the FIFO
|
||||
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
|
||||
* task has is now eligible to run, else unchanged
|
||||
*/
|
||||
static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
|
||||
{
|
||||
uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
|
||||
uint8_t mpu6000_rec_buf[3];
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
return -1;
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
|
||||
return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief EXTI IRQ Handler. Read all the data from onboard buffer
|
||||
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
|
||||
* @return a boleoan to the EXTI IRQ Handler wrapper indicating if a
|
||||
* higher priority task is now eligible to run
|
||||
*/
|
||||
uint32_t mpu6000_irq = 0;
|
||||
int32_t mpu6000_count;
|
||||
uint32_t mpu6000_fifo_backup = 0;
|
||||
|
||||
uint8_t mpu6000_last_read_count = 0;
|
||||
uint32_t mpu6000_fails = 0;
|
||||
|
||||
uint32_t mpu6000_interval_us;
|
||||
uint32_t mpu6000_time_us;
|
||||
uint32_t mpu6000_transfer_size;
|
||||
|
||||
bool PIOS_MPU6000_IRQHandler(void)
|
||||
{
|
||||
bool woken = false;
|
||||
static uint32_t timeval;
|
||||
|
||||
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
if (!mpu6000_configured) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool read_ok = false;
|
||||
if (dev->cfg->User_ctl & PIOS_MPU6000_USERCTL_FIFO_EN) {
|
||||
read_ok = PIOS_MPU6000_ReadFifo(&woken);
|
||||
} else {
|
||||
read_ok = PIOS_MPU6000_ReadSensor(&woken);
|
||||
}
|
||||
read_ok = PIOS_MPU6000_ReadSensor(&woken);
|
||||
|
||||
if (read_ok) {
|
||||
bool woken2 = PIOS_MPU6000_HandleData();
|
||||
woken |= woken2;
|
||||
}
|
||||
|
||||
mpu6000_irq++;
|
||||
|
||||
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
|
||||
|
||||
return woken;
|
||||
}
|
||||
|
||||
static bool PIOS_MPU6000_HandleData()
|
||||
{
|
||||
if (!queue_data) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
|
||||
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
|
||||
// to our convention
|
||||
|
||||
static struct pios_mpu6000_data data;
|
||||
|
||||
// Currently we only support rotations on top so switch X/Y accordingly
|
||||
switch (dev->cfg->orientation) {
|
||||
case PIOS_MPU6000_TOP_0DEG:
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
|
||||
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
|
||||
#endif
|
||||
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
|
||||
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
|
||||
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
|
||||
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
|
||||
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
|
||||
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
|
||||
break;
|
||||
case PIOS_MPU6000_TOP_90DEG:
|
||||
// -1 to bring it back to -32768 +32767 range
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
|
||||
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
|
||||
#endif
|
||||
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
|
||||
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
|
||||
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
|
||||
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
|
||||
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
|
||||
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
|
||||
break;
|
||||
case PIOS_MPU6000_TOP_180DEG:
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
|
||||
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
|
||||
#endif
|
||||
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
|
||||
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
|
||||
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
|
||||
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
|
||||
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
|
||||
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
|
||||
break;
|
||||
case PIOS_MPU6000_TOP_270DEG:
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
|
||||
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
|
||||
#endif
|
||||
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
|
||||
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
|
||||
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
|
||||
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
|
||||
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
|
||||
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
|
||||
break;
|
||||
}
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
data.accel_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
|
||||
#endif
|
||||
data.gyro_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
|
||||
data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
|
||||
queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
|
||||
queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
|
||||
const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
|
||||
queue_data->temperature = 3500 + ((float)(temp + 512)) * (1.0f / 3.4f);
|
||||
|
||||
BaseType_t higherPriorityTaskWoken;
|
||||
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
|
||||
xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
|
||||
return higherPriorityTaskWoken == pdTRUE;
|
||||
}
|
||||
|
||||
@ -704,72 +614,33 @@ static bool PIOS_MPU6000_ReadSensor(bool *woken)
|
||||
}
|
||||
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
mpu6000_fails++;
|
||||
return false;
|
||||
}
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool PIOS_MPU6000_ReadFifo(bool *woken)
|
||||
// Sensor driver implementation
|
||||
bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
/* Temporary fix for OP-1049. Expected to be superceded for next major release
|
||||
* by code changes for OP-1039.
|
||||
* Read interrupt status register to check for FIFO overflow. Must be the
|
||||
* first read after interrupt, in case the device is configured so that
|
||||
* any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
|
||||
* interrupt config register) */
|
||||
int32_t result;
|
||||
return !PIOS_MPU6000_Test();
|
||||
}
|
||||
|
||||
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
|
||||
return false;
|
||||
}
|
||||
if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
|
||||
/* The FIFO has overflowed, so reset it,
|
||||
* to enable sample sync to be recovered.
|
||||
* If the reset fails, we are in trouble, but
|
||||
* we keep trying on subsequent interrupts. */
|
||||
PIOS_MPU6000_ResetFifoISR(woken);
|
||||
/* Return and wait for the next new sample. */
|
||||
return false;
|
||||
}
|
||||
void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
PIOS_MPU6000_DummyReadGyros();
|
||||
}
|
||||
|
||||
/* Usual case - FIFO has not overflowed. */
|
||||
mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
|
||||
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
|
||||
return false;
|
||||
}
|
||||
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
|
||||
{
|
||||
PIOS_Assert(size >= 2);
|
||||
scales[0] = PIOS_MPU6000_GetAccelScale();
|
||||
scales[1] = PIOS_MPU6000_GetScale();
|
||||
}
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
|
||||
|
||||
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
mpu6000_fails++;
|
||||
return false;
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
|
||||
// In the case where extras samples backed up grabbed an extra
|
||||
if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
|
||||
mpu6000_fifo_backup++;
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
mpu6000_fails++;
|
||||
return false;
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
}
|
||||
return true;
|
||||
QueueHandle_t PIOS_MPU6000_driver_get_queue(__attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
return dev->queue;
|
||||
}
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
|
@ -29,17 +29,40 @@
|
||||
*/
|
||||
|
||||
#include "pios.h"
|
||||
|
||||
#ifdef PIOS_INCLUDE_MS5611
|
||||
|
||||
#include <pios_ms5611.h>
|
||||
#define POW2(x) (1 << x)
|
||||
|
||||
// TODO: Clean this up. Getting around old constant.
|
||||
#define PIOS_MS5611_OVERSAMPLING oversampling
|
||||
#define PIOS_MS5611_OVERSAMPLING oversampling
|
||||
|
||||
// Option to change the interleave between Temp and Pressure conversions
|
||||
// Undef for normal operation
|
||||
#define PIOS_MS5611_SLOW_TEMP_RATE 20
|
||||
#ifndef PIOS_MS5611_SLOW_TEMP_RATE
|
||||
#define PIOS_MS5611_SLOW_TEMP_RATE 1
|
||||
#endif
|
||||
|
||||
/* Local Types */
|
||||
typedef struct {
|
||||
uint16_t C[6];
|
||||
} MS5611CalibDataTypeDef;
|
||||
|
||||
typedef enum {
|
||||
MS5611_CONVERSION_TYPE_None = 0,
|
||||
MS5611_CONVERSION_TYPE_PressureConv,
|
||||
MS5611_CONVERSION_TYPE_TemperatureConv
|
||||
} ConversionTypeTypeDef;
|
||||
|
||||
typedef enum {
|
||||
MS5611_FSM_INIT = 0,
|
||||
MS5611_FSM_TEMPERATURE,
|
||||
MS5611_FSM_PRESSURE,
|
||||
MS5611_FSM_CALCULATE,
|
||||
} MS5611_FSM_State;
|
||||
|
||||
/* Glocal Variables */
|
||||
ConversionTypeTypeDef CurrentRead;
|
||||
ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
|
||||
|
||||
/* Local Variables */
|
||||
MS5611CalibDataTypeDef CalibData;
|
||||
@ -50,8 +73,14 @@ static uint32_t RawPressure;
|
||||
static int64_t Pressure;
|
||||
static int64_t Temperature;
|
||||
static int32_t lastConversionStart;
|
||||
|
||||
static uint32_t conversionDelayMs;
|
||||
static uint32_t conversionDelayUs;
|
||||
|
||||
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
|
||||
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
|
||||
static uint32_t PIOS_MS5611_GetDelay();
|
||||
static uint32_t PIOS_MS5611_GetDelayUs();
|
||||
|
||||
// Second order temperature compensation. Temperature offset
|
||||
static int64_t compensation_t2;
|
||||
@ -60,17 +89,36 @@ static int64_t compensation_t2;
|
||||
static uint32_t oversampling;
|
||||
static const struct pios_ms5611_cfg *dev_cfg;
|
||||
static int32_t i2c_id;
|
||||
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
|
||||
|
||||
// sensor driver interface
|
||||
bool PIOS_MS5611_driver_Test(uintptr_t context);
|
||||
void PIOS_MS5611_driver_Reset(uintptr_t context);
|
||||
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
|
||||
void PIOS_MS5611_driver_fetch(void *, uint8_t size, uintptr_t context);
|
||||
bool PIOS_MS5611_driver_poll(uintptr_t context);
|
||||
|
||||
const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
|
||||
.test = PIOS_MS5611_driver_Test,
|
||||
.poll = PIOS_MS5611_driver_poll,
|
||||
.fetch = PIOS_MS5611_driver_fetch,
|
||||
.reset = PIOS_MS5611_driver_Reset,
|
||||
.get_queue = NULL,
|
||||
.get_scale = PIOS_MS5611_driver_get_scale,
|
||||
.is_polled = true,
|
||||
};
|
||||
/**
|
||||
* Initialise the MS5611 sensor
|
||||
*/
|
||||
int32_t ms5611_read_flag;
|
||||
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
|
||||
{
|
||||
i2c_id = i2c_device;
|
||||
i2c_id = i2c_device;
|
||||
|
||||
oversampling = cfg->oversampling;
|
||||
conversionDelayMs = PIOS_MS5611_GetDelay();
|
||||
conversionDelayUs = PIOS_MS5611_GetDelayUs();
|
||||
|
||||
dev_cfg = cfg; // Store cfg before enabling interrupt
|
||||
|
||||
PIOS_MS5611_WriteCommand(MS5611_RESET);
|
||||
@ -80,10 +128,10 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
|
||||
|
||||
// reset temperature compensation values
|
||||
compensation_t2 = 0;
|
||||
|
||||
/* Calibration parameters */
|
||||
for (int i = 0; i < 6; i++) {
|
||||
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
|
||||
while (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2)) {}
|
||||
;
|
||||
CalibData.C[i] = (data[0] << 8) | data[1];
|
||||
}
|
||||
}
|
||||
@ -96,11 +144,11 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
|
||||
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
|
||||
{
|
||||
/* Start the conversion */
|
||||
if (Type == TemperatureConv) {
|
||||
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
|
||||
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
|
||||
continue;
|
||||
}
|
||||
} else if (Type == PressureConv) {
|
||||
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
|
||||
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
|
||||
continue;
|
||||
}
|
||||
@ -114,7 +162,7 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
|
||||
/**
|
||||
* @brief Return the delay for the current osr
|
||||
*/
|
||||
int32_t PIOS_MS5611_GetDelay()
|
||||
static uint32_t PIOS_MS5611_GetDelay()
|
||||
{
|
||||
switch (oversampling) {
|
||||
case MS5611_OSR_256:
|
||||
@ -141,7 +189,7 @@ int32_t PIOS_MS5611_GetDelay()
|
||||
/**
|
||||
* @brief Return the delay for the current osr in uS
|
||||
*/
|
||||
uint32_t PIOS_MS5611_GetDelayUs()
|
||||
static uint32_t PIOS_MS5611_GetDelayUs()
|
||||
{
|
||||
switch (oversampling) {
|
||||
case MS5611_OSR_256:
|
||||
@ -167,7 +215,7 @@ uint32_t PIOS_MS5611_GetDelayUs()
|
||||
|
||||
/**
|
||||
* Read the ADC conversion value (once ADC conversion has completed)
|
||||
* \return 0 if successfully read the ADC, -1 if failed
|
||||
* \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred
|
||||
*/
|
||||
int32_t PIOS_MS5611_ReadADC(void)
|
||||
{
|
||||
@ -177,16 +225,19 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
Data[1] = 0;
|
||||
Data[2] = 0;
|
||||
|
||||
while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) {
|
||||
vTaskDelay(0);
|
||||
if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
|
||||
return -2;
|
||||
}
|
||||
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
|
||||
return -1;
|
||||
}
|
||||
static int64_t deltaTemp;
|
||||
|
||||
/* Read and store the 16bit result */
|
||||
if (CurrentRead == TemperatureConv) {
|
||||
if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
|
||||
/* Read the temperature conversion */
|
||||
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
|
||||
return -1;
|
||||
return -2;
|
||||
}
|
||||
|
||||
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
|
||||
@ -205,7 +256,7 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
|
||||
/* Read the pressure conversion */
|
||||
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
|
||||
return -1;
|
||||
return -2;
|
||||
}
|
||||
// check if temperature is less than 20°C
|
||||
if (Temperature < 2000) {
|
||||
@ -248,18 +299,18 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
/**
|
||||
* Return the most recently computed temperature in kPa
|
||||
*/
|
||||
float PIOS_MS5611_GetTemperature(void)
|
||||
static float PIOS_MS5611_GetTemperature(void)
|
||||
{
|
||||
// Apply the second order low and very low temperature compensation offset
|
||||
return ((float)(Temperature - compensation_t2)) / 100.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the most recently computed pressure in kPa
|
||||
* Return the most recently computed pressure in Pa
|
||||
*/
|
||||
float PIOS_MS5611_GetPressure(void)
|
||||
static float PIOS_MS5611_GetPressure(void)
|
||||
{
|
||||
return ((float)Pressure) / 1000.0f;
|
||||
return (float)Pressure;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -270,7 +321,7 @@ float PIOS_MS5611_GetPressure(void)
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
*/
|
||||
int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
|
||||
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
|
||||
{
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
@ -300,7 +351,7 @@ int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
|
||||
* \return 0 if operation was successful
|
||||
* \return -1 if error during I2C transfer
|
||||
*/
|
||||
int32_t PIOS_MS5611_WriteCommand(uint8_t command)
|
||||
static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
|
||||
{
|
||||
const struct pios_i2c_txn txn_list[] = {
|
||||
{
|
||||
@ -326,16 +377,16 @@ int32_t PIOS_MS5611_Test()
|
||||
int32_t cur_value = 0;
|
||||
|
||||
cur_value = Temperature;
|
||||
PIOS_MS5611_StartADC(TemperatureConv);
|
||||
PIOS_DELAY_WaitmS(5);
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
PIOS_MS5611_ReadADC();
|
||||
if (cur_value == Temperature) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
cur_value = Pressure;
|
||||
PIOS_MS5611_StartADC(PressureConv);
|
||||
PIOS_DELAY_WaitmS(26);
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
|
||||
PIOS_DELAY_WaitmS(10);
|
||||
PIOS_MS5611_ReadADC();
|
||||
if (cur_value == Pressure) {
|
||||
return -1;
|
||||
@ -344,6 +395,80 @@ int32_t PIOS_MS5611_Test()
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/* PIOS sensor driver implementation */
|
||||
void PIOS_MS5611_Register()
|
||||
{
|
||||
PIOS_SENSORS_Register(&PIOS_MS5611_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0);
|
||||
}
|
||||
|
||||
bool PIOS_MS5611_driver_Test(__attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
return true; // !PIOS_MS5611_Test();
|
||||
}
|
||||
|
||||
void PIOS_MS5611_driver_Reset(__attribute__((unused)) uintptr_t context) {}
|
||||
|
||||
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
PIOS_Assert(size > 0);
|
||||
scales[0] = 1;
|
||||
}
|
||||
|
||||
void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
PIOS_Assert(data);
|
||||
memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp));
|
||||
}
|
||||
|
||||
bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
|
||||
{
|
||||
static uint8_t temp_press_interleave_count = 1;
|
||||
static MS5611_FSM_State next_state = MS5611_FSM_INIT;
|
||||
|
||||
int32_t conversionResult = PIOS_MS5611_ReadADC();
|
||||
|
||||
if (__builtin_expect(conversionResult == -1, 1)) {
|
||||
return false; // wait for conversion to complete
|
||||
} else if (__builtin_expect(conversionResult == -2, 0)) {
|
||||
temp_press_interleave_count = 1;
|
||||
next_state = MS5611_FSM_INIT;
|
||||
}
|
||||
switch (next_state) {
|
||||
case MS5611_FSM_INIT:
|
||||
case MS5611_FSM_TEMPERATURE:
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
|
||||
next_state = MS5611_FSM_PRESSURE;
|
||||
return false;
|
||||
|
||||
case MS5611_FSM_PRESSURE:
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
|
||||
next_state = MS5611_FSM_CALCULATE;
|
||||
return false;
|
||||
|
||||
case MS5611_FSM_CALCULATE:
|
||||
temp_press_interleave_count--;
|
||||
if (!temp_press_interleave_count) {
|
||||
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
|
||||
next_state = MS5611_FSM_PRESSURE;
|
||||
} else {
|
||||
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
|
||||
next_state = MS5611_FSM_CALCULATE;
|
||||
}
|
||||
|
||||
results.temperature = PIOS_MS5611_GetTemperature();
|
||||
results.sample = PIOS_MS5611_GetPressure();
|
||||
return true;
|
||||
|
||||
default:
|
||||
// it should not be there
|
||||
PIOS_Assert(0);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
#endif /* PIOS_INCLUDE_MS5611 */
|
||||
|
||||
/**
|
||||
|
65
flight/pios/common/pios_sensors.c
Normal file
65
flight/pios/common/pios_sensors.c
Normal file
@ -0,0 +1,65 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_sensors.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief PiOS sensors handling
|
||||
* --
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <pios_mem.h>
|
||||
#include <pios_sensors.h>
|
||||
#include <string.h>
|
||||
|
||||
// private variables
|
||||
|
||||
static PIOS_SENSORS_Instance *sensor_list = 0;
|
||||
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context)
|
||||
{
|
||||
PIOS_SENSORS_Instance *instance = (PIOS_SENSORS_Instance *)pios_malloc(sizeof(PIOS_SENSORS_Instance));
|
||||
|
||||
instance->driver = driver;
|
||||
instance->type = type;
|
||||
instance->context = context;
|
||||
instance->next = NULL;
|
||||
LL_APPEND(sensor_list, instance);
|
||||
return instance;
|
||||
}
|
||||
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList()
|
||||
{
|
||||
return sensor_list;
|
||||
}
|
||||
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type)
|
||||
{
|
||||
if (!previous_instance) {
|
||||
previous_instance = sensor_list;
|
||||
}
|
||||
PIOS_SENSORS_Instance *sensor;
|
||||
|
||||
LL_FOREACH((PIOS_SENSORS_Instance *)previous_instance, sensor) {
|
||||
if (sensor->type && type) {
|
||||
return sensor;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
@ -31,6 +31,7 @@
|
||||
#ifndef PIOS_HMC5x83_H
|
||||
#define PIOS_HMC5x83_H
|
||||
#include <stdint.h>
|
||||
#include <pios_sensors.h>
|
||||
/* HMC5x83 Addresses */
|
||||
#define PIOS_HMC5x83_I2C_ADDR 0x1E
|
||||
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
|
||||
@ -123,12 +124,16 @@ struct pios_hmc5x83_cfg {
|
||||
|
||||
/* Public Functions */
|
||||
extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
|
||||
extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler);
|
||||
|
||||
extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
|
||||
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
|
||||
extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
|
||||
extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
|
||||
extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
|
||||
|
||||
extern const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver;
|
||||
|
||||
#endif /* PIOS_HMC5x83_H */
|
||||
|
||||
/**
|
||||
|
@ -53,9 +53,11 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
|
||||
vPortEnterCritical();
|
||||
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
|
||||
counter->value = newValue;
|
||||
counter->max--;
|
||||
if (counter->value > counter->max) {
|
||||
counter->max = counter->value;
|
||||
}
|
||||
counter->min++;
|
||||
if (counter->value < counter->min) {
|
||||
counter->min = counter->value;
|
||||
}
|
||||
@ -88,9 +90,11 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
|
||||
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
|
||||
|
||||
counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
|
||||
counter->max--;
|
||||
if (counter->value > counter->max) {
|
||||
counter->max = counter->value;
|
||||
}
|
||||
counter->min++;
|
||||
if (counter->value < counter->min) {
|
||||
counter->min = counter->value;
|
||||
}
|
||||
@ -110,9 +114,11 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
|
||||
vPortEnterCritical();
|
||||
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
|
||||
counter->value = (counter->value * 15 + period) / 16;
|
||||
counter->max--;
|
||||
if ((int32_t)period > counter->max) {
|
||||
counter->max = period;
|
||||
}
|
||||
counter->min++;
|
||||
if ((int32_t)period < counter->min) {
|
||||
counter->min = period;
|
||||
}
|
||||
|
@ -72,6 +72,9 @@
|
||||
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
|
||||
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
|
||||
|
||||
// helper macros for LPFs
|
||||
#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
|
||||
|
||||
// Useful math macros
|
||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
|
@ -26,6 +26,7 @@
|
||||
*/
|
||||
#ifndef PIOS_MEM_H
|
||||
#define PIOS_MEM_H
|
||||
#include <strings.h>
|
||||
|
||||
void *pios_fastheapmalloc(size_t size);
|
||||
|
||||
|
@ -31,6 +31,7 @@
|
||||
|
||||
#ifndef PIOS_MPU6000_H
|
||||
#define PIOS_MPU6000_H
|
||||
#include <pios_sensors.h>
|
||||
|
||||
/* MPU6000 Addresses */
|
||||
#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19
|
||||
@ -131,18 +132,6 @@ enum pios_mpu6000_orientation { // clockwise rotation from board forward
|
||||
PIOS_MPU6000_TOP_270DEG = 0x03
|
||||
};
|
||||
|
||||
struct pios_mpu6000_data {
|
||||
int16_t gyro_x;
|
||||
int16_t gyro_y;
|
||||
int16_t gyro_z;
|
||||
#if defined(PIOS_MPU6000_ACCEL)
|
||||
int16_t accel_x;
|
||||
int16_t accel_y;
|
||||
int16_t accel_z;
|
||||
#endif /* PIOS_MPU6000_ACCEL */
|
||||
int16_t temperature;
|
||||
};
|
||||
|
||||
struct pios_mpu6000_cfg {
|
||||
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
|
||||
|
||||
@ -167,14 +156,11 @@ struct pios_mpu6000_cfg {
|
||||
/* Public Functions */
|
||||
extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg);
|
||||
extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting);
|
||||
extern xQueueHandle PIOS_MPU6000_GetQueue();
|
||||
extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer);
|
||||
extern int32_t PIOS_MPU6000_ReadID();
|
||||
extern int32_t PIOS_MPU6000_Test();
|
||||
extern float PIOS_MPU6000_GetScale();
|
||||
extern float PIOS_MPU6000_GetAccelScale();
|
||||
extern void PIOS_MPU6000_Register();
|
||||
extern bool PIOS_MPU6000_IRQHandler(void);
|
||||
|
||||
extern const PIOS_SENSORS_Driver PIOS_MPU6000_Driver;
|
||||
#endif /* PIOS_MPU6000_H */
|
||||
|
||||
/**
|
||||
|
@ -30,7 +30,7 @@
|
||||
|
||||
#ifndef PIOS_MS5611_H
|
||||
#define PIOS_MS5611_H
|
||||
|
||||
#include <pios_sensors.h>
|
||||
/* BMP085 Addresses */
|
||||
#define MS5611_I2C_ADDR 0x77
|
||||
#define MS5611_RESET 0x1E
|
||||
@ -40,17 +40,6 @@
|
||||
#define MS5611_PRES_ADDR 0x40
|
||||
#define MS5611_TEMP_ADDR 0x50
|
||||
#define MS5611_ADC_MSB 0xF6
|
||||
#define MS5611_P0 101.3250f
|
||||
|
||||
/* Local Types */
|
||||
typedef struct {
|
||||
uint16_t C[6];
|
||||
} MS5611CalibDataTypeDef;
|
||||
|
||||
typedef enum {
|
||||
PressureConv,
|
||||
TemperatureConv
|
||||
} ConversionTypeTypeDef;
|
||||
|
||||
struct pios_ms5611_cfg {
|
||||
uint32_t oversampling;
|
||||
@ -66,12 +55,8 @@ enum pios_ms5611_osr {
|
||||
|
||||
/* Public Functions */
|
||||
extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device);
|
||||
extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type);
|
||||
extern int32_t PIOS_MS5611_ReadADC(void);
|
||||
extern float PIOS_MS5611_GetTemperature(void);
|
||||
extern float PIOS_MS5611_GetPressure(void);
|
||||
extern int32_t PIOS_MS5611_Test();
|
||||
extern int32_t PIOS_MS5611_GetDelay();
|
||||
extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver;
|
||||
void PIOS_MS5611_Register();
|
||||
|
||||
#endif /* PIOS_MS5611_H */
|
||||
|
||||
|
190
flight/pios/inc/pios_sensors.h
Normal file
190
flight/pios/inc/pios_sensors.h
Normal file
@ -0,0 +1,190 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_sensors.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief PiOS sensors handling
|
||||
* --
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef PIOS_SENSORS_H
|
||||
#define PIOS_SENSORS_H
|
||||
#include <pios.h>
|
||||
#include <utlist.h>
|
||||
#include <stdint.h>
|
||||
#include <vectors.h>
|
||||
// needed for debug APIs.
|
||||
|
||||
typedef bool (*PIOS_SENSORS_test_function)(uintptr_t context);
|
||||
typedef void (*PIOS_SENSORS_reset_function)(uintptr_t context);
|
||||
typedef bool (*PIOS_SENSORS_poll_function)(uintptr_t context);
|
||||
typedef void (*PIOS_SENSORS_fetch_function)(void *samples, uint8_t size, uintptr_t context);
|
||||
/**
|
||||
* return an array with current scale for the instance.
|
||||
* Instances with multiples sensors returns several value in the same
|
||||
* order as they appear in PIOS_SENSORS_TYPE enums.
|
||||
*/
|
||||
typedef void (*PIOS_SENSORS_get_scale_function)(float *, uint8_t size, uintptr_t context);
|
||||
typedef QueueHandle_t (*PIOS_SENSORS_get_queue_function)(uintptr_t context);
|
||||
|
||||
typedef struct PIOS_SENSORS_Driver {
|
||||
PIOS_SENSORS_test_function test; // called at startup to test the sensor
|
||||
PIOS_SENSORS_poll_function poll; // called to check whether data are available for polled sensors
|
||||
PIOS_SENSORS_fetch_function fetch; // called to fetch data for polled sensors
|
||||
PIOS_SENSORS_reset_function reset; // reset sensor. for example if data are not received in the allotted time
|
||||
PIOS_SENSORS_get_queue_function get_queue; // get the queue reference
|
||||
PIOS_SENSORS_get_scale_function get_scale; // return scales for the sensors
|
||||
bool is_polled;
|
||||
} PIOS_SENSORS_Driver;
|
||||
|
||||
typedef enum PIOS_SENSORS_TYPE {
|
||||
PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
|
||||
PIOS_SENSORS_TYPE_3AXIS_GYRO = 0x02,
|
||||
PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL = 0x03,
|
||||
PIOS_SENSORS_TYPE_3AXIS_MAG = 0x04,
|
||||
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08,
|
||||
} 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 {
|
||||
const PIOS_SENSORS_Driver *driver;
|
||||
uintptr_t context;
|
||||
struct PIOS_SENSORS_Instance *next;
|
||||
uint8_t type;
|
||||
} PIOS_SENSORS_Instance;
|
||||
|
||||
/**
|
||||
* A 3d Accel sample with temperature
|
||||
*/
|
||||
typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp {
|
||||
uint16_t count; // number of sensor instances
|
||||
int16_t temperature; // Degrees Celsius * 100
|
||||
Vector3i16 sample[];
|
||||
} PIOS_SENSORS_3Axis_SensorsWithTemp;
|
||||
|
||||
typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp {
|
||||
float temperature; // Degrees Celsius
|
||||
float sample; // sample
|
||||
} PIOS_SENSORS_1Axis_SensorsWithTemp;
|
||||
|
||||
/**
|
||||
* Register a new sensor instance with sensor subsystem
|
||||
* @param driver sensor driver
|
||||
* @param type sensor type @ref PIOS_SENSORS_TYPE
|
||||
* @param context context to be passed to sensor driver
|
||||
* @return the new sensor instance
|
||||
*/
|
||||
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context);
|
||||
/**
|
||||
* return the list of registered sensors.
|
||||
* @return the first sensor instance in the list.
|
||||
*/
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
|
||||
|
||||
/**
|
||||
* Perform sensor test and return true if passed
|
||||
* @param sensor instance to test
|
||||
* @return true if test passes
|
||||
*/
|
||||
static inline bool PIOS_SENSORS_Test(const PIOS_SENSORS_Instance *sensor)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
|
||||
if (!sensor->driver->test) {
|
||||
return true;
|
||||
} else {
|
||||
return sensor->driver->test(sensor->context);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Poll sensor for new values
|
||||
* @param sensor instance to poll
|
||||
* @return true if sensor has samples available
|
||||
*/
|
||||
static inline bool PIOS_SENSORS_Poll(const PIOS_SENSORS_Instance *sensor)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
|
||||
if (!sensor->driver->poll) {
|
||||
return true;
|
||||
} else {
|
||||
return sensor->driver->poll(sensor->context);
|
||||
}
|
||||
}
|
||||
/**
|
||||
*
|
||||
* @param sensor
|
||||
* @param samples
|
||||
* @param size
|
||||
*/
|
||||
static inline void PIOS_SENSOR_Fetch(const PIOS_SENSORS_Instance *sensor, void *samples, uint8_t size)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
sensor->driver->fetch(samples, size, sensor->context);
|
||||
}
|
||||
|
||||
static inline void PIOS_SENSOR_Reset(const PIOS_SENSORS_Instance *sensor)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
sensor->driver->reset(sensor->context);
|
||||
}
|
||||
|
||||
/**
|
||||
* retrieve the sensor queue
|
||||
* @param sensor
|
||||
* @return sensor queue or null if not supported
|
||||
*/
|
||||
static inline QueueHandle_t PIOS_SENSORS_GetQueue(const PIOS_SENSORS_Instance *sensor)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
if (!sensor->driver->get_queue) {
|
||||
return NULL;
|
||||
}
|
||||
return sensor->driver->get_queue(sensor->context);
|
||||
}
|
||||
/**
|
||||
* Get the sensor scales.
|
||||
* @param sensor sensor instance
|
||||
* @param scales float array that will contains scales
|
||||
* @param size number of floats within the array
|
||||
*/
|
||||
static inline void PIOS_SENSORS_GetScales(const PIOS_SENSORS_Instance *sensor, float *scales, uint8_t size)
|
||||
{
|
||||
PIOS_Assert(sensor);
|
||||
sensor->driver->get_scale(scales, size, sensor->context);
|
||||
}
|
||||
/**
|
||||
* return head of sensor list
|
||||
* @return head of sensor list
|
||||
*/
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
|
||||
|
||||
/**
|
||||
* Return the first occurrence of specified sensor type
|
||||
* @param previous_instance last instance found or 0
|
||||
* @param type type of sensor to find
|
||||
* @return the first occurence found or NULL
|
||||
*/
|
||||
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type);
|
||||
|
||||
#endif /* PIOS_SENSORS_H */
|
@ -182,49 +182,6 @@
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* PIOS sensor interfaces */
|
||||
#ifdef PIOS_INCLUDE_ADXL345
|
||||
/* ADXL345 3-Axis Accelerometer */
|
||||
#include <pios_adxl345.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_BMA180
|
||||
/* BMA180 3-Axis Accelerometer */
|
||||
#include <pios_bma180.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_L3GD20
|
||||
/* L3GD20 3-Axis Gyro */
|
||||
#include <pios_l3gd20.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_MPU6000
|
||||
/* MPU6000 3-Axis Gyro/Accelerometer */
|
||||
/* #define PIOS_MPU6000_ACCEL */
|
||||
#include <pios_mpu6000.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_HMC5843
|
||||
/* HMC5843 3-Axis Digital Compass */
|
||||
#include <pios_hmc5843.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_HMC5X83
|
||||
/* HMC5883/HMC5983 3-Axis Digital Compass */
|
||||
/* #define PIOS_HMC5x83_HAS_GPIOS */
|
||||
#include <pios_hmc5x83.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_BMP085
|
||||
/* BMP085 Barometric Pressure Sensor */
|
||||
#include <pios_bmp085.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_MS5611
|
||||
/* MS5611 Barometric Pressure Sensor */
|
||||
#include <pios_ms5611.h>
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_MPXV
|
||||
/* MPXV5004, MPXV7002 based Airspeed Sensor */
|
||||
#include <pios_mpxv.h>
|
||||
|
@ -38,6 +38,10 @@
|
||||
#ifdef PIOS_INCLUDE_INSTRUMENTATION
|
||||
#include <pios_instrumentation.h>
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_ADXL345)
|
||||
#include <pios_adxl345.h>
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Pull in the board-specific static HW definitions.
|
||||
* Including .c files is a bit ugly but this allows all of
|
||||
@ -844,7 +848,7 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
|
||||
PIOS_MPU6000_CONFIG_Configure();
|
||||
init_test = PIOS_MPU6000_Test();
|
||||
init_test = !PIOS_MPU6000_Driver.test(0);
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
break;
|
||||
|
@ -205,8 +205,9 @@ void PIOS_SPI_mag_flash_irq_handler(void)
|
||||
#endif /* PIOS_INCLUDE_FLASH */
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
pios_hmc5x83_dev_t onboard_mag;
|
||||
#include "pios_hmc5x83.h"
|
||||
pios_hmc5x83_dev_t onboard_mag;
|
||||
|
||||
#ifdef PIOS_HMC5X83_HAS_GPIOS
|
||||
bool pios_board_mag_handler()
|
||||
{
|
||||
|
@ -31,7 +31,7 @@ MODULES += Osd/osdgen
|
||||
MODULES += Osd/osdinput
|
||||
MODULES += Osd/WavPlayer
|
||||
MODULES += GPS
|
||||
MODULES += Extensions/MagBaro
|
||||
#MODULES += Extensions/MagBaro
|
||||
MODULES += FirmwareIAP
|
||||
MODULES += Telemetry
|
||||
|
||||
|
@ -29,11 +29,8 @@ USE_DSP_LIB ?= NO
|
||||
|
||||
# List of mandatory modules to include
|
||||
MODULES += Sensors
|
||||
#MODULES += Attitude/revolution
|
||||
MODULES += StateEstimation # use instead of Attitude
|
||||
MODULES += Altitude/revolution
|
||||
MODULES += StateEstimation
|
||||
MODULES += Airspeed
|
||||
#MODULES += AltitudeHold # now integrated in Stabilization
|
||||
MODULES += Stabilization
|
||||
MODULES += ManualControl
|
||||
MODULES += Receiver
|
||||
|
@ -206,7 +206,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
|
||||
.orientation = PIOS_MPU6000_TOP_180DEG,
|
||||
.fast_prescaler = PIOS_SPI_PRESCALER_4,
|
||||
.std_prescaler = PIOS_SPI_PRESCALER_64,
|
||||
.max_downsample = 16,
|
||||
.max_downsample = 20,
|
||||
};
|
||||
#endif /* PIOS_INCLUDE_MPU6000 */
|
||||
|
||||
@ -922,20 +922,23 @@ void PIOS_Board_Init(void)
|
||||
PIOS_ADC_Init(&pios_adc_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
|
||||
PIOS_MPU6000_CONFIG_Configure();
|
||||
PIOS_MPU6000_Register();
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
|
||||
PIOS_HMC5x83_Register(onboard_mag);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_MS5611)
|
||||
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
|
||||
PIOS_MS5611_Register();
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
|
||||
PIOS_MPU6000_CONFIG_Configure();
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_WS2811
|
||||
#ifdef PIOS_INCLUDE_WS2811
|
||||
#include <pios_ws2811.h>
|
||||
HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
|
||||
HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);
|
||||
|
@ -29,11 +29,8 @@ USE_DSP_LIB ?= NO
|
||||
|
||||
# List of mandatory modules to include
|
||||
MODULES += Sensors
|
||||
#MODULES += Attitude/revolution
|
||||
MODULES += StateEstimation # use instead of Attitude
|
||||
MODULES += Altitude/revolution
|
||||
MODULES += StateEstimation
|
||||
MODULES += Airspeed
|
||||
#MODULES += AltitudeHold # now integrated in Stabilization
|
||||
MODULES += Stabilization
|
||||
MODULES += ManualControl
|
||||
MODULES += Receiver
|
||||
|
@ -82,13 +82,14 @@ void PIOS_ADC_DMC_irq_handler(void)
|
||||
#endif /* if defined(PIOS_INCLUDE_ADC) */
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
#include "pios_hmc5x83.h"
|
||||
|
||||
pios_hmc5x83_dev_t onboard_mag = 0;
|
||||
|
||||
bool pios_board_internal_mag_handler()
|
||||
{
|
||||
return PIOS_HMC5x83_IRQHandler(onboard_mag);
|
||||
}
|
||||
#include "pios_hmc5x83.h"
|
||||
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
|
||||
.vector = pios_board_internal_mag_handler,
|
||||
.line = EXTI_Line5,
|
||||
@ -893,19 +894,23 @@ void PIOS_Board_Init(void)
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0);
|
||||
PIOS_HMC5x83_Register(onboard_mag);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_MS5611)
|
||||
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
|
||||
PIOS_MS5611_Register();
|
||||
#endif
|
||||
|
||||
switch (bdinfo->board_rev) {
|
||||
case 0x01:
|
||||
#if defined(PIOS_INCLUDE_L3GD20)
|
||||
PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!!
|
||||
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
|
||||
PIOS_Assert(PIOS_L3GD20_Test() == 0);
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_BMA180)
|
||||
PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!!
|
||||
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
|
||||
PIOS_Assert(PIOS_BMA180_Test() == 0);
|
||||
#endif
|
||||
@ -914,6 +919,7 @@ void PIOS_Board_Init(void)
|
||||
#if defined(PIOS_INCLUDE_MPU6000)
|
||||
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
|
||||
PIOS_MPU6000_CONFIG_Configure();
|
||||
PIOS_MPU6000_Register();
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
|
@ -81,6 +81,7 @@ SRC += $(PIOSCOMMON)/pios_rfm22b_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_rcvr.c
|
||||
SRC += $(PIOSCOMMON)/pios_sbus.c
|
||||
SRC += $(PIOSCOMMON)/pios_sdcard.c
|
||||
SRC += $(PIOSCOMMON)/pios_sensors.c
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/sanitycheck.c
|
||||
|
@ -1,9 +1,10 @@
|
||||
<xml>
|
||||
<object name="MagSensor" singleinstance="true" settings="false" category="Sensors">
|
||||
<description>Calibrated sensor data from 3 axis magnetometer in MilliGauss.</description>
|
||||
<field name="x" 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="x" 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="temperature" units="deg C" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="10000"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user