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

Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1721

Browse Source 
Conflicts:
	flight/modules/Sensors/sensors.c
This commit is contained in:
abeck70 2015-02-23 14:40:55 +11:00
commit 57e0a8baf5
37 changed files with 1376 additions and 1065 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 * @addtogroup Reuseable vector data type and functions
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
* @{ * @{
* *
* @file altitude.h * @file vectors.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Altitude module, reads temperature and pressure from BMP085 * @brief Reuseable vector data type and functions
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
@ -28,14 +27,37 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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));
}
/**
* @}
* @}
*/

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

@ -62,6 +62,10 @@
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "taskinfo.h" #include "taskinfo.h"
#include <pios_sensors.h>
#include <pios_adxl345.h>
#include <pios_mpu6000.h>
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_notify.h> #include <pios_notify.h>
#include <mathmisc.h> #include <mathmisc.h>
@ -160,6 +164,8 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f #define STD_CC_ANALOG_GYRO_GAIN 0.42f
static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
// Used to detect CC vs CC3D // Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob; static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02) #define BOARDISCC3D (bdinfo->board_rev == 0x02)
@ -238,7 +244,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
if (cc3d) { if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000) #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 #endif
} else { } else {
#if defined(PIOS_INCLUDE_ADXL345) #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 * @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not * @return 0 if successfull, -1 if not
*/ */
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData) static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{ {
float accels[3] = { 0 }; float accels[3] = { 0 };
@ -465,18 +472,18 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000) #if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue(); xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms); BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
while (ret == pdTRUE) { while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x; gyros[0] += mpu6000_data->sample[1].x;
gyros[1] += mpu6000_data.gyro_y; gyros[1] += mpu6000_data->sample[1].y;
gyros[2] += mpu6000_data.gyro_z; gyros[2] += mpu6000_data->sample[1].z;
accels[0] += mpu6000_data.accel_x; accels[0] += mpu6000_data->sample[0].x;
accels[1] += mpu6000_data.accel_y; accels[1] += mpu6000_data->sample[0].y;
accels[2] += mpu6000_data.accel_z; accels[2] += mpu6000_data->sample[0].z;
temp += mpu6000_data.temperature; temp += mpu6000_data->temperature;
count++; count++;
// check if further samples are already in queue // 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; temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) { if (BOARDISCC3D) {
float scales[2];
PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
accel_bias.X = accelGyroSettings.accel_bias.X; accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y; accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z; accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale(); gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[0];
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale(); gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[0];
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale(); gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[0];
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale(); accel_scale.X = accelGyroSettings.accel_scale.X * scales[1];
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale(); accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[1];
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale(); accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[1];
} else { } else {
// Original CC with analog gyros and ADXL accel // Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X; 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 OpenPilotModules OpenPilot Modules
* @{ * @{
* @addtogroup Sensors Sensors Module * @addtogroup Sensors
* @brief Acquires sensor data
* @{ * @{
* *
* @file attitude.h * @file sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Acquires sensor data and fuses it into attitude estimate for CC * @brief Module to handle fetch and preprocessing of sensor data
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
*****************************************************************************/ ******************************************************************************/
/* /*
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by

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

@ -4,12 +4,11 @@
* @{ * @{
* @addtogroup Sensors * @addtogroup Sensors
* @brief Acquires sensor data * @brief Acquires sensor data
* Specifically updates the the @ref GyroSensor, @ref AccelSensor, and @ref MagSensor objects
* @{ * @{
* *
* @file sensors.c * @file sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Module to handle all comms to the AHRS on a periodic basis. * @brief Module to handle fetch and preprocessing of sensor data
* *
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
@ -47,69 +46,118 @@
*/ */
#include <openpilot.h> #include <openpilot.h>
#include <pios_sensors.h>
#include <homelocation.h> #include <homelocation.h>
#include <magsensor.h> #include <magsensor.h>
#include <accelsensor.h> #include <accelsensor.h>
#include <gyrosensor.h> #include <gyrosensor.h>
#include <attitudestate.h> #include <barosensor.h>
#include <flightstatus.h>
#include <attitudesettings.h> #include <attitudesettings.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <accelgyrosettings.h> #include <accelgyrosettings.h>
#include <flightstatus.h> #include <revosettings.h>
#include <mathmisc.h>
#include <taskinfo.h> #include <taskinfo.h>
#include <pios_math.h> #include <pios_math.h>
#include <pios_constants.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_board_info.h> #include <pios_board_info.h>
#include <string.h>
// Private constants // Private constants
#define STACK_SIZE_BYTES 1000 #define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); #define MAX_SENSORS_PER_INSTANCE 2
#ifdef PIOS_INCLUDE_WDG
#define RELOAD_WDG() PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS)
#define REGISTER_WDG() PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS)
#else
#define RELOAD_WDG()
#define REGISTER_WDG()
#endif
static const TickType_t sensor_period_ticks = ((uint32_t)1000.0f / PIOS_SENSOR_RATE) / portTICK_RATE_MS;
// Interval in number of sample to recalculate temp bias // Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 30 #define TEMP_CALIB_INTERVAL 30
// LPF // LPF
#define TEMP_DT (1.0f / PIOS_SENSOR_RATE) #define TEMP_DT_GYRO_ACCEL (1.0f / PIOS_SENSOR_RATE)
#define TEMP_LPF_FC 5.0f #define TEMP_LPF_FC_GYRO_ACCEL 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); static const float temp_alpha_gyro_accel = LPF_ALPHA(TEMP_DT_GYRO_ACCEL, TEMP_LPF_FC_GYRO_ACCEL);
// Interval in number of sample to recalculate temp bias
#define BARO_TEMP_CALIB_INTERVAL 10
// LPF
#define TEMP_DT_BARO (1.0f / 120.0f)
#define TEMP_LPF_FC_BARO 5.0f
static const float temp_alpha_baro = TEMP_DT_BARO / (TEMP_DT_BARO + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC_BARO));
#define ZERO_ROT_ANGLE 0.00001f #define ZERO_ROT_ANGLE 0.00001f
// Private types // Private types
typedef struct {
// used to accumulate all samples in a task iteration
Vector3i32 accum[2];
int32_t temperature;
uint32_t count;
} sensor_fetch_context;
#define MAX_SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + MAX_SENSORS_PER_INSTANCE * sizeof(Vector3i16))
typedef union {
PIOS_SENSORS_3Axis_SensorsWithTemp sensorSample3Axis;
PIOS_SENSORS_1Axis_SensorsWithTemp sensorSample1Axis;
} sensor_data;
#define PIOS_INSTRUMENT_MODULE #define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h> #include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples); PERF_DEFINE_COUNTER(counterAccelSamples);
PERF_DEFINE_COUNTER(counterAccelPeriod);
PERF_DEFINE_COUNTER(counterMagPeriod);
PERF_DEFINE_COUNTER(counterBaroPeriod);
PERF_DEFINE_COUNTER(counterSensorPeriod); PERF_DEFINE_COUNTER(counterSensorPeriod);
PERF_DEFINE_COUNTER(counterSensorResets);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
// Private functions // Private functions
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
static void settingsUpdatedCb(UAVObjEvent *objEv); static void settingsUpdatedCb(UAVObjEvent *objEv);
static void accumulateSamples(sensor_fetch_context *sensor_context, sensor_data *sample);
static void 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 // Private variables
static sensor_data *source_data;
static xTaskHandle sensorsTaskHandle; static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal; RevoCalibrationData cal;
AccelGyroSettingsData agcal; AccelGyroSettingsData agcal;
#ifdef PIOS_INCLUDE_HMC5X83
#include <pios_hmc5x83.h>
extern pios_hmc5x83_dev_t onboard_mag;
#endif
// These values are initialized by settings but can be updated by the attitude algorithm // These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 }; static float mag_bias[3] = { 0, 0, 0 };
static float mag_transform[3][3] = { static float mag_transform[3][3] = {
{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
}; };
// temp coefficient to calculate gyro bias
// Variables used to handle accel/gyro temperature bias
static volatile bool gyro_temp_calibrated = false; static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false; static volatile bool accel_temp_calibrated = false;
@ -117,42 +165,45 @@ static float accel_temperature = NAN;
static float gyro_temperature = NAN; static float gyro_temperature = NAN;
static float accel_temp_bias[3] = { 0 }; static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 }; static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0; static uint8_t accel_temp_calibration_count = 0;
static uint8_t gyro_temp_calibration_count = 0;
static float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
}; };
// Variables used to handle baro temperature bias
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool baro_temp_correction_enabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t baro_temp_calibration_count = 0;
static int8_t rotate = 0; static int8_t rotate = 0;
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
* Stores all the queues the algorithm will pull data from
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
* Update() -- queries queues and updates the attitude estiamte
*/
/** /**
* Initialise the module. Called before the start function * Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed * \returns 0 on success or -1 if initialisation failed
*/ */
int32_t SensorsInitialize(void) int32_t SensorsInitialize(void)
{ {
source_data = (sensor_data *)pios_malloc(MAX_SENSOR_DATA_SIZE);
GyroSensorInitialize(); GyroSensorInitialize();
AccelSensorInitialize(); AccelSensorInitialize();
MagSensorInitialize(); MagSensorInitialize();
BaroSensorInitialize();
RevoCalibrationInitialize(); RevoCalibrationInitialize();
RevoSettingsInitialize();
AttitudeSettingsInitialize(); AttitudeSettingsInitialize();
AccelGyroSettingsInitialize(); AccelGyroSettingsInitialize();
rotate = 0; rotate = 0;
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb); AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb); AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0; return 0;
} }
@ -165,10 +216,7 @@ int32_t SensorsStart(void)
// Start main task // Start main task
xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle); xTaskCreate(SensorsTask, "Sensors", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &sensorsTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SENSORS, sensorsTaskHandle);
#ifdef PIOS_INCLUDE_WDG REGISTER_WDG();
PIOS_WDG_RegisterFlag(PIOS_WDG_SENSORS);
#endif
return 0; return 0;
} }
@ -184,83 +232,55 @@ int32_t mag_test;
* The sensor task. This polls the gyros at 500 Hz and pumps that data to * The sensor task. This polls the gyros at 500 Hz and pumps that data to
* stabilization and to the attitude loop * stabilization and to the attitude loop
* *
* This function has a lot of if/defs right now to allow these configurations:
* 1. BMA180 accel and MPU6000 gyro
* 2. MPU6000 gyro and accel
* 3. BMA180 accel and L3GD20 gyro
*/ */
uint32_t sensor_dt_us; uint32_t sensor_dt_us;
static void SensorsTask(__attribute__((unused)) void *parameters) static void SensorsTask(__attribute__((unused)) void *parameters)
{ {
portTickType lastSysTime; portTickType lastSysTime;
uint32_t accel_samples = 0; sensor_fetch_context sensor_context;
uint32_t gyro_samples = 0; bool error = false;
int32_t accel_accum[3] = { 0, 0, 0 }; const PIOS_SENSORS_Instance *sensors_list = PIOS_SENSORS_GetList();
int32_t gyro_accum[3] = { 0, 0, 0 }; PIOS_SENSORS_Instance *sensor;
float gyro_scaling = 0;
float accel_scaling = 0;
static int32_t timeval;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
settingsUpdatedCb(NULL);
UAVObjEvent ev; // Performance counters
settingsUpdatedCb(&ev); 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; // Test sensors
bool sensors_test = true;
switch (bdinfo->board_rev) { uint8_t count = 0;
case 0x01: LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
#if defined(PIOS_INCLUDE_L3GD20) sensors_test &= PIOS_SENSORS_Test(sensor);
gyro_test = PIOS_L3GD20_Test(); count++;
#endif
#if defined(PIOS_INCLUDE_BMA180)
accel_test = PIOS_BMA180_Test();
#endif
break;
case 0x02:
case 0x03:
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
accel_test = gyro_test;
#endif
break;
default:
PIOS_DEBUG_Assert(0);
} }
#if defined(PIOS_INCLUDE_HMC5X83) PIOS_Assert(count);
mag_test = PIOS_HMC5x83_Test(onboard_mag); RELOAD_WDG();
#else if (!sensors_test) {
mag_test = 0;
#endif
if (accel_test < 0 || gyro_test < 0 || mag_test < 0) {
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
while (1) { while (1) {
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelay(10); vTaskDelay(10);
} }
} }
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
bool error = false; uint32_t reset_counter = 0;
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
while (1) { while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail // TODO: add timeouts to the sensor reads and set an error if the fail
sensor_dt_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (error) { if (error) {
#ifdef PIOS_INCLUDE_WDG RELOAD_WDG();
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false; error = false;
} else { } else {
@ -268,221 +288,250 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
} }
for (int i = 0; i < 3; i++) { // reset the fetch context
accel_accum[i] = 0; clearContext(&sensor_context);
gyro_accum[i] = 0; LL_FOREACH((PIOS_SENSORS_Instance *)sensors_list, sensor) {
} // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
accel_samples = 0; bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
gyro_samples = 0;
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 (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;
}
} 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);
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; 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; GyroSensorData gyroSensorData;
uint8_t board_rev = bdinfo->board_rev; 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] };
switch (board_rev) { rot_mult(R, gyros_out, samples);
case 0x01: // L3GD20 + BMA180 board gyroSensorData.temperature = temperature;
#if defined(PIOS_INCLUDE_BMA180) gyroSensorData.x = samples[0];
{ gyroSensorData.y = samples[1];
struct pios_bma180_data accel; gyroSensorData.z = samples[2];
int32_t read_good; GyroSensorSet(&gyroSensorData);
int32_t count; }
count = 0; static void handleMag(float *samples, float temperature)
while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) { {
error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error; MagSensorData mag;
} float mags[3] = { (float)samples[1] - mag_bias[0],
if (error) { (float)samples[0] - mag_bias[1],
// Unfortunately if the BMA180 ever misses getting read, then it will not (float)samples[2] - mag_bias[2] };
// trigger more interrupts. In this case we must force a read to kickstarts
// it. rot_mult(mag_transform, mags, samples);
struct pios_bma180_data data;
PIOS_BMA180_ReadAccels(&data); mag.x = samples[0];
continue; mag.y = samples[1];
} mag.z = samples[2];
while (read_good == 0) { mag.temperature = temperature;
count++;
MagSensorSet(&mag);
accel_accum[1] += accel.x; }
accel_accum[0] += accel.y;
accel_accum[2] -= accel.z; static void handleBaro(float sample, float temperature)
{
read_good = PIOS_BMA180_ReadFifo(&accel); updateBaroTempBias(temperature);
} sample -= baro_temp_bias;
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale(); float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
// Get temp from last reading if (!isnan(altitude)) {
accelSensorData.temperature = 25.0f + ((float)accel.temperature - 2.0f) / 2.0f; BaroSensorData data;
} data.Altitude = altitude;
#endif /* if defined(PIOS_INCLUDE_BMA180) */ data.Temperature = temperature;
#if defined(PIOS_INCLUDE_L3GD20) data.Pressure = sample;
{ // Update the BasoSensor UAVObject
struct pios_l3gd20_data gyro; BaroSensorSet(&data);
gyro_samples = 0;
xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue();
if (xQueueReceive(gyro_queue, (void *)&gyro, 4) == errQUEUE_EMPTY) {
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++;
}
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);
} }
}
static void updateAccelTempBias(float temperature)
{
if (isnan(accel_temperature)) { if (isnan(accel_temperature)) {
accel_temperature = accelSensorData.temperature; accel_temperature = temperature;
gyro_temperature = gyroSensorData.temperature;
} }
accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature; if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
gyro_temperature = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature; accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) { if (accel_temp_calibrated) {
float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); 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[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp; accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * 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) { if (gyro_temp_calibrated) {
float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min); 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[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[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_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
} }
} }
temp_calibration_count--; gyro_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);
}
} }
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 * Locally cache some variables from the AtttitudeSettings object
*/ */
@ -536,6 +585,11 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
Quaternion2R(rotationQuat, R); Quaternion2R(rotationQuat, R);
} }
matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform); matrix_mult_3x3f((float(*)[3])RevoCalibrationmag_transformToArray(cal.mag_transform), R, mag_transform);
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
baro_temp_correction_enabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
} }
/** /**
* @} * @}

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

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

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

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

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

@ -29,7 +29,7 @@
*/ */
#include "pios.h" #include "pios.h"
#include <pios_bmp085.h>
#ifdef PIOS_INCLUDE_BMP085 #ifdef PIOS_INCLUDE_BMP085
#ifndef PIOS_INCLUDE_EXTI #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); 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 * Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created 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; 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 * @brief Initialize the HMC5x83 magnetometer sensor
* \return none * \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 */ #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 */ #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) { if (!counter_handle) {
pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter]; pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
newcounter->id = id; newcounter->id = id;
newcounter->max = INT32_MIN; newcounter->max = INT32_MIN + 1;
newcounter->min = INT32_MAX; newcounter->min = INT32_MAX - 1;
counter_handle = (pios_counter_t)newcounter; counter_handle = (pios_counter_t)newcounter;
} }
return counter_handle; return counter_handle;

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

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

311
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 * @addtogroup PIOS_MPU6000 MPU6000 Functions
* @brief Deals with the hardware interface to the 3-axis gyro * @brief Deals with the hardware interface to the 3-axis gyro
@ -13,8 +13,8 @@
* *
****************************************************************************** ******************************************************************************
*/ */
/* /*istribu
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or * the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
@ -30,20 +30,40 @@
*/ */
#include "pios.h" #include "pios.h"
#include <pios_mpu6000.h>
#ifdef PIOS_INCLUDE_MPU6000 #ifdef PIOS_INCLUDE_MPU6000
#include <stdint.h>
#include <pios_constants.h> #include <pios_constants.h>
#include <pios_sensors.h>
/* Global Variables */ /* Global Variables */
enum pios_mpu6000_dev_magic { enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, 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 { struct mpu6000_dev {
uint32_t spi_id; uint32_t spi_id;
uint32_t slave_num; uint32_t slave_num;
xQueueHandle queue; QueueHandle_t queue;
const struct pios_mpu6000_cfg *cfg; const struct pios_mpu6000_cfg *cfg;
enum pios_mpu6000_range gyro_range; enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_accel_range accel_range; enum pios_mpu6000_accel_range accel_range;
@ -51,26 +71,19 @@ struct mpu6000_dev {
enum pios_mpu6000_dev_magic magic; enum pios_mpu6000_dev_magic magic;
}; };
#ifdef PIOS_MPU6000_ACCEL
#define PIOS_MPU6000_SAMPLES_BYTES 14 #define PIOS_MPU6000_SAMPLES_BYTES 14
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB #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 { typedef union {
uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES]; uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
struct { struct {
uint8_t dummy; uint8_t dummy;
#ifdef PIOS_MPU6000_ACCEL
uint8_t Accel_X_h; uint8_t Accel_X_h;
uint8_t Accel_X_l; uint8_t Accel_X_l;
uint8_t Accel_Y_h; uint8_t Accel_Y_h;
uint8_t Accel_Y_l; uint8_t Accel_Y_l;
uint8_t Accel_Z_h; uint8_t Accel_Z_h;
uint8_t Accel_Z_l; uint8_t Accel_Z_l;
#endif
uint8_t Temperature_h; uint8_t Temperature_h;
uint8_t Temperature_l; uint8_t Temperature_l;
uint8_t Gyro_X_h; uint8_t Gyro_X_h;
@ -88,7 +101,9 @@ typedef union {
static struct mpu6000_dev *dev; static struct mpu6000_dev *dev;
volatile bool mpu6000_configured = false; volatile bool mpu6000_configured = false;
static mpu6000_data_t mpu6000_data; 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 // ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg); static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev); 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 int32_t PIOS_MPU6000_GetReg(uint8_t address);
static void PIOS_MPU6000_SetSpeed(const bool fast); static void PIOS_MPU6000_SetSpeed(const bool fast);
static bool PIOS_MPU6000_HandleData(); static bool PIOS_MPU6000_HandleData();
static bool PIOS_MPU6000_ReadFifo(bool *woken);
static bool PIOS_MPU6000_ReadSensor(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 * @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; struct mpu6000_dev *mpu6000_dev;
mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev)); mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev));
if (!mpu6000_dev) { PIOS_Assert(mpu6000_dev);
return NULL;
}
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data)); mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
if (mpu6000_dev->queue == NULL) { PIOS_Assert(mpu6000_dev->queue);
vPortFree(mpu6000_dev);
return NULL;
}
queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
PIOS_Assert(queue_data);
queue_data->count = SENSOR_COUNT;
return mpu6000_dev; return mpu6000_dev;
} }
@ -211,15 +230,9 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
} }
// FIFO storage // 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) { 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); PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
// Interrupt configuration // Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { 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; dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range // Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) { while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
; ;
} }
dev->accel_range = accelRange; dev->accel_range = accelRange;
#endif
return 0; 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) * @brief Perform a dummy read in order to restart interrupt generation
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \returns 0 if succesful * \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 // 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 }; 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(); 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; return 0;
} }
@ -469,7 +475,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
} }
float PIOS_MPU6000_GetScale() static float PIOS_MPU6000_GetScale()
{ {
switch (dev->gyro_range) { switch (dev->gyro_range) {
case PIOS_MPU6000_SCALE_250_DEG: case PIOS_MPU6000_SCALE_250_DEG:
@ -487,7 +493,7 @@ float PIOS_MPU6000_GetScale()
return 0; return 0;
} }
float PIOS_MPU6000_GetAccelScale() static float PIOS_MPU6000_GetAccelScale()
{ {
switch (dev->accel_range) { switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G: case PIOS_MPU6000_ACCEL_2G:
@ -510,7 +516,7 @@ float PIOS_MPU6000_GetAccelScale()
* \return 0 if test succeeded * \return 0 if test succeeded
* \return non-zero value 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) */ /* Verify that ID matches (MPU6000 ID is 0x69) */
int32_t mpu6000_id = PIOS_MPU6000_ReadID(); int32_t mpu6000_id = PIOS_MPU6000_ReadID();
@ -526,172 +532,76 @@ int32_t PIOS_MPU6000_Test(void)
return 0; 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 * @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 * 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 PIOS_MPU6000_IRQHandler(void)
{ {
bool woken = false; bool woken = false;
static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (!mpu6000_configured) { if (!mpu6000_configured) {
return false; return false;
} }
bool read_ok = 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) { if (read_ok) {
bool woken2 = PIOS_MPU6000_HandleData(); bool woken2 = PIOS_MPU6000_HandleData();
woken |= woken2; woken |= woken2;
} }
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return woken; return woken;
} }
static bool PIOS_MPU6000_HandleData() static bool PIOS_MPU6000_HandleData()
{ {
if (!queue_data) {
return false;
}
// Rotate the sensor to OP convention. The datasheet defines X as towards the right // 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 // and Y as forward. OP convention transposes this. Also the Z is defined negatively
// to our convention // to our convention
static struct pios_mpu6000_data data;
// Currently we only support rotations on top so switch X/Y accordingly // Currently we only support rotations on top so switch X/Y accordingly
switch (dev->cfg->orientation) { switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG: case PIOS_MPU6000_TOP_0DEG:
#ifdef PIOS_MPU6000_ACCEL queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
#endif queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
break; break;
case PIOS_MPU6000_TOP_90DEG: case PIOS_MPU6000_TOP_90DEG:
// -1 to bring it back to -32768 +32767 range // -1 to bring it back to -32768 +32767 range
#ifdef PIOS_MPU6000_ACCEL queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
data.accel_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
data.accel_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
#endif queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
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
break; break;
case PIOS_MPU6000_TOP_180DEG: case PIOS_MPU6000_TOP_180DEG:
#ifdef PIOS_MPU6000_ACCEL queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
data.accel_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
data.accel_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
#endif queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
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
break; break;
case PIOS_MPU6000_TOP_270DEG: case PIOS_MPU6000_TOP_270DEG:
#ifdef PIOS_MPU6000_ACCEL queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
data.accel_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
data.accel_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
#endif queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
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
break; break;
} }
#ifdef PIOS_MPU6000_ACCEL queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
data.accel_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z)); queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
#endif const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
data.gyro_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z)); queue_data->temperature = 3500 + ((float)(temp + 512)) * (1.0f / 3.4f);
data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
BaseType_t higherPriorityTaskWoken; BaseType_t higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken); xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
return higherPriorityTaskWoken == pdTRUE; 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) { 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); PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false; return false;
} }
PIOS_MPU6000_ReleaseBusISR(woken); PIOS_MPU6000_ReleaseBusISR(woken);
return true; 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 return !PIOS_MPU6000_Test();
* 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;
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) { void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context)
return false; {
} PIOS_MPU6000_DummyReadGyros();
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;
}
/* Usual case - FIFO has not overflowed. */ void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken); {
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) { PIOS_Assert(size >= 2);
return false; scales[0] = PIOS_MPU6000_GetAccelScale();
} scales[1] = PIOS_MPU6000_GetScale();
}
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) { QueueHandle_t PIOS_MPU6000_driver_get_queue(__attribute__((unused)) uintptr_t context)
return false; {
} return dev->queue;
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;
} }
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */

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

@ -29,17 +29,40 @@
*/ */
#include "pios.h" #include "pios.h"
#ifdef PIOS_INCLUDE_MS5611 #ifdef PIOS_INCLUDE_MS5611
#include <pios_ms5611.h>
#define POW2(x) (1 << x) #define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant. // 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 */ /* Glocal Variables */
ConversionTypeTypeDef CurrentRead; ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
/* Local Variables */ /* Local Variables */
MS5611CalibDataTypeDef CalibData; MS5611CalibDataTypeDef CalibData;
@ -50,8 +73,14 @@ static uint32_t RawPressure;
static int64_t Pressure; static int64_t Pressure;
static int64_t Temperature; static int64_t Temperature;
static int32_t lastConversionStart; 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_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command); 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 // Second order temperature compensation. Temperature offset
static int64_t compensation_t2; static int64_t compensation_t2;
@ -60,8 +89,24 @@ static int64_t compensation_t2;
static uint32_t oversampling; static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg; static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id; 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 * Initialise the MS5611 sensor
*/ */
@ -71,6 +116,9 @@ 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; oversampling = cfg->oversampling;
conversionDelayMs = PIOS_MS5611_GetDelay();
conversionDelayUs = PIOS_MS5611_GetDelayUs();
dev_cfg = cfg; // Store cfg before enabling interrupt dev_cfg = cfg; // Store cfg before enabling interrupt
PIOS_MS5611_WriteCommand(MS5611_RESET); 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 // reset temperature compensation values
compensation_t2 = 0; compensation_t2 = 0;
/* Calibration parameters */ /* Calibration parameters */
for (int i = 0; i < 6; i++) { 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]; 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) int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
{ {
/* Start the conversion */ /* Start the conversion */
if (Type == TemperatureConv) { if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) { while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
continue; continue;
} }
} else if (Type == PressureConv) { } else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) { while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
continue; continue;
} }
@ -114,7 +162,7 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
/** /**
* @brief Return the delay for the current osr * @brief Return the delay for the current osr
*/ */
int32_t PIOS_MS5611_GetDelay() static uint32_t PIOS_MS5611_GetDelay()
{ {
switch (oversampling) { switch (oversampling) {
case MS5611_OSR_256: case MS5611_OSR_256:
@ -141,7 +189,7 @@ int32_t PIOS_MS5611_GetDelay()
/** /**
* @brief Return the delay for the current osr in uS * @brief Return the delay for the current osr in uS
*/ */
uint32_t PIOS_MS5611_GetDelayUs() static uint32_t PIOS_MS5611_GetDelayUs()
{ {
switch (oversampling) { switch (oversampling) {
case MS5611_OSR_256: case MS5611_OSR_256:
@ -167,7 +215,7 @@ uint32_t PIOS_MS5611_GetDelayUs()
/** /**
* Read the ADC conversion value (once ADC conversion has completed) * 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) int32_t PIOS_MS5611_ReadADC(void)
{ {
@ -177,16 +225,19 @@ int32_t PIOS_MS5611_ReadADC(void)
Data[1] = 0; Data[1] = 0;
Data[2] = 0; Data[2] = 0;
while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) { if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
vTaskDelay(0); return -2;
}
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
return -1;
} }
static int64_t deltaTemp; static int64_t deltaTemp;
/* Read and store the 16bit result */ /* Read and store the 16bit result */
if (CurrentRead == TemperatureConv) { if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
/* Read the temperature conversion */ /* Read the temperature conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1; return -2;
} }
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2]; RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
@ -205,7 +256,7 @@ int32_t PIOS_MS5611_ReadADC(void)
/* Read the pressure conversion */ /* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1; return -2;
} }
// check if temperature is less than 20°C // check if temperature is less than 20°C
if (Temperature < 2000) { if (Temperature < 2000) {
@ -248,18 +299,18 @@ int32_t PIOS_MS5611_ReadADC(void)
/** /**
* Return the most recently computed temperature in kPa * 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 // Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f; 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 0 if operation was successful
* \return -1 if error during I2C transfer * \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[] = { 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 0 if operation was successful
* \return -1 if error during I2C transfer * \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[] = { const struct pios_i2c_txn txn_list[] = {
{ {
@ -326,16 +377,16 @@ int32_t PIOS_MS5611_Test()
int32_t cur_value = 0; int32_t cur_value = 0;
cur_value = Temperature; cur_value = Temperature;
PIOS_MS5611_StartADC(TemperatureConv); PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
PIOS_DELAY_WaitmS(5); PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC(); PIOS_MS5611_ReadADC();
if (cur_value == Temperature) { if (cur_value == Temperature) {
return -1; return -1;
} }
cur_value = Pressure; cur_value = Pressure;
PIOS_MS5611_StartADC(PressureConv); PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
PIOS_DELAY_WaitmS(26); PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC(); PIOS_MS5611_ReadADC();
if (cur_value == Pressure) { if (cur_value == Pressure) {
return -1; return -1;
@ -344,6 +395,80 @@ int32_t PIOS_MS5611_Test()
return 0; 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 */ #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 #ifndef PIOS_HMC5x83_H
#define PIOS_HMC5x83_H #define PIOS_HMC5x83_H
#include <stdint.h> #include <stdint.h>
#include <pios_sensors.h>
/* HMC5x83 Addresses */ /* HMC5x83 Addresses */
#define PIOS_HMC5x83_I2C_ADDR 0x1E #define PIOS_HMC5x83_I2C_ADDR 0x1E
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D #define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
@ -123,12 +124,16 @@ struct pios_hmc5x83_cfg {
/* Public Functions */ /* 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 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 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 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 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 int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_IRQHandler(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 */ #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(); vPortEnterCritical();
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle; pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = newValue; counter->value = newValue;
counter->max--;
if (counter->value > counter->max) { if (counter->value > counter->max) {
counter->max = counter->value; counter->max = counter->value;
} }
counter->min++;
if (counter->value < counter->min) { if (counter->value < counter->min) {
counter->min = counter->value; 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; pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS); counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->max--;
if (counter->value > counter->max) { if (counter->value > counter->max) {
counter->max = counter->value; counter->max = counter->value;
} }
counter->min++;
if (counter->value < counter->min) { if (counter->value < counter->min) {
counter->min = counter->value; counter->min = counter->value;
} }
@ -110,9 +114,11 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
vPortEnterCritical(); vPortEnterCritical();
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS); uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->value = (counter->value * 15 + period) / 16; counter->value = (counter->value * 15 + period) / 16;
counter->max--;
if ((int32_t)period > counter->max) { if ((int32_t)period > counter->max) {
counter->max = period; counter->max = period;
} }
counter->min++;
if ((int32_t)period < counter->min) { if ((int32_t)period < counter->min) {
counter->min = period; 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 RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d)) #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 // Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b)) #define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(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 #ifndef PIOS_MEM_H
#define PIOS_MEM_H #define PIOS_MEM_H
#include <strings.h>
void *pios_fastheapmalloc(size_t size); void *pios_fastheapmalloc(size_t size);

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

@ -31,6 +31,7 @@
#ifndef PIOS_MPU6000_H #ifndef PIOS_MPU6000_H
#define PIOS_MPU6000_H #define PIOS_MPU6000_H
#include <pios_sensors.h>
/* MPU6000 Addresses */ /* MPU6000 Addresses */
#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19 #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 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 { struct pios_mpu6000_cfg {
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
@ -167,14 +156,11 @@ struct pios_mpu6000_cfg {
/* Public Functions */ /* 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_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 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_ReadID();
extern int32_t PIOS_MPU6000_Test(); extern void PIOS_MPU6000_Register();
extern float PIOS_MPU6000_GetScale();
extern float PIOS_MPU6000_GetAccelScale();
extern bool PIOS_MPU6000_IRQHandler(void); extern bool PIOS_MPU6000_IRQHandler(void);
extern const PIOS_SENSORS_Driver PIOS_MPU6000_Driver;
#endif /* PIOS_MPU6000_H */ #endif /* PIOS_MPU6000_H */
/** /**

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

@ -30,7 +30,7 @@
#ifndef PIOS_MS5611_H #ifndef PIOS_MS5611_H
#define PIOS_MS5611_H #define PIOS_MS5611_H
#include <pios_sensors.h>
/* BMP085 Addresses */ /* BMP085 Addresses */
#define MS5611_I2C_ADDR 0x77 #define MS5611_I2C_ADDR 0x77
#define MS5611_RESET 0x1E #define MS5611_RESET 0x1E
@ -40,17 +40,6 @@
#define MS5611_PRES_ADDR 0x40 #define MS5611_PRES_ADDR 0x40
#define MS5611_TEMP_ADDR 0x50 #define MS5611_TEMP_ADDR 0x50
#define MS5611_ADC_MSB 0xF6 #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 { struct pios_ms5611_cfg {
uint32_t oversampling; uint32_t oversampling;
@ -66,12 +55,8 @@ enum pios_ms5611_osr {
/* Public Functions */ /* Public Functions */
extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device); extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device);
extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type); extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver;
extern int32_t PIOS_MS5611_ReadADC(void); void PIOS_MS5611_Register();
extern float PIOS_MS5611_GetTemperature(void);
extern float PIOS_MS5611_GetPressure(void);
extern int32_t PIOS_MS5611_Test();
extern int32_t PIOS_MS5611_GetDelay();
#endif /* PIOS_MS5611_H */ #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

@ -186,49 +186,6 @@ extern "C" {
#endif #endif
#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 #ifdef PIOS_INCLUDE_MPXV
/* MPXV5004, MPXV7002 based Airspeed Sensor */ /* MPXV5004, MPXV7002 based Airspeed Sensor */
#include <pios_mpxv.h> #include <pios_mpxv.h>

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

@ -38,6 +38,10 @@
#ifdef PIOS_INCLUDE_INSTRUMENTATION #ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h> #include <pios_instrumentation.h>
#endif #endif
#if defined(PIOS_INCLUDE_ADXL345)
#include <pios_adxl345.h>
#endif
/* /*
* Pull in the board-specific static HW definitions. * Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of * 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_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure(); PIOS_MPU6000_CONFIG_Configure();
init_test = PIOS_MPU6000_Test(); init_test = !PIOS_MPU6000_Driver.test(0);
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */
break; 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 */ #endif /* PIOS_INCLUDE_FLASH */
#if defined(PIOS_INCLUDE_HMC5X83) #if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t onboard_mag;
#include "pios_hmc5x83.h" #include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag;
#ifdef PIOS_HMC5X83_HAS_GPIOS #ifdef PIOS_HMC5X83_HAS_GPIOS
bool pios_board_mag_handler() 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/osdinput
MODULES += Osd/WavPlayer MODULES += Osd/WavPlayer
MODULES += GPS MODULES += GPS
MODULES += Extensions/MagBaro #MODULES += Extensions/MagBaro
MODULES += FirmwareIAP MODULES += FirmwareIAP
MODULES += Telemetry MODULES += Telemetry

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

@ -32,11 +32,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include # List of mandatory modules to include
MODULES += Sensors MODULES += Sensors
#MODULES += Attitude/revolution MODULES += StateEstimation
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization MODULES += Stabilization
MODULES += ManualControl MODULES += ManualControl
MODULES += Receiver 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, .orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4, .fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64, .std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 16, .max_downsample = 20,
}; };
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */
@ -922,20 +922,23 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg); PIOS_ADC_Init(&pios_adc_cfg);
#endif #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) #if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif #endif
#if defined(PIOS_INCLUDE_MS5611) #if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
PIOS_MS5611_Register();
#endif #endif
#if defined(PIOS_INCLUDE_MPU6000) #ifdef PIOS_INCLUDE_WS2811
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h> #include <pios_ws2811.h>
HwSettingsWS2811LED_OutOptions ws2811_pin_settings; HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
HwSettingsWS2811LED_OutGet(&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 # List of mandatory modules to include
MODULES += Sensors MODULES += Sensors
#MODULES += Attitude/revolution MODULES += StateEstimation
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += Airspeed MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization MODULES += Stabilization
MODULES += ManualControl MODULES += ManualControl
MODULES += Receiver 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) */ #endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5X83) #if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0; pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler() bool pios_board_internal_mag_handler()
{ {
return PIOS_HMC5x83_IRQHandler(onboard_mag); return PIOS_HMC5x83_IRQHandler(onboard_mag);
} }
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler, .vector = pios_board_internal_mag_handler,
.line = EXTI_Line5, .line = EXTI_Line5,
@ -893,19 +894,23 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_HMC5X83) #if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0); onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif #endif
#if defined(PIOS_INCLUDE_MS5611) #if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
PIOS_MS5611_Register();
#endif #endif
switch (bdinfo->board_rev) { switch (bdinfo->board_rev) {
case 0x01: case 0x01:
#if defined(PIOS_INCLUDE_L3GD20) #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_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0); PIOS_Assert(PIOS_L3GD20_Test() == 0);
#endif #endif
#if defined(PIOS_INCLUDE_BMA180) #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_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0); PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif #endif
@ -914,6 +919,7 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_MPU6000) #if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure(); PIOS_MPU6000_CONFIG_Configure();
PIOS_MPU6000_Register();
#endif #endif
break; break;
default: default:

1
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -109,6 +109,7 @@ FORMS += \
cc_hw_settings.ui \ cc_hw_settings.ui \
stabilization.ui \ stabilization.ui \
input.ui \ input.ui \
input_wizard.ui \
output.ui \ output.ui \
ccattitude.ui \ ccattitude.ui \
defaultattitude.ui \ defaultattitude.ui \

133
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
View File

@ -80,6 +80,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
ui = new Ui_InputWidget(); ui = new Ui_InputWidget();
ui->setupUi(this); ui->setupUi(this);
wizardUi = new Ui_InputWizardWidget();
wizardUi->setupUi(ui->wizard);
addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD); addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
@ -154,9 +157,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int))); connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int)));
connect(ui->runCalibration, SIGNAL(toggled(bool)), this, SLOT(simpleCalibration(bool))); connect(ui->runCalibration, SIGNAL(toggled(bool)), this, SLOT(simpleCalibration(bool)));
connect(ui->wzNext, SIGNAL(clicked()), this, SLOT(wzNext())); connect(wizardUi->wzNext, SIGNAL(clicked()), this, SLOT(wzNext()));
connect(ui->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel())); connect(wizardUi->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel()));
connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack())); connect(wizardUi->wzBack, SIGNAL(clicked()), this, SLOT(wzBack()));
ui->stackedWidget->setCurrentIndex(0); ui->stackedWidget->setCurrentIndex(0);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true); addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
@ -207,11 +210,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
// Connect the help button // Connect the help button
connect(ui->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); connect(ui->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
ui->graphicsView->setScene(new QGraphicsScene(this)); wizardUi->graphicsView->setScene(new QGraphicsScene(this));
ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); wizardUi->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
m_renderer = new QSvgRenderer(); m_renderer = new QSvgRenderer();
QGraphicsScene *l_scene = ui->graphicsView->scene(); QGraphicsScene *l_scene = wizardUi->graphicsView->scene();
ui->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); wizardUi->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor()));
if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) { if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) {
l_scene->clear(); // Deletes all items contained in the scene as well. l_scene->clear(); // Deletes all items contained in the scene as well.
@ -323,7 +326,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
m_txAccess2Orig.translate(orig.x(), orig.y()); m_txAccess2Orig.translate(orig.x(), orig.y());
m_txAccess2->setTransform(m_txAccess2Orig, true); m_txAccess2->setTransform(m_txAccess2Orig, true);
} }
ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio); wizardUi->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio);
animate = new QTimer(this); animate = new QTimer(this);
connect(animate, SIGNAL(timeout()), this, SLOT(moveTxControls())); connect(animate, SIGNAL(timeout()), this, SLOT(moveTxControls()));
@ -377,7 +380,7 @@ void ConfigInputWidget::resizeEvent(QResizeEvent *event)
{ {
QWidget::resizeEvent(event); QWidget::resizeEvent(event);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
} }
void ConfigInputWidget::openHelp() void ConfigInputWidget::openHelp()
@ -426,7 +429,7 @@ void ConfigInputWidget::goToWizard()
// start the wizard // start the wizard
wizardSetUpStep(wizardWelcome); wizardSetUpStep(wizardWelcome);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
} }
void ConfigInputWidget::disableWizardButton(int value) void ConfigInputWidget::disableWizardButton(int value)
@ -600,8 +603,7 @@ void ConfigInputWidget::wzBack()
void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
{ {
ui->wzText2->clear(); wizardUi->wzNext->setText(tr("Next"));
ui->wzNext->setText(tr("Next"));
switch (step) { switch (step) {
case wizardWelcome: case wizardWelcome:
@ -611,43 +613,36 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
} }
} }
extraWidgets.clear(); extraWidgets.clear();
ui->graphicsView->setVisible(false); wizardUi->graphicsView->setVisible(false);
setTxMovement(nothing); setTxMovement(nothing);
ui->wzText->setText(tr("Welcome to the inputs configuration wizard.\n\n" wizardUi->wzBack->setEnabled(false);
"Please follow the instructions on the screen and only move your controls when asked to.\n" wizardUi->pagesStack->setCurrentWidget(wizardUi->welcomePage);
"Make sure you already configured your hardware settings on the proper tab and restarted your board.\n\n"
"You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.\n"));
ui->stackedWidget->setCurrentIndex(1); ui->stackedWidget->setCurrentIndex(1);
ui->wzBack->setEnabled(false);
break; break;
case wizardChooseType: case wizardChooseType:
{ {
ui->graphicsView->setVisible(true); wizardUi->graphicsView->setVisible(true);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio); wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
setTxMovement(nothing); setTxMovement(nothing);
ui->wzText->setText(tr("Please choose your transmitter type:")); wizardUi->wzBack->setEnabled(true);
ui->wzBack->setEnabled(true);
QRadioButton *typeAcro = new QRadioButton(tr("Acro: normal transmitter for fixed-wing or quad"), this);
QRadioButton *typeHeli = new QRadioButton(tr("Helicopter: has collective pitch and throttle input"), this);
if (transmitterType == heli) { if (transmitterType == heli) {
typeHeli->setChecked(true); wizardUi->typeHeli->setChecked(true);
} else { } else {
typeAcro->setChecked(true); wizardUi->typeAcro->setChecked(true);
} }
ui->wzText2->setText(tr("If selecting the Helicopter option, please engage throttle hold now.")); wizardUi->pagesStack->setCurrentWidget(wizardUi->chooseTypePage);
extraWidgets.clear();
extraWidgets.append(typeAcro);
extraWidgets.append(typeHeli);
ui->radioButtonsLayout->layout()->addWidget(typeAcro);
ui->radioButtonsLayout->layout()->addWidget(typeHeli);
} }
break; break;
case wizardChooseMode: case wizardChooseMode:
{ {
ui->wzBack->setEnabled(true); wizardUi->wzBack->setEnabled(true);
extraWidgets.clear(); QRadioButton* modeButtons[] = {
ui->wzText->setText(tr("Please choose your transmitter mode:")); wizardUi->mode1Button,
wizardUi->mode2Button,
wizardUi->mode3Button,
wizardUi->mode4Button
};
for (int i = 0; i <= mode4; ++i) { for (int i = 0; i <= mode4; ++i) {
QString label; QString label;
txMode mode = static_cast<txMode>(i); txMode mode = static_cast<txMode>(i);
@ -659,6 +654,7 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break; case mode4: label = tr("Mode 4: Throttle/Collective and Left/Right Cyclic on the left, Fore/Aft Cyclic and Yaw on the right"); break;
default: Q_ASSERT(0); break; default: Q_ASSERT(0); break;
} }
wizardUi->typePageFooter->setText(" ");
} else { } else {
switch (mode) { switch (mode) {
case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break; case mode1: label = tr("Mode 1: Elevator and Rudder on the left, Throttle and Ailerons on the right"); break;
@ -667,15 +663,14 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break; case mode4: label = tr("Mode 4: Throttle and Ailerons on the left, Elevator and Rudder on the right"); break;
default: Q_ASSERT(0); break; default: Q_ASSERT(0); break;
} }
ui->wzText2->setText(tr("For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw.")); wizardUi->typePageFooter->setText(tr("For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw."));
} }
QRadioButton *modeButton = new QRadioButton(label, this); modeButtons[i]->setText(label);
if (transmitterMode == mode) { if (transmitterMode == mode) {
modeButton->setChecked(true); modeButtons[i]->setChecked(true);
} }
extraWidgets.append(modeButton);
ui->radioButtonsLayout->layout()->addWidget(modeButton);
} }
wizardUi->pagesStack->setCurrentWidget(wizardUi->chooseModePage);
} }
break; break;
case wizardIdentifySticks: case wizardIdentifySticks:
@ -684,17 +679,16 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
nextChannel(); nextChannel();
manualSettingsData = manualSettingsObj->getData(); manualSettingsData = manualSettingsObj->getData();
connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls()));
ui->wzNext->setEnabled(false); wizardUi->wzNext->setEnabled(false);
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifySticksPage);
break; break;
case wizardIdentifyCenter: case wizardIdentifyCenter:
setTxMovement(centerAll); setTxMovement(centerAll);
ui->wzText->setText(QString(tr("Please center all controls and trims and press Next when ready.\n\n" wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyCenterPage);
"If your FlightMode switch has only two positions, leave it in either position.")));
break; break;
case wizardIdentifyLimits: case wizardIdentifyLimits:
{ {
setTxMovement(nothing); setTxMovement(nothing);
ui->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions.\n\nPress Next when ready.")));
manualSettingsData = manualSettingsObj->getData(); manualSettingsData = manualSettingsObj->getData();
for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) { for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) {
// Preserve the inverted status // Preserve the inverted status
@ -711,6 +705,8 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyLimitsPage);
} }
break; break;
case wizardIdentifyInverted: case wizardIdentifyInverted:
@ -724,22 +720,20 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
QCheckBox *cb = new QCheckBox(name, this); QCheckBox *cb = new QCheckBox(name, this);
// Make sure checked status matches current one // Make sure checked status matches current one
cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]); cb->setChecked(manualSettingsData.ChannelMax[index] < manualSettingsData.ChannelMin[index]);
dynamic_cast<QGridLayout *>(ui->checkBoxesLayout->layout())->addWidget(cb, extraWidgets.size() / 4, extraWidgets.size() % 4); wizardUi->checkBoxesLayout->addWidget(cb, extraWidgets.size() / 4, extraWidgets.size() % 4);
extraWidgets.append(cb); extraWidgets.append(cb);
connect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls())); connect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls()));
} }
} }
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
ui->wzText->setText(QString(tr("Please check the picture below and correct all the sticks which show an inverted movement. Press Next when ready."))); wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyInvertedPage);
break; break;
case wizardFinish: case wizardFinish:
dimOtherControls(false); dimOtherControls(false);
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks())); connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
ui->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture mimics your sticks movement.\n\n" wizardUi->pagesStack->setCurrentWidget(wizardUi->finishPage);
"IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings "
"tab where you can set your desired arming sequence and save the configuration.")));
break; break;
default: default:
Q_ASSERT(0); Q_ASSERT(0);
@ -749,36 +743,35 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step)
{ {
QRadioButton *mode, *type;
Q_ASSERT(step == wizardStep); Q_ASSERT(step == wizardStep);
switch (step) { switch (step) {
case wizardWelcome: case wizardWelcome:
break; break;
case wizardChooseType: case wizardChooseType:
type = qobject_cast<QRadioButton *>(extraWidgets.at(0)); if (wizardUi->typeAcro->isChecked()) {
if (type->isChecked()) {
transmitterType = acro; transmitterType = acro;
} else { } else {
transmitterType = heli; transmitterType = heli;
} }
delete extraWidgets.at(0);
delete extraWidgets.at(1);
extraWidgets.clear();
break; break;
case wizardChooseMode: case wizardChooseMode:
{
QRadioButton* modeButtons[] = {
wizardUi->mode1Button,
wizardUi->mode2Button,
wizardUi->mode3Button,
wizardUi->mode4Button
};
for (int i = mode1; i <= mode4; ++i) { for (int i = mode1; i <= mode4; ++i) {
mode = qobject_cast<QRadioButton *>(extraWidgets.first()); if (modeButtons[i]->isChecked()) {
if (mode->isChecked()) {
transmitterMode = static_cast<txMode>(i); transmitterMode = static_cast<txMode>(i);
} }
delete mode; }
extraWidgets.removeFirst();
} }
break; break;
case wizardIdentifySticks: case wizardIdentifySticks:
disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls())); disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls()));
ui->wzNext->setEnabled(true); wizardUi->wzNext->setEnabled(true);
setTxMovement(nothing); setTxMovement(nothing);
break; break;
case wizardIdentifyCenter: case wizardIdentifyCenter:
@ -861,22 +854,22 @@ void ConfigInputWidget::restoreMdata()
void ConfigInputWidget::setChannel(int newChan) void ConfigInputWidget::setChannel(int newChan)
{ {
if (newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) { if (newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) {
ui->wzText->setText(QString(tr("Please enable throttle hold mode.\n\nMove the Collective Pitch stick."))); wizardUi->identifyStickInstructions->setText(QString(tr("Please enable throttle hold mode.\n\nMove the Collective Pitch stick.")));
} else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) { } else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) {
ui->wzText->setText(QString(tr("Please toggle the Flight Mode switch.\n\nFor switches you may have to repeat this rapidly."))); wizardUi->identifyStickInstructions->setText(QString(tr("Please toggle the Flight Mode switch.\n\nFor switches you may have to repeat this rapidly.")));
} else if ((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) { } else if ((transmitterType == heli) && (newChan == ManualControlSettings::CHANNELGROUPS_THROTTLE)) {
ui->wzText->setText(QString(tr("Please disable throttle hold mode.\n\nMove the Throttle stick."))); wizardUi->identifyStickInstructions->setText(QString(tr("Please disable throttle hold mode.\n\nMove the Throttle stick.")));
} else { } else {
ui->wzText->setText(QString(tr("Please move each control one at a time according to the instructions and picture below.\n\n" wizardUi->identifyStickInstructions->setText(QString(tr("Please move each control one at a time according to the instructions and picture below.\n\n"
"Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); "Move the %1 stick.")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan)));
} }
if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) { if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) {
ui->wzNext->setEnabled(true); wizardUi->wzNext->setEnabled(true);
ui->wzNext->setText(tr("Next / Skip")); wizardUi->wzNext->setText(tr("Next / Skip"));
ui->wzText->setText(ui->wzText->text() + tr(" Alternatively, click Next to skip this channel.")); wizardUi->identifyStickInstructions->setText(wizardUi->identifyStickInstructions->text() + tr(" Alternatively, click Next to skip this channel."));
} else { } else {
ui->wzNext->setEnabled(false); wizardUi->wzNext->setEnabled(false);
} }
setMoveFromCommand(newChan); setMoveFromCommand(newChan);

3
ground/openpilotgcs/src/plugins/config/configinputwidget.h
View File

@ -28,6 +28,7 @@
#define CONFIGINPUTWIDGET_H #define CONFIGINPUTWIDGET_H
#include "ui_input.h" #include "ui_input.h"
#include "ui_input_wizard.h"
#include "../uavobjectwidgetutils/configtaskwidget.h" #include "../uavobjectwidgetutils/configtaskwidget.h"
#include "extensionsystem/pluginmanager.h" #include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h" #include "uavobjectmanager.h"
@ -76,6 +77,8 @@ private:
int movePos; int movePos;
void setTxMovement(txMovements movement); void setTxMovement(txMovements movement);
Ui_InputWidget *ui; Ui_InputWidget *ui;
Ui_InputWizardWidget *wizardUi;
wizardSteps wizardStep; wizardSteps wizardStep;
QList<QPointer<QWidget> > extraWidgets; QList<QPointer<QWidget> > extraWidgets;
txMode transmitterMode; txMode transmitterMode;

78
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -275,80 +275,6 @@
</layout> </layout>
</widget> </widget>
<widget class="QWidget" name="wizard"> <widget class="QWidget" name="wizard">
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="leftMargin">
<number>12</number>
</property>
<property name="topMargin">
<number>12</number>
</property>
<property name="rightMargin">
<number>12</number>
</property>
<property name="bottomMargin">
<number>12</number>
</property>
<item>
<widget class="QLabel" name="wzText">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QVBoxLayout" name="radioButtonsLayout"/>
</item>
<item>
<layout class="QGridLayout" name="checkBoxesLayout"/>
</item>
<item>
<widget class="QLabel" name="wzText2">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QGraphicsView" name="graphicsView"/>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QPushButton" name="wzBack">
<property name="text">
<string>Back</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="wzNext">
<property name="text">
<string>Next</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="wzCancel">
<property name="text">
<string>Cancel</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget> </widget>
</widget> </widget>
</item> </item>
@ -2551,10 +2477,6 @@ Applies and Saves all settings to SD</string>
<tabstops> <tabstops>
<tabstop>tabWidget</tabstop> <tabstop>tabWidget</tabstop>
<tabstop>deadband</tabstop> <tabstop>deadband</tabstop>
<tabstop>graphicsView</tabstop>
<tabstop>wzBack</tabstop>
<tabstop>wzNext</tabstop>
<tabstop>wzCancel</tabstop>
<tabstop>armControl</tabstop> <tabstop>armControl</tabstop>
<tabstop>armTimeout</tabstop> <tabstop>armTimeout</tabstop>
<tabstop>inputHelp</tabstop> <tabstop>inputHelp</tabstop>

271
ground/openpilotgcs/src/plugins/config/input_wizard.ui Normal file
View File

@ -0,0 +1,271 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>InputWizardWidget</class>
<widget class="QWidget" name="InputWizardWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>644</width>
<height>529</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QStackedWidget" name="pagesStack">
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="welcomePage">
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0">
<widget class="QLabel" name="welcomeText">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>Welcome to the inputs configuration wizard.
Please follow the instructions on the screen and only move your controls when asked to.
Make sure you already configured your hardware settings on the proper tab and restarted your board.
You can press 'back' at any time to return to the previous screen or press 'Cancel' to quit the wizard.
</string>
</property>
<property name="textFormat">
<enum>Qt::AutoText</enum>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="chooseTypePage">
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="typePageHeader">
<property name="text">
<string>Please choose your transmitter type:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="typeAcro">
<property name="text">
<string>Acro: normal transmitter for fixed-wing or quad</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="typeHeli">
<property name="text">
<string>Helicopter: has collective pitch and throttle input</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="typePageFooter">
<property name="text">
<string>If selecting the Helicopter option, please engage throttle hold now.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="chooseModePage">
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QLabel" name="modePageHeader">
<property name="text">
<string>Please choose your transmitter mode:</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="mode1Button">
<property name="text">
<string>Mode 1</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="mode2Button">
<property name="text">
<string>Mode 2</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="mode3Button">
<property name="text">
<string>Mode 3</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="mode4Button">
<property name="text">
<string>Mode 4</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="modePageFooter">
<property name="text">
<string>For a Quad: Elevator is Pitch, Ailerons are Roll, and Rudder is Yaw.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="identifySticksPage">
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<widget class="QLabel" name="identifyStickInstructions">
<property name="text">
<string>Identify sticks instructions</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="identifyCenterPage">
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="0">
<widget class="QLabel" name="identifyCenterInstructions">
<property name="text">
<string>Please center all controls and trims and press Next when ready.
If your FlightMode switch has only two positions, leave it in either position.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="identifyLimitsPage">
<layout class="QGridLayout" name="gridLayout_4">
<item row="0" column="0">
<widget class="QLabel" name="identifyLimitsInstructions">
<property name="text">
<string>Please move all controls to their maximum extents on both directions.
Press Next when ready.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="identifyInvertedPage">
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<layout class="QGridLayout" name="checkBoxesLayout"/>
</item>
<item>
<widget class="QLabel" name="identifyInvertedInstructions">
<property name="text">
<string>Please check the picture below and correct all the sticks which show an inverted movement. Press Next when ready.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="finishPage">
<layout class="QGridLayout" name="gridLayout_5">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>You have completed this wizard, please check below if the picture mimics your sticks movement.
IMPORTANT: These new settings have not been saved to the board yet. After pressing Next you will go to the Arming Settings tab where you can set your desired arming sequence and save the configuration.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<widget class="QGraphicsView" name="graphicsView">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>1</verstretch>
</sizepolicy>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QPushButton" name="wzBack">
<property name="text">
<string>Back</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="wzNext">
<property name="text">
<string>Next</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="wzCancel">
<property name="text">
<string>Cancel</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>

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_rcvr.c
SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_sbus.c
SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_sensors.c
## Misc library functions ## Misc library functions
SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(FLIGHTLIB)/sanitycheck.c

1
shared/uavobjectdefinition/magsensor.xml
View File

@ -4,6 +4,7 @@
<field name="x" units="mGa" type="float" elements="1"/> <field name="x" units="mGa" type="float" elements="1"/>
<field name="y" units="mGa" type="float" elements="1"/> <field name="y" units="mGa" type="float" elements="1"/>
<field name="z" units="mGa" type="float" elements="1"/> <field name="z" units="mGa" type="float" elements="1"/>
<field name="temperature" units="deg C" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="10000"/> <telemetryflight acked="false" updatemode="periodic" period="10000"/>