rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +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
* @brief Communicate with BMP085 and update @ref AltitudeActual "AltitudeActual UAV Object"
* @addtogroup Reuseable vector data type and functions
* @{
*
* @file altitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Altitude module, reads temperature and pressure from BMP085
* @file vectors.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Reuseable vector data type and functions
*
* @see The GNU Public License (GPL) Version 3
*
@ -28,14 +27,37 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ALTITUDE_H
#define ALTITUDE_H
int32_t AltitudeInitialize();
#ifndef VECTORS_H_
#define VECTORS_H_
#endif // ALTITUDE_H
#include <math.h>
#include <stdint.h>
/**
* @}
* @}
*/
#define DECLAREVECTOR3(suffix, datatype) \
typedef struct Vector3##suffix##_t { \
datatype x; \
datatype y; \
datatype z; \
} Vector3##suffix
#define DECLAREVECTOR2(suffix, datatype) \
typedef struct Vector2##suffix##_t { \
datatype x; \
datatype y; \
} Vector2##suffix
DECLAREVECTOR3(i16, int16_t);
DECLAREVECTOR3(i32, int32_t);
DECLAREVECTOR3(u16, uint16_t);
DECLAREVECTOR3(u32, uint32_t);
DECLAREVECTOR3(f, float);
DECLAREVECTOR2(i16, int16_t);
DECLAREVECTOR2(i32, int32_t);
DECLAREVECTOR2(u16, uint16_t);
DECLAREVECTOR2(u32, uint32_t);
DECLAREVECTOR2(f, float);
#endif /* VECTORS_H_ */

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

@ -1,221 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AltitudeModule Altitude Module
* @brief Communicate with BMP085 and update @ref BaroSensor "BaroSensor UAV Object"
* @{
*
* @file altitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Altitude module, handles temperature and pressure readings from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: BaroSensor
*
* This module will periodically update the value of the BaroSensor object.
*
*/
#include <openpilot.h>
#include "altitude.h"
#include "barosensor.h" // object that will be updated by the module
#include "revosettings.h"
#include <mathmisc.h>
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module
#endif
#include "taskinfo.h"
// Private constants
#define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 10
// LPF
#define TEMP_DT (1.0f / 120.0f)
#define TEMP_LPF_FC 5.0f
static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
// Private types
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t temp_calibration_count = 0;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeStart()
{
// Start main task
xTaskCreate(altitudeTask, "Altitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDE, taskHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
{
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
return 0;
}
MODULE_INITCALL(AltitudeInitialize, AltitudeStart);
/**
* Module thread, should not return.
*/
static void altitudeTask(__attribute__((unused)) void *parameters)
{
BaroSensorData data;
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeData sonardata;
int32_t value = 0, timeout = 10, sample_rate = 0;
float coeff = 0.25, height_out = 0, height_in = 0;
PIOS_HCSR04_Trigger();
#endif
// TODO: Check the pressure sensor and set a warning if it fails test
// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
// #define PIOS_MS5611_SLOW_TEMP_RATE 20
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
uint8_t temp_press_interleave_count = 1;
#endif
// Main task loop
while (1) {
#if defined(PIOS_INCLUDE_HCSR04)
// Compute the current altitude
// depends on baro samplerate
if (!(sample_rate--)) {
if (PIOS_HCSR04_Completed()) {
value = PIOS_HCSR04_Get();
// from 3.4cm to 5.1m
if ((value > 100) && (value < 15000)) {
height_in = value * 0.00034f / 2.0f;
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
sonardata.Altitude = height_out; // m/us
}
// Update the SonarAltitude UAVObject
SonarAltitudeSet(&sonardata);
timeout = 10;
PIOS_HCSR04_Trigger();
}
if (!(timeout--)) {
// retrigger
timeout = 10;
PIOS_HCSR04_Trigger();
}
sample_rate = 25;
}
#endif /* if defined(PIOS_INCLUDE_HCSR04) */
float temp, press;
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
temp_press_interleave_count--;
if (temp_press_interleave_count == 0) {
#endif
// Update the temperature data
PIOS_MS5611_StartADC(TemperatureConv);
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
#ifdef PIOS_MS5611_SLOW_TEMP_RATE
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
}
#endif
// Update the pressure data
PIOS_MS5611_StartADC(PressureConv);
vTaskDelay(PIOS_MS5611_GetDelay());
PIOS_MS5611_ReadADC();
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
if (isnan(baro_temperature)) {
baro_temperature = temp;
}
baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
if (tempCorrectionEnabled && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
// pressure bias = A + B*t + C*t^2 + D * t^3
// in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
}
press -= baro_temp_bias;
float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
data.Altitude = altitude;
data.Temperature = temp;
data.Pressure = press;
// Update the BasoSensor UAVObject
BaroSensorSet(&data);
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}
* @}
*/

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

@ -62,6 +62,10 @@
#include "manualcontrolcommand.h"
#include "taskinfo.h"
#include <pios_sensors.h>
#include <pios_adxl345.h>
#include <pios_mpu6000.h>
#include "CoordinateConversions.h"
#include <pios_notify.h>
#include <mathmisc.h>
@ -160,9 +164,11 @@ int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
static struct PIOS_SENSORS_3Axis_SensorsWithTemp *mpu6000_data = NULL;
// Used to detect CC vs CC3D
static const struct pios_board_info *bdinfo = &pios_board_info_blob;
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
#define BOARDISCC3D (bdinfo->board_rev == 0x02)
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
@ -238,7 +244,9 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
if (cc3d) {
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
gyro_test = PIOS_MPU6000_Driver.test(0);
mpu6000_data = pios_malloc(sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * 2);
#endif
} else {
#if defined(PIOS_INCLUDE_ADXL345)
@ -455,7 +463,6 @@ static int32_t updateSensors(AccelStateData *accelState, GyroStateData *gyros)
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static struct pios_mpu6000_data mpu6000_data;
static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *gyrosData)
{
float accels[3] = { 0 };
@ -465,18 +472,18 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue();
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
xQueueHandle queue = PIOS_MPU6000_Driver.get_queue(0);
BaseType_t ret = xQueueReceive(queue, (void *)mpu6000_data, sensor_period_ms);
while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x;
gyros[1] += mpu6000_data.gyro_y;
gyros[2] += mpu6000_data.gyro_z;
gyros[0] += mpu6000_data->sample[1].x;
gyros[1] += mpu6000_data->sample[1].y;
gyros[2] += mpu6000_data->sample[1].z;
accels[0] += mpu6000_data.accel_x;
accels[1] += mpu6000_data.accel_y;
accels[2] += mpu6000_data.accel_z;
accels[0] += mpu6000_data->sample[0].x;
accels[1] += mpu6000_data->sample[0].y;
accels[2] += mpu6000_data->sample[0].z;
temp += mpu6000_data.temperature;
temp += mpu6000_data->temperature;
count++;
// check if further samples are already in queue
@ -751,17 +758,19 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
temp_calibrated_extent.max = accelGyroSettings.temp_calibrated_extent.max;
if (BOARDISCC3D) {
float scales[2];
PIOS_MPU6000_Driver.get_scale(scales, 2, 0);
accel_bias.X = accelGyroSettings.accel_bias.X;
accel_bias.Y = accelGyroSettings.accel_bias.Y;
accel_bias.Z = accelGyroSettings.accel_bias.Z;
gyro_scale.X = accelGyroSettings.gyro_scale.X * PIOS_MPU6000_GetScale();
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * PIOS_MPU6000_GetScale();
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * PIOS_MPU6000_GetScale();
gyro_scale.X = accelGyroSettings.gyro_scale.X * scales[0];
gyro_scale.Y = accelGyroSettings.gyro_scale.Y * scales[0];
gyro_scale.Z = accelGyroSettings.gyro_scale.Z * scales[0];
accel_scale.X = accelGyroSettings.accel_scale.X * PIOS_MPU6000_GetAccelScale();
accel_scale.Y = accelGyroSettings.accel_scale.Y * PIOS_MPU6000_GetAccelScale();
accel_scale.Z = accelGyroSettings.accel_scale.Z * PIOS_MPU6000_GetAccelScale();
accel_scale.X = accelGyroSettings.accel_scale.X * scales[1];
accel_scale.Y = accelGyroSettings.accel_scale.Y * scales[1];
accel_scale.Z = accelGyroSettings.accel_scale.Z * scales[1];
} else {
// Original CC with analog gyros and ADXL accel
accel_bias.X = accelGyroSettings.accel_bias.X;

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

@ -2,16 +2,17 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Sensors Sensors Module
* @addtogroup Sensors
* @brief Acquires sensor data
* @{
*
* @file attitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
* @file sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Module to handle fetch and preprocessing of sensor data
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by

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

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

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

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

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

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

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

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

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

@ -50,6 +50,22 @@ typedef struct {
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
// sensor driver interface
bool PIOS_HMC5x83_driver_Test(uintptr_t context);
void PIOS_HMC5x83_driver_Reset(uintptr_t context);
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
void PIOS_HMC5x83_driver_fetch(void *, uint8_t size, uintptr_t context);
bool PIOS_HMC5x83_driver_poll(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver = {
.test = PIOS_HMC5x83_driver_Test,
.poll = PIOS_HMC5x83_driver_poll,
.fetch = PIOS_HMC5x83_driver_fetch,
.reset = PIOS_HMC5x83_driver_Reset,
.get_queue = NULL,
.get_scale = PIOS_HMC5x83_driver_get_scale,
.is_polled = true,
};
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
@ -99,6 +115,11 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
return (pios_hmc5x83_dev_t)dev;
}
void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler)
{
PIOS_SENSORS_Register(&PIOS_HMC5x83_Driver, PIOS_SENSORS_TYPE_3AXIS_MAG, handler);
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor
* \return none
@ -549,6 +570,37 @@ int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint
}
#endif /* PIOS_INCLUDE_I2C */
/* PIOS sensor driver implementation */
bool PIOS_HMC5x83_driver_Test(uintptr_t context)
{
return !PIOS_HMC5x83_Test((pios_hmc5x83_dev_t)context);
}
void PIOS_HMC5x83_driver_Reset(__attribute__((unused)) uintptr_t context) {}
void PIOS_HMC5x83_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(size > 0);
scales[0] = 1;
}
void PIOS_HMC5x83_driver_fetch(void *data, uint8_t size, uintptr_t context)
{
PIOS_Assert(size > 0);
int16_t mag[3];
PIOS_HMC5x83_ReadMag((pios_hmc5x83_dev_t)context, mag);
PIOS_SENSORS_3Axis_SensorsWithTemp *tmp = data;
tmp->count = 1;
tmp->sample[0].x = mag[0];
tmp->sample[0].y = mag[1];
tmp->sample[0].z = mag[2];
tmp->temperature = 0;
}
bool PIOS_HMC5x83_driver_poll(uintptr_t context)
{
return PIOS_HMC5x83_NewDataAvailable((pios_hmc5x83_dev_t)context);
}
#endif /* PIOS_INCLUDE_HMC5x83 */

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

@ -52,8 +52,8 @@ pios_counter_t PIOS_Instrumentation_CreateCounter(uint32_t id)
if (!counter_handle) {
pios_perf_counter_t *newcounter = &pios_instrumentation_perf_counters[++pios_instrumentation_last_used_counter];
newcounter->id = id;
newcounter->max = INT32_MIN;
newcounter->min = INT32_MAX;
newcounter->max = INT32_MIN + 1;
newcounter->min = INT32_MAX - 1;
counter_handle = (pios_counter_t)newcounter;
}
return counter_handle;

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

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

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

@ -1,6 +1,6 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @addtogroup PIOS PIOS Core haoftware; you can rnedtt
* @{
* @addtogroup PIOS_MPU6000 MPU6000 Functions
* @brief Deals with the hardware interface to the 3-axis gyro
@ -13,8 +13,8 @@
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
/*istribu
* This program is free software; you can rnedtt ad/oe ir modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
@ -30,20 +30,40 @@
*/
#include "pios.h"
#include <pios_mpu6000.h>
#ifdef PIOS_INCLUDE_MPU6000
#include <stdint.h>
#include <pios_constants.h>
#include <pios_sensors.h>
/* Global Variables */
enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
};
// sensor driver interface
bool PIOS_MPU6000_driver_Test(uintptr_t context);
void PIOS_MPU6000_driver_Reset(uintptr_t context);
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
QueueHandle_t PIOS_MPU6000_driver_get_queue(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_MPU6000_Driver = {
.test = PIOS_MPU6000_driver_Test,
.poll = NULL,
.fetch = NULL,
.reset = PIOS_MPU6000_driver_Reset,
.get_queue = PIOS_MPU6000_driver_get_queue,
.get_scale = PIOS_MPU6000_driver_get_scale,
.is_polled = false,
};
//
struct mpu6000_dev {
uint32_t spi_id;
uint32_t slave_num;
xQueueHandle queue;
uint32_t spi_id;
uint32_t slave_num;
QueueHandle_t queue;
const struct pios_mpu6000_cfg *cfg;
enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_accel_range accel_range;
@ -51,26 +71,19 @@ struct mpu6000_dev {
enum pios_mpu6000_dev_magic magic;
};
#ifdef PIOS_MPU6000_ACCEL
#define PIOS_MPU6000_SAMPLES_BYTES 14
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB
#else
#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_TEMP_OUT_MSB
#define PIOS_MPU6000_SAMPLES_BYTES 8
#endif
typedef union {
uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
struct {
uint8_t dummy;
#ifdef PIOS_MPU6000_ACCEL
uint8_t Accel_X_h;
uint8_t Accel_X_l;
uint8_t Accel_Y_h;
uint8_t Accel_Y_l;
uint8_t Accel_Z_h;
uint8_t Accel_Z_l;
#endif
uint8_t Temperature_h;
uint8_t Temperature_l;
uint8_t Gyro_X_h;
@ -88,7 +101,9 @@ typedef union {
static struct mpu6000_dev *dev;
volatile bool mpu6000_configured = false;
static mpu6000_data_t mpu6000_data;
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
#define SENSOR_COUNT 2
#define SENSOR_DATA_SIZE (sizeof(PIOS_SENSORS_3Axis_SensorsWithTemp) + sizeof(Vector3i16) * SENSOR_COUNT)
// ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
@ -97,8 +112,14 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_MPU6000_GetReg(uint8_t address);
static void PIOS_MPU6000_SetSpeed(const bool fast);
static bool PIOS_MPU6000_HandleData();
static bool PIOS_MPU6000_ReadFifo(bool *woken);
static bool PIOS_MPU6000_ReadSensor(bool *woken);
static int32_t PIOS_MPU6000_Test(void);
void PIOS_MPU6000_Register()
{
PIOS_SENSORS_Register(&PIOS_MPU6000_Driver, PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL, 0);
}
/**
* @brief Allocate a new device
*/
@ -107,18 +128,16 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg
struct mpu6000_dev *mpu6000_dev;
mpu6000_dev = (struct mpu6000_dev *)pios_malloc(sizeof(*mpu6000_dev));
if (!mpu6000_dev) {
return NULL;
}
PIOS_Assert(mpu6000_dev);
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
if (mpu6000_dev->queue == NULL) {
vPortFree(mpu6000_dev);
return NULL;
}
mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, SENSOR_DATA_SIZE);
PIOS_Assert(mpu6000_dev->queue);
queue_data = (PIOS_SENSORS_3Axis_SensorsWithTemp *)pios_malloc(SENSOR_DATA_SIZE);
PIOS_Assert(queue_data);
queue_data->count = SENSOR_COUNT;
return mpu6000_dev;
}
@ -211,15 +230,9 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg)
}
// FIFO storage
#if defined(PIOS_MPU6000_ACCEL)
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) {
;
}
#else
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) {
;
}
#endif
PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter);
// Interrupt configuration
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) {
@ -279,14 +292,12 @@ int32_t PIOS_MPU6000_ConfigureRanges(
}
dev->gyro_range = gyroRange;
#if defined(PIOS_MPU6000_ACCEL)
// Set the accel range
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_ACCEL_CFG_REG, accelRange) != 0) {
;
}
dev->accel_range = accelRange;
#endif
return 0;
}
@ -414,11 +425,10 @@ static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* @brief Perform a dummy read in order to restart interrupt generation
* \returns 0 if succesful
*/
int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
int32_t PIOS_MPU6000_DummyReadGyros()
{
// THIS FUNCTION IS DEPRECATED AND DOES NOT PERFORM A ROTATION
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
@ -434,10 +444,6 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
PIOS_MPU6000_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
return 0;
}
@ -469,7 +475,7 @@ xQueueHandle PIOS_MPU6000_GetQueue()
}
float PIOS_MPU6000_GetScale()
static float PIOS_MPU6000_GetScale()
{
switch (dev->gyro_range) {
case PIOS_MPU6000_SCALE_250_DEG:
@ -487,7 +493,7 @@ float PIOS_MPU6000_GetScale()
return 0;
}
float PIOS_MPU6000_GetAccelScale()
static float PIOS_MPU6000_GetAccelScale()
{
switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G:
@ -510,7 +516,7 @@ float PIOS_MPU6000_GetAccelScale()
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
int32_t PIOS_MPU6000_Test(void)
static int32_t PIOS_MPU6000_Test(void)
{
/* Verify that ID matches (MPU6000 ID is 0x69) */
int32_t mpu6000_id = PIOS_MPU6000_ReadID();
@ -526,172 +532,76 @@ int32_t PIOS_MPU6000_Test(void)
return 0;
}
/**
* @brief Reads the contents of the MPU6000 Interrupt Status register from an ISR
* @return The register value or -1 on failure to claim the bus
*/
static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
{
/* Interrupt Status register can be read at high SPI clock speed */
uint8_t data;
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
data = PIOS_SPI_TransferByte(dev->spi_id, 0);
PIOS_MPU6000_ReleaseBusISR(woken);
return data;
}
/**
* @brief Resets the MPU6000 FIFO from an ISR
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
* @return 0 if operation was successful
* @return -1 if unable to claim SPI bus
* @return -2 if write to the device failed
*/
static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{
int32_t result = 0;
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
/* Reset FIFO. */
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
result = -2;
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
result = -2;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return result;
}
/**
* @brief Obtains the number of bytes in the FIFO. Call from ISR only.
* @return the number of bytes in the FIFO
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
* task has is now eligible to run, else unchanged
*/
static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
{
uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
uint8_t mpu6000_rec_buf[3];
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
return -1;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return (mpu6000_rec_buf[1] << 8) | mpu6000_rec_buf[2];
}
/**
* @brief EXTI IRQ Handler. Read all the data from onboard buffer
* @return a boolean to the EXTI IRQ Handler wrapper indicating if a
* @return a boleoan to the EXTI IRQ Handler wrapper indicating if a
* higher priority task is now eligible to run
*/
uint32_t mpu6000_irq = 0;
int32_t mpu6000_count;
uint32_t mpu6000_fifo_backup = 0;
uint8_t mpu6000_last_read_count = 0;
uint32_t mpu6000_fails = 0;
uint32_t mpu6000_interval_us;
uint32_t mpu6000_time_us;
uint32_t mpu6000_transfer_size;
bool PIOS_MPU6000_IRQHandler(void)
{
bool woken = false;
static uint32_t timeval;
mpu6000_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if (!mpu6000_configured) {
return false;
}
bool read_ok = false;
if (dev->cfg->User_ctl & PIOS_MPU6000_USERCTL_FIFO_EN) {
read_ok = PIOS_MPU6000_ReadFifo(&woken);
} else {
read_ok = PIOS_MPU6000_ReadSensor(&woken);
}
read_ok = PIOS_MPU6000_ReadSensor(&woken);
if (read_ok) {
bool woken2 = PIOS_MPU6000_HandleData();
woken |= woken2;
}
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return woken;
}
static bool PIOS_MPU6000_HandleData()
{
if (!queue_data) {
return false;
}
// Rotate the sensor to OP convention. The datasheet defines X as towards the right
// and Y as forward. OP convention transposes this. Also the Z is defined negatively
// to our convention
static struct pios_mpu6000_data data;
// Currently we only support rotations on top so switch X/Y accordingly
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
#endif
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
break;
case PIOS_MPU6000_TOP_90DEG:
// -1 to bring it back to -32768 +32767 range
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
#endif
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
data.gyro_x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
queue_data->sample[0].x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
queue_data->sample[1].x = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
#endif
data.gyro_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
queue_data->sample[0].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
queue_data->sample[1].y = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
break;
case PIOS_MPU6000_TOP_270DEG:
#ifdef PIOS_MPU6000_ACCEL
data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
#endif
data.gyro_y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
data.gyro_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
queue_data->sample[0].y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
queue_data->sample[0].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
queue_data->sample[1].y = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
queue_data->sample[1].x = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
break;
}
#ifdef PIOS_MPU6000_ACCEL
data.accel_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
#endif
data.gyro_z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
queue_data->sample[0].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
queue_data->sample[1].z = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
const int16_t temp = GET_SENSOR_DATA(mpu6000_data, Temperature);
queue_data->temperature = 3500 + ((float)(temp + 512)) * (1.0f / 3.4f);
BaseType_t higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
xQueueSendToBackFromISR(dev->queue, (void *)queue_data, &higherPriorityTaskWoken);
return higherPriorityTaskWoken == pdTRUE;
}
@ -704,72 +614,33 @@ static bool PIOS_MPU6000_ReadSensor(bool *woken)
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
return true;
}
static bool PIOS_MPU6000_ReadFifo(bool *woken)
// Sensor driver implementation
bool PIOS_MPU6000_driver_Test(__attribute__((unused)) uintptr_t context)
{
/* Temporary fix for OP-1049. Expected to be superceded for next major release
* by code changes for OP-1039.
* Read interrupt status register to check for FIFO overflow. Must be the
* first read after interrupt, in case the device is configured so that
* any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
* interrupt config register) */
int32_t result;
return !PIOS_MPU6000_Test();
}
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
return false;
}
if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
/* The FIFO has overflowed, so reset it,
* to enable sample sync to be recovered.
* If the reset fails, we are in trouble, but
* we keep trying on subsequent interrupts. */
PIOS_MPU6000_ResetFifoISR(woken);
/* Return and wait for the next new sample. */
return false;
}
void PIOS_MPU6000_driver_Reset(__attribute__((unused)) uintptr_t context)
{
PIOS_MPU6000_DummyReadGyros();
}
/* Usual case - FIFO has not overflowed. */
mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
return false;
}
void PIOS_MPU6000_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t contet)
{
PIOS_Assert(size >= 2);
scales[0] = PIOS_MPU6000_GetAccelScale();
scales[1] = PIOS_MPU6000_GetScale();
}
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
// In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return false;
}
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++;
return false;
}
PIOS_MPU6000_ReleaseBusISR(woken);
}
return true;
QueueHandle_t PIOS_MPU6000_driver_get_queue(__attribute__((unused)) uintptr_t context)
{
return dev->queue;
}
#endif /* PIOS_INCLUDE_MPU6000 */

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

@ -29,17 +29,40 @@
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_MS5611
#include <pios_ms5611.h>
#define POW2(x) (1 << x)
// TODO: Clean this up. Getting around old constant.
#define PIOS_MS5611_OVERSAMPLING oversampling
#define PIOS_MS5611_OVERSAMPLING oversampling
// Option to change the interleave between Temp and Pressure conversions
// Undef for normal operation
#define PIOS_MS5611_SLOW_TEMP_RATE 20
#ifndef PIOS_MS5611_SLOW_TEMP_RATE
#define PIOS_MS5611_SLOW_TEMP_RATE 1
#endif
/* Local Types */
typedef struct {
uint16_t C[6];
} MS5611CalibDataTypeDef;
typedef enum {
MS5611_CONVERSION_TYPE_None = 0,
MS5611_CONVERSION_TYPE_PressureConv,
MS5611_CONVERSION_TYPE_TemperatureConv
} ConversionTypeTypeDef;
typedef enum {
MS5611_FSM_INIT = 0,
MS5611_FSM_TEMPERATURE,
MS5611_FSM_PRESSURE,
MS5611_FSM_CALCULATE,
} MS5611_FSM_State;
/* Glocal Variables */
ConversionTypeTypeDef CurrentRead;
ConversionTypeTypeDef CurrentRead = MS5611_CONVERSION_TYPE_None;
/* Local Variables */
MS5611CalibDataTypeDef CalibData;
@ -50,8 +73,14 @@ static uint32_t RawPressure;
static int64_t Pressure;
static int64_t Temperature;
static int32_t lastConversionStart;
static uint32_t conversionDelayMs;
static uint32_t conversionDelayUs;
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
static uint32_t PIOS_MS5611_GetDelay();
static uint32_t PIOS_MS5611_GetDelayUs();
// Second order temperature compensation. Temperature offset
static int64_t compensation_t2;
@ -60,17 +89,36 @@ static int64_t compensation_t2;
static uint32_t oversampling;
static const struct pios_ms5611_cfg *dev_cfg;
static int32_t i2c_id;
static PIOS_SENSORS_1Axis_SensorsWithTemp results;
// sensor driver interface
bool PIOS_MS5611_driver_Test(uintptr_t context);
void PIOS_MS5611_driver_Reset(uintptr_t context);
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, uintptr_t context);
void PIOS_MS5611_driver_fetch(void *, uint8_t size, uintptr_t context);
bool PIOS_MS5611_driver_poll(uintptr_t context);
const PIOS_SENSORS_Driver PIOS_MS5611_Driver = {
.test = PIOS_MS5611_driver_Test,
.poll = PIOS_MS5611_driver_poll,
.fetch = PIOS_MS5611_driver_fetch,
.reset = PIOS_MS5611_driver_Reset,
.get_queue = NULL,
.get_scale = PIOS_MS5611_driver_get_scale,
.is_polled = true,
};
/**
* Initialise the MS5611 sensor
*/
int32_t ms5611_read_flag;
void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
{
i2c_id = i2c_device;
i2c_id = i2c_device;
oversampling = cfg->oversampling;
conversionDelayMs = PIOS_MS5611_GetDelay();
conversionDelayUs = PIOS_MS5611_GetDelayUs();
dev_cfg = cfg; // Store cfg before enabling interrupt
PIOS_MS5611_WriteCommand(MS5611_RESET);
@ -80,10 +128,10 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
// reset temperature compensation values
compensation_t2 = 0;
/* Calibration parameters */
for (int i = 0; i < 6; i++) {
PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
while (PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2)) {}
;
CalibData.C[i] = (data[0] << 8) | data[1];
}
}
@ -96,11 +144,11 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
{
/* Start the conversion */
if (Type == TemperatureConv) {
if (Type == MS5611_CONVERSION_TYPE_TemperatureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_TEMP_ADDR + oversampling) != 0) {
continue;
}
} else if (Type == PressureConv) {
} else if (Type == MS5611_CONVERSION_TYPE_PressureConv) {
while (PIOS_MS5611_WriteCommand(MS5611_PRES_ADDR + oversampling) != 0) {
continue;
}
@ -114,7 +162,7 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
/**
* @brief Return the delay for the current osr
*/
int32_t PIOS_MS5611_GetDelay()
static uint32_t PIOS_MS5611_GetDelay()
{
switch (oversampling) {
case MS5611_OSR_256:
@ -141,7 +189,7 @@ int32_t PIOS_MS5611_GetDelay()
/**
* @brief Return the delay for the current osr in uS
*/
uint32_t PIOS_MS5611_GetDelayUs()
static uint32_t PIOS_MS5611_GetDelayUs()
{
switch (oversampling) {
case MS5611_OSR_256:
@ -167,7 +215,7 @@ uint32_t PIOS_MS5611_GetDelayUs()
/**
* Read the ADC conversion value (once ADC conversion has completed)
* \return 0 if successfully read the ADC, -1 if failed
* \return 0 if successfully read the ADC, -1 if conversion time has not elapsed, -2 if failure occurred
*/
int32_t PIOS_MS5611_ReadADC(void)
{
@ -177,16 +225,19 @@ int32_t PIOS_MS5611_ReadADC(void)
Data[1] = 0;
Data[2] = 0;
while (PIOS_MS5611_GetDelayUs() > PIOS_DELAY_DiffuS(lastConversionStart)) {
vTaskDelay(0);
if (CurrentRead == MS5611_CONVERSION_TYPE_None) {
return -2;
}
if (conversionDelayUs > PIOS_DELAY_DiffuS(lastConversionStart)) {
return -1;
}
static int64_t deltaTemp;
/* Read and store the 16bit result */
if (CurrentRead == TemperatureConv) {
if (CurrentRead == MS5611_CONVERSION_TYPE_TemperatureConv) {
/* Read the temperature conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
return -2;
}
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
@ -205,7 +256,7 @@ int32_t PIOS_MS5611_ReadADC(void)
/* Read the pressure conversion */
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
return -1;
return -2;
}
// check if temperature is less than 20°C
if (Temperature < 2000) {
@ -248,18 +299,18 @@ int32_t PIOS_MS5611_ReadADC(void)
/**
* Return the most recently computed temperature in kPa
*/
float PIOS_MS5611_GetTemperature(void)
static float PIOS_MS5611_GetTemperature(void)
{
// Apply the second order low and very low temperature compensation offset
return ((float)(Temperature - compensation_t2)) / 100.0f;
}
/**
* Return the most recently computed pressure in kPa
* Return the most recently computed pressure in Pa
*/
float PIOS_MS5611_GetPressure(void)
static float PIOS_MS5611_GetPressure(void)
{
return ((float)Pressure) / 1000.0f;
return (float)Pressure;
}
/**
@ -270,7 +321,7 @@ float PIOS_MS5611_GetPressure(void)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
const struct pios_i2c_txn txn_list[] = {
{
@ -300,7 +351,7 @@ int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len)
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_MS5611_WriteCommand(uint8_t command)
static int32_t PIOS_MS5611_WriteCommand(uint8_t command)
{
const struct pios_i2c_txn txn_list[] = {
{
@ -326,16 +377,16 @@ int32_t PIOS_MS5611_Test()
int32_t cur_value = 0;
cur_value = Temperature;
PIOS_MS5611_StartADC(TemperatureConv);
PIOS_DELAY_WaitmS(5);
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC();
if (cur_value == Temperature) {
return -1;
}
cur_value = Pressure;
PIOS_MS5611_StartADC(PressureConv);
PIOS_DELAY_WaitmS(26);
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
PIOS_DELAY_WaitmS(10);
PIOS_MS5611_ReadADC();
if (cur_value == Pressure) {
return -1;
@ -344,6 +395,80 @@ int32_t PIOS_MS5611_Test()
return 0;
}
/* PIOS sensor driver implementation */
void PIOS_MS5611_Register()
{
PIOS_SENSORS_Register(&PIOS_MS5611_Driver, PIOS_SENSORS_TYPE_1AXIS_BARO, 0);
}
bool PIOS_MS5611_driver_Test(__attribute__((unused)) uintptr_t context)
{
return true; // !PIOS_MS5611_Test();
}
void PIOS_MS5611_driver_Reset(__attribute__((unused)) uintptr_t context) {}
void PIOS_MS5611_driver_get_scale(float *scales, uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(size > 0);
scales[0] = 1;
}
void PIOS_MS5611_driver_fetch(void *data, __attribute__((unused)) uint8_t size, __attribute__((unused)) uintptr_t context)
{
PIOS_Assert(data);
memcpy(data, (void *)&results, sizeof(PIOS_SENSORS_1Axis_SensorsWithTemp));
}
bool PIOS_MS5611_driver_poll(__attribute__((unused)) uintptr_t context)
{
static uint8_t temp_press_interleave_count = 1;
static MS5611_FSM_State next_state = MS5611_FSM_INIT;
int32_t conversionResult = PIOS_MS5611_ReadADC();
if (__builtin_expect(conversionResult == -1, 1)) {
return false; // wait for conversion to complete
} else if (__builtin_expect(conversionResult == -2, 0)) {
temp_press_interleave_count = 1;
next_state = MS5611_FSM_INIT;
}
switch (next_state) {
case MS5611_FSM_INIT:
case MS5611_FSM_TEMPERATURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE;
return false;
case MS5611_FSM_PRESSURE:
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE;
return false;
case MS5611_FSM_CALCULATE:
temp_press_interleave_count--;
if (!temp_press_interleave_count) {
temp_press_interleave_count = PIOS_MS5611_SLOW_TEMP_RATE;
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_TemperatureConv);
next_state = MS5611_FSM_PRESSURE;
} else {
PIOS_MS5611_StartADC(MS5611_CONVERSION_TYPE_PressureConv);
next_state = MS5611_FSM_CALCULATE;
}
results.temperature = PIOS_MS5611_GetTemperature();
results.sample = PIOS_MS5611_GetPressure();
return true;
default:
// it should not be there
PIOS_Assert(0);
}
return false;
}
#endif /* PIOS_INCLUDE_MS5611 */
/**

65
flight/pios/common/pios_sensors.c Normal file
View File

@ -0,0 +1,65 @@
/**
******************************************************************************
*
* @file pios_sensors.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PiOS sensors handling
* --
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <pios_mem.h>
#include <pios_sensors.h>
#include <string.h>
// private variables
static PIOS_SENSORS_Instance *sensor_list = 0;
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context)
{
PIOS_SENSORS_Instance *instance = (PIOS_SENSORS_Instance *)pios_malloc(sizeof(PIOS_SENSORS_Instance));
instance->driver = driver;
instance->type = type;
instance->context = context;
instance->next = NULL;
LL_APPEND(sensor_list, instance);
return instance;
}
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList()
{
return sensor_list;
}
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type)
{
if (!previous_instance) {
previous_instance = sensor_list;
}
PIOS_SENSORS_Instance *sensor;
LL_FOREACH((PIOS_SENSORS_Instance *)previous_instance, sensor) {
if (sensor->type && type) {
return sensor;
}
}
return NULL;
}

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

@ -31,6 +31,7 @@
#ifndef PIOS_HMC5x83_H
#define PIOS_HMC5x83_H
#include <stdint.h>
#include <pios_sensors.h>
/* HMC5x83 Addresses */
#define PIOS_HMC5x83_I2C_ADDR 0x1E
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
@ -123,12 +124,16 @@ struct pios_hmc5x83_cfg {
/* Public Functions */
extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
extern void PIOS_HMC5x83_Register(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
extern const PIOS_SENSORS_Driver PIOS_HMC5x83_Driver;
#endif /* PIOS_HMC5x83_H */
/**

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

@ -53,9 +53,11 @@ inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_handle, in
vPortEnterCritical();
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = newValue;
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
@ -88,9 +90,11 @@ inline void PIOS_Instrumentation_TimeEnd(pios_counter_t counter_handle)
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
@ -110,9 +114,11 @@ inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handle)
vPortEnterCritical();
uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
counter->value = (counter->value * 15 + period) / 16;
counter->max--;
if ((int32_t)period > counter->max) {
counter->max = period;
}
counter->min++;
if ((int32_t)period < counter->min) {
counter->min = period;
}

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

@ -72,6 +72,9 @@
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
// helper macros for LPFs
#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
// Useful math macros
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))

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

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

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

@ -31,6 +31,7 @@
#ifndef PIOS_MPU6000_H
#define PIOS_MPU6000_H
#include <pios_sensors.h>
/* MPU6000 Addresses */
#define PIOS_MPU6000_SMPLRT_DIV_REG 0X19
@ -131,18 +132,6 @@ enum pios_mpu6000_orientation { // clockwise rotation from board forward
PIOS_MPU6000_TOP_270DEG = 0x03
};
struct pios_mpu6000_data {
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
#if defined(PIOS_MPU6000_ACCEL)
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
#endif /* PIOS_MPU6000_ACCEL */
int16_t temperature;
};
struct pios_mpu6000_cfg {
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
@ -167,14 +156,11 @@ struct pios_mpu6000_cfg {
/* Public Functions */
extern int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *new_cfg);
extern int32_t PIOS_MPU6000_ConfigureRanges(enum pios_mpu6000_range gyroRange, enum pios_mpu6000_accel_range accelRange, enum pios_mpu6000_filter filterSetting);
extern xQueueHandle PIOS_MPU6000_GetQueue();
extern int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *buffer);
extern int32_t PIOS_MPU6000_ReadID();
extern int32_t PIOS_MPU6000_Test();
extern float PIOS_MPU6000_GetScale();
extern float PIOS_MPU6000_GetAccelScale();
extern void PIOS_MPU6000_Register();
extern bool PIOS_MPU6000_IRQHandler(void);
extern const PIOS_SENSORS_Driver PIOS_MPU6000_Driver;
#endif /* PIOS_MPU6000_H */
/**

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

@ -30,7 +30,7 @@
#ifndef PIOS_MS5611_H
#define PIOS_MS5611_H
#include <pios_sensors.h>
/* BMP085 Addresses */
#define MS5611_I2C_ADDR 0x77
#define MS5611_RESET 0x1E
@ -40,17 +40,6 @@
#define MS5611_PRES_ADDR 0x40
#define MS5611_TEMP_ADDR 0x50
#define MS5611_ADC_MSB 0xF6
#define MS5611_P0 101.3250f
/* Local Types */
typedef struct {
uint16_t C[6];
} MS5611CalibDataTypeDef;
typedef enum {
PressureConv,
TemperatureConv
} ConversionTypeTypeDef;
struct pios_ms5611_cfg {
uint32_t oversampling;
@ -66,12 +55,8 @@ enum pios_ms5611_osr {
/* Public Functions */
extern void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device);
extern int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type);
extern int32_t PIOS_MS5611_ReadADC(void);
extern float PIOS_MS5611_GetTemperature(void);
extern float PIOS_MS5611_GetPressure(void);
extern int32_t PIOS_MS5611_Test();
extern int32_t PIOS_MS5611_GetDelay();
extern const PIOS_SENSORS_Driver PIOS_MS5611_Driver;
void PIOS_MS5611_Register();
#endif /* PIOS_MS5611_H */

190
flight/pios/inc/pios_sensors.h Normal file
View File

@ -0,0 +1,190 @@
/**
******************************************************************************
*
* @file pios_sensors.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief PiOS sensors handling
* --
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_SENSORS_H
#define PIOS_SENSORS_H
#include <pios.h>
#include <utlist.h>
#include <stdint.h>
#include <vectors.h>
// needed for debug APIs.
typedef bool (*PIOS_SENSORS_test_function)(uintptr_t context);
typedef void (*PIOS_SENSORS_reset_function)(uintptr_t context);
typedef bool (*PIOS_SENSORS_poll_function)(uintptr_t context);
typedef void (*PIOS_SENSORS_fetch_function)(void *samples, uint8_t size, uintptr_t context);
/**
* return an array with current scale for the instance.
* Instances with multiples sensors returns several value in the same
* order as they appear in PIOS_SENSORS_TYPE enums.
*/
typedef void (*PIOS_SENSORS_get_scale_function)(float *, uint8_t size, uintptr_t context);
typedef QueueHandle_t (*PIOS_SENSORS_get_queue_function)(uintptr_t context);
typedef struct PIOS_SENSORS_Driver {
PIOS_SENSORS_test_function test; // called at startup to test the sensor
PIOS_SENSORS_poll_function poll; // called to check whether data are available for polled sensors
PIOS_SENSORS_fetch_function fetch; // called to fetch data for polled sensors
PIOS_SENSORS_reset_function reset; // reset sensor. for example if data are not received in the allotted time
PIOS_SENSORS_get_queue_function get_queue; // get the queue reference
PIOS_SENSORS_get_scale_function get_scale; // return scales for the sensors
bool is_polled;
} PIOS_SENSORS_Driver;
typedef enum PIOS_SENSORS_TYPE {
PIOS_SENSORS_TYPE_3AXIS_ACCEL = 0x01,
PIOS_SENSORS_TYPE_3AXIS_GYRO = 0x02,
PIOS_SENSORS_TYPE_3AXIS_GYRO_ACCEL = 0x03,
PIOS_SENSORS_TYPE_3AXIS_MAG = 0x04,
PIOS_SENSORS_TYPE_1AXIS_BARO = 0x08,
} PIOS_SENSORS_TYPE;
#define PIOS_SENSORS_TYPE_1D (PIOS_SENSORS_TYPE_1AXIS_BARO)
#define PIOS_SENSORS_TYPE_3D (PIOS_SENSORS_TYPE_3AXIS_ACCEL | PIOS_SENSORS_TYPE_3AXIS_GYRO | PIOS_SENSORS_TYPE_3AXIS_MAG)
typedef struct PIOS_SENSORS_Instance {
const PIOS_SENSORS_Driver *driver;
uintptr_t context;
struct PIOS_SENSORS_Instance *next;
uint8_t type;
} PIOS_SENSORS_Instance;
/**
* A 3d Accel sample with temperature
*/
typedef struct PIOS_SENSORS_3Axis_SensorsWithTemp {
uint16_t count; // number of sensor instances
int16_t temperature; // Degrees Celsius * 100
Vector3i16 sample[];
} PIOS_SENSORS_3Axis_SensorsWithTemp;
typedef struct PIOS_SENSORS_1Axis_SensorsWithTemp {
float temperature; // Degrees Celsius
float sample; // sample
} PIOS_SENSORS_1Axis_SensorsWithTemp;
/**
* Register a new sensor instance with sensor subsystem
* @param driver sensor driver
* @param type sensor type @ref PIOS_SENSORS_TYPE
* @param context context to be passed to sensor driver
* @return the new sensor instance
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_Register(const PIOS_SENSORS_Driver *driver, PIOS_SENSORS_TYPE type, uintptr_t context);
/**
* return the list of registered sensors.
* @return the first sensor instance in the list.
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
/**
* Perform sensor test and return true if passed
* @param sensor instance to test
* @return true if test passes
*/
static inline bool PIOS_SENSORS_Test(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->test) {
return true;
} else {
return sensor->driver->test(sensor->context);
}
}
/**
* Poll sensor for new values
* @param sensor instance to poll
* @return true if sensor has samples available
*/
static inline bool PIOS_SENSORS_Poll(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->poll) {
return true;
} else {
return sensor->driver->poll(sensor->context);
}
}
/**
*
* @param sensor
* @param samples
* @param size
*/
static inline void PIOS_SENSOR_Fetch(const PIOS_SENSORS_Instance *sensor, void *samples, uint8_t size)
{
PIOS_Assert(sensor);
sensor->driver->fetch(samples, size, sensor->context);
}
static inline void PIOS_SENSOR_Reset(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
sensor->driver->reset(sensor->context);
}
/**
* retrieve the sensor queue
* @param sensor
* @return sensor queue or null if not supported
*/
static inline QueueHandle_t PIOS_SENSORS_GetQueue(const PIOS_SENSORS_Instance *sensor)
{
PIOS_Assert(sensor);
if (!sensor->driver->get_queue) {
return NULL;
}
return sensor->driver->get_queue(sensor->context);
}
/**
* Get the sensor scales.
* @param sensor sensor instance
* @param scales float array that will contains scales
* @param size number of floats within the array
*/
static inline void PIOS_SENSORS_GetScales(const PIOS_SENSORS_Instance *sensor, float *scales, uint8_t size)
{
PIOS_Assert(sensor);
sensor->driver->get_scale(scales, size, sensor->context);
}
/**
* return head of sensor list
* @return head of sensor list
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetList();
/**
* Return the first occurrence of specified sensor type
* @param previous_instance last instance found or 0
* @param type type of sensor to find
* @return the first occurence found or NULL
*/
PIOS_SENSORS_Instance *PIOS_SENSORS_GetInstanceByType(const PIOS_SENSORS_Instance *previous_instance, PIOS_SENSORS_TYPE type);
#endif /* PIOS_SENSORS_H */

43
flight/pios/pios.h
View File

@ -186,49 +186,6 @@ extern "C" {
#endif
#endif
/* PIOS sensor interfaces */
#ifdef PIOS_INCLUDE_ADXL345
/* ADXL345 3-Axis Accelerometer */
#include <pios_adxl345.h>
#endif
#ifdef PIOS_INCLUDE_BMA180
/* BMA180 3-Axis Accelerometer */
#include <pios_bma180.h>
#endif
#ifdef PIOS_INCLUDE_L3GD20
/* L3GD20 3-Axis Gyro */
#include <pios_l3gd20.h>
#endif
#ifdef PIOS_INCLUDE_MPU6000
/* MPU6000 3-Axis Gyro/Accelerometer */
/* #define PIOS_MPU6000_ACCEL */
#include <pios_mpu6000.h>
#endif
#ifdef PIOS_INCLUDE_HMC5843
/* HMC5843 3-Axis Digital Compass */
#include <pios_hmc5843.h>
#endif
#ifdef PIOS_INCLUDE_HMC5X83
/* HMC5883/HMC5983 3-Axis Digital Compass */
/* #define PIOS_HMC5x83_HAS_GPIOS */
#include <pios_hmc5x83.h>
#endif
#ifdef PIOS_INCLUDE_BMP085
/* BMP085 Barometric Pressure Sensor */
#include <pios_bmp085.h>
#endif
#ifdef PIOS_INCLUDE_MS5611
/* MS5611 Barometric Pressure Sensor */
#include <pios_ms5611.h>
#endif
#ifdef PIOS_INCLUDE_MPXV
/* MPXV5004, MPXV7002 based Airspeed Sensor */
#include <pios_mpxv.h>

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

@ -38,6 +38,10 @@
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
#endif
#if defined(PIOS_INCLUDE_ADXL345)
#include <pios_adxl345.h>
#endif
/*
* Pull in the board-specific static HW definitions.
* Including .c files is a bit ugly but this allows all of
@ -844,7 +848,7 @@ void PIOS_Board_Init(void)
}
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
init_test = PIOS_MPU6000_Test();
init_test = !PIOS_MPU6000_Driver.test(0);
#endif /* PIOS_INCLUDE_MPU6000 */
break;

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

@ -205,8 +205,9 @@ void PIOS_SPI_mag_flash_irq_handler(void)
#endif /* PIOS_INCLUDE_FLASH */
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t onboard_mag;
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag;
#ifdef PIOS_HMC5X83_HAS_GPIOS
bool pios_board_mag_handler()
{

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

@ -31,7 +31,7 @@ MODULES += Osd/osdgen
MODULES += Osd/osdinput
MODULES += Osd/WavPlayer
MODULES += GPS
MODULES += Extensions/MagBaro
#MODULES += Extensions/MagBaro
MODULES += FirmwareIAP
MODULES += Telemetry

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

@ -32,11 +32,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += StateEstimation
MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += ManualControl
MODULES += Receiver

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

@ -206,7 +206,7 @@ static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 16,
.max_downsample = 20,
};
#endif /* PIOS_INCLUDE_MPU6000 */
@ -922,20 +922,23 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
PIOS_MPU6000_Register();
#endif
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif
#if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
PIOS_MS5611_Register();
#endif
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
#endif
#ifdef PIOS_INCLUDE_WS2811
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h>
HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);

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

@ -29,11 +29,8 @@ USE_DSP_LIB ?= NO
# List of mandatory modules to include
MODULES += Sensors
#MODULES += Attitude/revolution
MODULES += StateEstimation # use instead of Attitude
MODULES += Altitude/revolution
MODULES += StateEstimation
MODULES += Airspeed
#MODULES += AltitudeHold # now integrated in Stabilization
MODULES += Stabilization
MODULES += ManualControl
MODULES += Receiver

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

@ -82,13 +82,14 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line5,
@ -893,19 +894,23 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_adapter_id, 0);
PIOS_HMC5x83_Register(onboard_mag);
#endif
#if defined(PIOS_INCLUDE_MS5611)
PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id);
PIOS_MS5611_Register();
#endif
switch (bdinfo->board_rev) {
case 0x01:
#if defined(PIOS_INCLUDE_L3GD20)
PIOS_Assert(0); // L3gd20 has not been ported to Sensor framework!!!
PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg);
PIOS_Assert(PIOS_L3GD20_Test() == 0);
#endif
#if defined(PIOS_INCLUDE_BMA180)
PIOS_Assert(0); // BMA180 has not been ported to Sensor framework!!!
PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg);
PIOS_Assert(PIOS_BMA180_Test() == 0);
#endif
@ -914,6 +919,7 @@ void PIOS_Board_Init(void)
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
PIOS_MPU6000_CONFIG_Configure();
PIOS_MPU6000_Register();
#endif
break;
default:

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

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

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

@ -80,6 +80,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
ui = new Ui_InputWidget();
ui->setupUi(this);
wizardUi = new Ui_InputWizardWidget();
wizardUi->setupUi(ui->wizard);
addApplySaveButtons(ui->saveRCInputToRAM, ui->saveRCInputToSD);
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->runCalibration, SIGNAL(toggled(bool)), this, SLOT(simpleCalibration(bool)));
connect(ui->wzNext, SIGNAL(clicked()), this, SLOT(wzNext()));
connect(ui->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel()));
connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack()));
connect(wizardUi->wzNext, SIGNAL(clicked()), this, SLOT(wzNext()));
connect(wizardUi->wzCancel, SIGNAL(clicked()), this, SLOT(wzCancel()));
connect(wizardUi->wzBack, SIGNAL(clicked()), this, SLOT(wzBack()));
ui->stackedWidget->setCurrentIndex(0);
addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true);
@ -207,11 +210,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) :
// Connect the help button
connect(ui->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
ui->graphicsView->setScene(new QGraphicsScene(this));
ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
wizardUi->graphicsView->setScene(new QGraphicsScene(this));
wizardUi->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
m_renderer = new QSvgRenderer();
QGraphicsScene *l_scene = ui->graphicsView->scene();
ui->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor()));
QGraphicsScene *l_scene = wizardUi->graphicsView->scene();
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()) {
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_txAccess2->setTransform(m_txAccess2Orig, true);
}
ui->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio);
wizardUi->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio);
animate = new QTimer(this);
connect(animate, SIGNAL(timeout()), this, SLOT(moveTxControls()));
@ -377,7 +380,7 @@ void ConfigInputWidget::resizeEvent(QResizeEvent *event)
{
QWidget::resizeEvent(event);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
}
void ConfigInputWidget::openHelp()
@ -426,7 +429,7 @@ void ConfigInputWidget::goToWizard()
// start the wizard
wizardSetUpStep(wizardWelcome);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
}
void ConfigInputWidget::disableWizardButton(int value)
@ -600,8 +603,7 @@ void ConfigInputWidget::wzBack()
void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
{
ui->wzText2->clear();
ui->wzNext->setText(tr("Next"));
wizardUi->wzNext->setText(tr("Next"));
switch (step) {
case wizardWelcome:
@ -611,43 +613,36 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
}
}
extraWidgets.clear();
ui->graphicsView->setVisible(false);
wizardUi->graphicsView->setVisible(false);
setTxMovement(nothing);
ui->wzText->setText(tr("Welcome to the inputs configuration wizard.\n\n"
"Please follow the instructions on the screen and only move your controls when asked to.\n"
"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"));
wizardUi->wzBack->setEnabled(false);
wizardUi->pagesStack->setCurrentWidget(wizardUi->welcomePage);
ui->stackedWidget->setCurrentIndex(1);
ui->wzBack->setEnabled(false);
break;
case wizardChooseType:
{
ui->graphicsView->setVisible(true);
ui->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
wizardUi->graphicsView->setVisible(true);
wizardUi->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio);
setTxMovement(nothing);
ui->wzText->setText(tr("Please choose your transmitter type:"));
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);
wizardUi->wzBack->setEnabled(true);
if (transmitterType == heli) {
typeHeli->setChecked(true);
wizardUi->typeHeli->setChecked(true);
} else {
typeAcro->setChecked(true);
wizardUi->typeAcro->setChecked(true);
}
ui->wzText2->setText(tr("If selecting the Helicopter option, please engage throttle hold now."));
extraWidgets.clear();
extraWidgets.append(typeAcro);
extraWidgets.append(typeHeli);
ui->radioButtonsLayout->layout()->addWidget(typeAcro);
ui->radioButtonsLayout->layout()->addWidget(typeHeli);
wizardUi->pagesStack->setCurrentWidget(wizardUi->chooseTypePage);
}
break;
case wizardChooseMode:
{
ui->wzBack->setEnabled(true);
extraWidgets.clear();
ui->wzText->setText(tr("Please choose your transmitter mode:"));
wizardUi->wzBack->setEnabled(true);
QRadioButton* modeButtons[] = {
wizardUi->mode1Button,
wizardUi->mode2Button,
wizardUi->mode3Button,
wizardUi->mode4Button
};
for (int i = 0; i <= mode4; ++i) {
QString label;
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;
default: Q_ASSERT(0); break;
}
wizardUi->typePageFooter->setText(" ");
} else {
switch (mode) {
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;
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) {
modeButton->setChecked(true);
modeButtons[i]->setChecked(true);
}
extraWidgets.append(modeButton);
ui->radioButtonsLayout->layout()->addWidget(modeButton);
}
wizardUi->pagesStack->setCurrentWidget(wizardUi->chooseModePage);
}
break;
case wizardIdentifySticks:
@ -684,17 +679,16 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
nextChannel();
manualSettingsData = manualSettingsObj->getData();
connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls()));
ui->wzNext->setEnabled(false);
wizardUi->wzNext->setEnabled(false);
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifySticksPage);
break;
case wizardIdentifyCenter:
setTxMovement(centerAll);
ui->wzText->setText(QString(tr("Please center all controls and trims and press Next when ready.\n\n"
"If your FlightMode switch has only two positions, leave it in either position.")));
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyCenterPage);
break;
case wizardIdentifyLimits:
{
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();
for (uint i = 0; i < ManualControlSettings::CHANNELMAX_NUMELEM; ++i) {
// Preserve the inverted status
@ -711,6 +705,8 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(flightStatusObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(accessoryDesiredObj0, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
wizardUi->pagesStack->setCurrentWidget(wizardUi->identifyLimitsPage);
}
break;
case wizardIdentifyInverted:
@ -724,22 +720,20 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
QCheckBox *cb = new QCheckBox(name, this);
// Make sure checked status matches current one
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);
connect(cb, SIGNAL(toggled(bool)), this, SLOT(invertControls()));
}
}
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;
case wizardFinish:
dimOtherControls(false);
connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveSticks()));
connect(flightStatusObj, 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"
"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.")));
wizardUi->pagesStack->setCurrentWidget(wizardUi->finishPage);
break;
default:
Q_ASSERT(0);
@ -749,36 +743,35 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step)
void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step)
{
QRadioButton *mode, *type;
Q_ASSERT(step == wizardStep);
switch (step) {
case wizardWelcome:
break;
case wizardChooseType:
type = qobject_cast<QRadioButton *>(extraWidgets.at(0));
if (type->isChecked()) {
if (wizardUi->typeAcro->isChecked()) {
transmitterType = acro;
} else {
transmitterType = heli;
}
delete extraWidgets.at(0);
delete extraWidgets.at(1);
extraWidgets.clear();
break;
case wizardChooseMode:
for (int i = mode1; i <= mode4; ++i) {
mode = qobject_cast<QRadioButton *>(extraWidgets.first());
if (mode->isChecked()) {
transmitterMode = static_cast<txMode>(i);
{
QRadioButton* modeButtons[] = {
wizardUi->mode1Button,
wizardUi->mode2Button,
wizardUi->mode3Button,
wizardUi->mode4Button
};
for (int i = mode1; i <= mode4; ++i) {
if (modeButtons[i]->isChecked()) {
transmitterMode = static_cast<txMode>(i);
}
}
delete mode;
extraWidgets.removeFirst();
}
break;
case wizardIdentifySticks:
disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(identifyControls()));
ui->wzNext->setEnabled(true);
wizardUi->wzNext->setEnabled(true);
setTxMovement(nothing);
break;
case wizardIdentifyCenter:
@ -861,22 +854,22 @@ void ConfigInputWidget::restoreMdata()
void ConfigInputWidget::setChannel(int newChan)
{
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) {
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)) {
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 {
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)));
}
if (manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) {
ui->wzNext->setEnabled(true);
ui->wzNext->setText(tr("Next / Skip"));
ui->wzText->setText(ui->wzText->text() + tr(" Alternatively, click Next to skip this channel."));
wizardUi->wzNext->setEnabled(true);
wizardUi->wzNext->setText(tr("Next / Skip"));
wizardUi->identifyStickInstructions->setText(wizardUi->identifyStickInstructions->text() + tr(" Alternatively, click Next to skip this channel."));
} else {
ui->wzNext->setEnabled(false);
wizardUi->wzNext->setEnabled(false);
}
setMoveFromCommand(newChan);

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

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

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

@ -275,80 +275,6 @@
</layout>
</widget>
<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>
</item>
@ -2551,10 +2477,6 @@ Applies and Saves all settings to SD</string>
<tabstops>
<tabstop>tabWidget</tabstop>
<tabstop>deadband</tabstop>
<tabstop>graphicsView</tabstop>
<tabstop>wzBack</tabstop>
<tabstop>wzNext</tabstop>
<tabstop>wzCancel</tabstop>
<tabstop>armControl</tabstop>
<tabstop>armTimeout</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_sbus.c
SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_sensors.c
## Misc library functions
SRC += $(FLIGHTLIB)/sanitycheck.c

7
shared/uavobjectdefinition/magsensor.xml
View File

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