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

Merge remote-tracking branch 'origin/amorale/OP-1658_sensor_overhaul_next' into next

Browse Source
This commit is contained in:
Alessio Morale 2015-02-22 16:28:04 +01:00
commit ad42ca7f16
32 changed files with 1036 additions and 912 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
flight/uavobjects/inc/utlist.h → flight/libraries/inc/utlist.h
View File

50
flight/modules/Altitude/revolution/inc/altitude.h → flight/libraries/math/vectors.h
View File

@ -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_ */

221
flight/modules/Altitude/revolution/altitude.c
View File

@ -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));
}
/**
* @}
* @}
*/

45
flight/modules/Attitude/attitude.c
View File

@ -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;

11
flight/modules/Sensors/inc/sensors.h
View File

@ -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

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

@ -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));
}
/**
* @}

2
flight/pios/common/pios_adxl345.c
View File

@ -29,7 +29,7 @@
*/
#include "pios.h"
#include <pios_adxl345.h>
#ifdef PIOS_INCLUDE_ADXL345
enum pios_adxl345_dev_magic {

2
flight/pios/common/pios_bma180.c
View File

@ -30,7 +30,7 @@
*/
#include "pios.h"
#include <pios_bma180.h>
#ifdef PIOS_INCLUDE_BMA180
#include "fifo_buffer.h"

2
flight/pios/common/pios_bmp085.c
View File

@ -29,7 +29,7 @@
*/
#include "pios.h"
#include <pios_bmp085.h>
#ifdef PIOS_INCLUDE_BMP085
#ifndef PIOS_INCLUDE_EXTI

52
flight/pios/common/pios_hmc5x83.c
View File

@ -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 */

4
flight/pios/common/pios_instrumentation.c
View File

@ -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;

2
flight/pios/common/pios_l3gd20.c
View File

@ -30,7 +30,7 @@
*/
#include "pios.h"
#include <pios_l3gd20.h>
#ifdef PIOS_INCLUDE_L3GD20
#include "fifo_buffer.h"

317
flight/pios/common/pios_mpu6000.c
View File

@ -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 */

179
flight/pios/common/pios_ms5611.c
View File

@ -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
View 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;
}

5
flight/pios/inc/pios_hmc5x83.h
View File

@ -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 */
/**

6
flight/pios/inc/pios_instrumentation.h
View File

@ -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;
}

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

@ -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))

1
flight/pios/inc/pios_mem.h
View File

@ -26,6 +26,7 @@
*/
#ifndef PIOS_MEM_H
#define PIOS_MEM_H
#include <strings.h>
void *pios_fastheapmalloc(size_t size);

20
flight/pios/inc/pios_mpu6000.h
View File

@ -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 */
/**

21
flight/pios/inc/pios_ms5611.h
View File

@ -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
View 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 */

43
flight/pios/pios.h
View File

@ -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>

6
flight/targets/boards/coptercontrol/firmware/pios_board.c
View File

@ -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;

3
flight/targets/boards/gpsplatinum/board_hw_defs.c
View File

@ -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()
{

2
flight/targets/boards/osd/firmware/Makefile
View File

@ -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

5
flight/targets/boards/revolution/firmware/Makefile
View File

@ -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

17
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -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);

5
flight/targets/boards/revoproto/firmware/Makefile
View File

@ -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

8
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -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:

1
make/apps-defs.mk
View File

@ -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

7
shared/uavobjectdefinition/magsensor.xml
View File

@ -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"/>