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

Merge branch 'amorale/OP-1535_mpu_overhaul' into next

Browse Source
This commit is contained in:
Alessio Morale 2014-10-16 00:51:02 +02:00
commit a9f35e2afe
21 changed files with 537 additions and 287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
flight/libraries/math/mathmisc.h
View File

@ -119,6 +119,30 @@ static inline float y_on_curve(float x, const pointf points[], int num_points)
// Find the y value on the selected line. // Find the y value on the selected line.
return y_on_line(x, &points[end_point - 1], &points[end_point]); return y_on_line(x, &points[end_point - 1], &points[end_point]);
} }
// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c"
// http://en.wikipedia.org/wiki/Fast_inverse_square_root
static inline float fast_invsqrtf(float number)
{
float x2, y;
const float threehalfs = 1.5F;
union {
float f;
uint32_t u;
} i;
x2 = number * 0.5F;
y = number;
i.f = y; // evil floating point bit level hacking
i.u = 0x5f3759df - (i.u >> 1); // what the fxck?
y = i.f;
y = y * (threehalfs - (x2 * y * y)); // 1st iteration
// y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
return y;
}
/** /**
* Ultrafast pow() aproximation needed for expo * Ultrafast pow() aproximation needed for expo

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

@ -41,6 +41,7 @@
#include "altitude.h" #include "altitude.h"
#include "barosensor.h" // object that will be updated by the module #include "barosensor.h" // object that will be updated by the module
#include "revosettings.h" #include "revosettings.h"
#include <mathmisc.h>
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module #include "sonaraltitude.h" // object that will be updated by the module
#endif #endif
@ -50,6 +51,14 @@
#define STACK_SIZE_BYTES 550 #define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #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 types
// Private variables // Private variables
@ -57,6 +66,11 @@ static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent; static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled; static volatile bool tempCorrectionEnabled;
static float baro_temp_bias = 0;
static float baro_temperature = NAN;
static uint8_t temp_calibration_count = 0;
// Private functions // Private functions
static void altitudeTask(void *parameters); static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -166,13 +180,22 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature(); temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure(); press = PIOS_MS5611_GetPressure();
if (tempCorrectionEnabled) { 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 // 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 // in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max : float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp); baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
press -= 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))); float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) { if (!isnan(altitude)) {

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

@ -64,8 +64,8 @@
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_notify.h> #include <pios_notify.h>
#include <mathmisc.h>
#include <pios_constants.h>
#include <pios_instrumentation_helper.h> #include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterUpd); PERF_DEFINE_COUNTER(counterUpd);
@ -82,10 +82,19 @@ PERF_DEFINE_COUNTER(counterAtt);
#define STACK_SIZE_BYTES 540 #define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 4 // Attitude module loop interval (defined by sensor rate in pios_config.h)
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
#define UPDATE_RATE 25.0f #define UPDATE_RATE 25.0f
#define UPDATE_EXPECTED (1.0f / 500.0f) // Interval in number of sample to recalculate temp bias
#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 UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f #define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f #define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f #define UPDATE_ALPHA 1.0e-2f
@ -130,6 +139,10 @@ static bool apply_accel_temp = false;
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;; static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff; static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent; static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
static float temperature = NAN;
static float accel_temp_bias[3] = { 0 };
static float gyro_temp_bias[3] = { 0 };
static uint8_t temp_calibration_count = 0;
// Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings // Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
static AccelGyroSettingsgyro_scaleData gyro_scale; static AccelGyroSettingsgyro_scaleData gyro_scale;
@ -142,8 +155,7 @@ static volatile int32_t trim_accels[3];
static volatile int32_t trim_samples; static volatile int32_t trim_samples;
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535; int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
#define GRAV 9.81f #define STD_CC_ACCEL_SCALE (PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f)
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
/* 0.004f is gravity / LSB */ /* 0.004f is gravity / LSB */
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
#define STD_CC_ANALOG_GYRO_GAIN 0.42f #define STD_CC_ANALOG_GYRO_GAIN 0.42f
@ -222,12 +234,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
bool cc3d = BOARDISCC3D; bool cc3d = BOARDISCC3D;
if (cc3d) { if (cc3d) {
@ -236,6 +242,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
#endif #endif
} else { } else {
#if defined(PIOS_INCLUDE_ADXL345) #if defined(PIOS_INCLUDE_ADXL345)
// Set critical error and wait until the accel is producing data
while (PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
accel_test = PIOS_ADXL345_Test(); accel_test = PIOS_ADXL345_Test();
#endif #endif
@ -257,7 +268,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
settingsUpdatedCb(AttitudeSettingsHandle()); settingsUpdatedCb(AttitudeSettingsHandle());
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
portTickType lastSysTime = xTaskGetTickCount();
// Main task loop // Main task loop
while (1) { while (1) {
FlightStatusData flightStatus; FlightStatusData flightStatus;
@ -317,6 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PERF_MEASURE_PERIOD(counterPeriod); PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
} }
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS);
} }
} }
@ -454,7 +466,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
#if defined(PIOS_INCLUDE_MPU6000) #if defined(PIOS_INCLUDE_MPU6000)
xQueueHandle queue = PIOS_MPU6000_GetQueue(); xQueueHandle queue = PIOS_MPU6000_GetQueue();
BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD); BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
while (ret == pdTRUE) { while (ret == pdTRUE) {
gyros[0] += mpu6000_data.gyro_x; gyros[0] += mpu6000_data.gyro_x;
gyros[1] += mpu6000_data.gyro_y; gyros[1] += mpu6000_data.gyro_y;
@ -470,6 +482,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
// check if further samples are already in queue // check if further samples are already in queue
ret = xQueueReceive(queue, (void *)&mpu6000_data, 0); ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
} }
PERF_TRACK_VALUE(counterAccelSamples, count);
if (!count) { if (!count) {
return -1; // Error, no data return -1; // Error, no data
@ -488,21 +501,39 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
accels[1] *= accel_scale.Y * invcount; accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount; accels[2] *= accel_scale.Z * invcount;
temp *= invcount; temp *= invcount;
float ctemp = temp > temp_calibrated_extent.max ? temp_calibrated_extent.max :
(temp < temp_calibrated_extent.min ? temp_calibrated_extent.min
: temp);
if (isnan(temperature)) {
temperature = temp;
}
temperature = temp_alpha * (temp - temperature) + temperature;
if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;
float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min);
if (apply_gyro_temp) { if (apply_gyro_temp) {
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp; gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp; gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp; gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
} }
if (apply_accel_temp) { if (apply_accel_temp) {
accels[0] -= accel_temp_coeff.X * ctemp; accel_temp_bias[0] = accel_temp_coeff.X * ctemp;
accels[1] -= accel_temp_coeff.Y * ctemp; accel_temp_bias[1] = accel_temp_coeff.Y * ctemp;
accels[2] -= accel_temp_coeff.Z * ctemp; accel_temp_bias[2] = accel_temp_coeff.Z * ctemp;
}
}
temp_calibration_count--;
if (apply_gyro_temp) {
gyros[0] -= gyro_temp_bias[0];
gyros[1] -= gyro_temp_bias[1];
gyros[2] -= gyro_temp_bias[2];
}
if (apply_accel_temp) {
accels[0] -= accel_temp_bias[0];
accels[1] -= accel_temp_bias[1];
accels[2] -= accel_temp_bias[2];
} }
// gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; // gyrosData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
// accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f; // accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
@ -587,24 +618,24 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude // Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]); float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
if (accel_mag < 1.0e-3f) { if (inv_accel_mag > 1e3f) {
return; return;
} }
// Account for filtered gravity vector magnitude // Account for filtered gravity vector magnitude
float grot_mag; float inv_grot_mag;
if (accel_filter_enabled) { if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else { } else {
grot_mag = 1.0f; inv_grot_mag = 1.0f;
} }
if (grot_mag < 1.0e-3f) { if (inv_grot_mag > 1e3f) {
return; return;
} }
const float invMag = 1.0f / (accel_mag * grot_mag); const float invMag = (inv_accel_mag * inv_grot_mag);
accel_err[0] *= invMag; accel_err[0] *= invMag;
accel_err[1] *= invMag; accel_err[1] *= invMag;
accel_err[2] *= invMag; accel_err[2] *= invMag;
@ -645,21 +676,20 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
} }
// Renomalize // Renomalize
float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
// If quaternion has become inappropriately short or is nan reinit. // If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN // THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) { if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
q[0] = 1; q[0] = 1;
q[1] = 0; q[1] = 0;
q[2] = 0; q[2] = 0;
q[3] = 0; q[3] = 0;
} else { } else {
const float invQmag = 1.0f / qmag; q[0] = q[0] * inv_qmag;
q[0] = q[0] * invQmag; q[1] = q[1] * inv_qmag;
q[1] = q[1] * invQmag; q[2] = q[2] * inv_qmag;
q[2] = q[2] * invQmag; q[3] = q[3] * inv_qmag;
q[3] = q[3] * invQmag;
} }
AttitudeStateData attitudeState; AttitudeStateData attitudeState;
@ -777,7 +807,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples; accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples; accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
// Z should average -grav // Z should average -grav
accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV; accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + PIOS_CONST_MKS_GRAV_ACCEL_F;
attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL; attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
AttitudeSettingsSet(&attitudeSettings); AttitudeSettingsSet(&attitudeSettings);
} else { } else {

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

@ -58,7 +58,7 @@
#include <accelgyrosettings.h> #include <accelgyrosettings.h>
#include <flightstatus.h> #include <flightstatus.h>
#include <taskinfo.h> #include <taskinfo.h>
#include <pios_math.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_board_info.h> #include <pios_board_info.h>
@ -66,11 +66,28 @@
// Private constants // Private constants
#define STACK_SIZE_BYTES 1000 #define STACK_SIZE_BYTES 1000
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define SENSOR_PERIOD 2
static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
// Interval in number of sample to recalculate temp bias
#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 ZERO_ROT_ANGLE 0.00001f #define ZERO_ROT_ANGLE 0.00001f
// Private types // Private types
#define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(counterGyroSamples);
PERF_DEFINE_COUNTER(counterSensorPeriod);
// Counters:
// - 0x53000001 Sensor fetch rate(period)
// - 0x53000002 number of gyro samples read for each loop
// Private functions // Private functions
static void SensorsTask(void *parameters); static void SensorsTask(void *parameters);
@ -96,6 +113,13 @@ static float mag_transform[3][3] = {
static volatile bool gyro_temp_calibrated = false; static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false; static volatile bool accel_temp_calibrated = false;
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 float R[3][3] = { static float R[3][3] = {
{ 0 } { 0 }
}; };
@ -219,7 +243,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
vTaskDelay(10); vTaskDelay(10);
} }
} }
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
// Main task loop // Main task loop
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
bool error = false; bool error = false;
@ -234,7 +259,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif #endif
lastSysTime = xTaskGetTickCount(); lastSysTime = xTaskGetTickCount();
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
error = false; error = false;
} else { } else {
@ -263,7 +288,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
count = 0; count = 0;
while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) { while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
} }
if (error) { if (error) {
// Unfortunately if the BMA180 ever misses getting read, then it will not // Unfortunately if the BMA180 ever misses getting read, then it will not
@ -332,6 +357,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accel_samples++; accel_samples++;
} }
PERF_MEASURE_PERIOD(counterSensorPeriod);
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
if (gyro_samples == 0) { if (gyro_samples == 0) {
PIOS_MPU6000_ReadGyros(&mpu6000_data); PIOS_MPU6000_ReadGyros(&mpu6000_data);
error = true; error = true;
@ -350,24 +378,43 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0); 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 // Scale the accels
float accels[3] = { (float)accel_accum[0] / accel_samples, float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples, (float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples }; (float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X, 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, 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 }; accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
if (accel_temp_calibrated) {
float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: accelSensorData.temperature);
accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) { if (rotate) {
rot_mult(R, accels_out, accels); rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0]; accelSensorData.x = accels[0];
@ -385,18 +432,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
(float)gyro_accum[1] / gyro_samples, (float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples }; (float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X, 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, 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 }; gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
if (gyro_temp_calibrated) {
float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
(gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
: gyroSensorData.temperature);
gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) { if (rotate) {
rot_mult(R, gyros_out, gyros); rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0]; gyroSensorData.x = gyros[0];
@ -437,8 +476,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_WDG #ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif #endif
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
lastSysTime = xTaskGetTickCount();
} }
} }

2
flight/modules/Stabilization/altitudeloop.c
View File

@ -42,7 +42,7 @@
#ifdef REVOLUTION #ifdef REVOLUTION
#define UPDATE_EXPECTED (1.0f / 666.0f) #define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f #define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f #define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f #define UPDATE_ALPHA 1.0e-2f

2
flight/modules/Stabilization/innerloop.c
View File

@ -55,7 +55,7 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#define UPDATE_EXPECTED (1.0f / 666.0f) #define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f #define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f #define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f #define UPDATE_ALPHA 1.0e-2f

2
flight/modules/Stabilization/outerloop.c
View File

@ -52,7 +52,7 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define UPDATE_EXPECTED (1.0f / 666.0f) #define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f #define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f #define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f #define UPDATE_ALPHA 1.0e-2f

2
flight/modules/StateEstimation/filterekf.c
View File

@ -47,7 +47,7 @@
#define DT_ALPHA 1e-3f #define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f #define DT_MIN 1e-6f
#define DT_MAX 1.0f #define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo) #define DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \

2
flight/modules/StateEstimation/filtervelocity.c
View File

@ -40,7 +40,7 @@
#define DT_ALPHA 1e-3f #define DT_ALPHA 1e-3f
#define DT_MIN 1e-6f #define DT_MIN 1e-6f
#define DT_MAX 1.0f #define DT_MAX 1.0f
#define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo) #define DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
// Private types // Private types
struct data { struct data {

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

@ -33,15 +33,13 @@
#ifdef PIOS_INCLUDE_MPU6000 #ifdef PIOS_INCLUDE_MPU6000
#include "fifo_buffer.h" #include <pios_constants.h>
/* Global Variables */ /* Global Variables */
enum pios_mpu6000_dev_magic { enum pios_mpu6000_dev_magic {
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed, PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
}; };
#define PIOS_MPU6000_MAX_DOWNSAMPLE 2
struct mpu6000_dev { struct mpu6000_dev {
uint32_t spi_id; uint32_t spi_id;
uint32_t slave_num; uint32_t slave_num;
@ -53,29 +51,58 @@ struct mpu6000_dev {
enum pios_mpu6000_dev_magic magic; enum pios_mpu6000_dev_magic magic;
}; };
#ifdef PIOS_MPU6000_ACCEL
#define PIOS_MPU6000_SAMPLES_BYTES 14
#define PIOS_MPU6000_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;
uint8_t Gyro_X_l;
uint8_t Gyro_Y_h;
uint8_t Gyro_Y_l;
uint8_t Gyro_Z_h;
uint8_t Gyro_Z_l;
} data;
} mpu6000_data_t;
#define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l)
// ! Global structure for this device device // ! Global structure for this device device
static struct mpu6000_dev *dev; static struct mpu6000_dev *dev;
volatile bool mpu6000_configured = false; volatile bool mpu6000_configured = false;
static mpu6000_data_t mpu6000_data;
// ! Private functions // ! Private functions
static struct mpu6000_dev *PIOS_MPU6000_alloc(void); static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev); static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg); static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg);
static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_MPU6000_GetReg(uint8_t address); static int32_t PIOS_MPU6000_GetReg(uint8_t address);
static void PIOS_MPU6000_SetSpeed(const bool fast);
#define GRAV 9.81f static bool PIOS_MPU6000_HandleData();
static bool PIOS_MPU6000_ReadFifo(bool *woken);
#ifdef PIOS_MPU6000_ACCEL static bool PIOS_MPU6000_ReadSensor(bool *woken);
#define PIOS_MPU6000_SAMPLES_BYTES 14
#else
#define PIOS_MPU6000_SAMPLES_BYTES 8
#endif
/** /**
* @brief Allocate a new device * @brief Allocate a new device
*/ */
static struct mpu6000_dev *PIOS_MPU6000_alloc(void) static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg)
{ {
struct mpu6000_dev *mpu6000_dev; struct mpu6000_dev *mpu6000_dev;
@ -86,7 +113,7 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC; mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data)); mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
if (mpu6000_dev->queue == NULL) { if (mpu6000_dev->queue == NULL) {
vPortFree(mpu6000_dev); vPortFree(mpu6000_dev);
return NULL; return NULL;
@ -119,7 +146,7 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev)
*/ */
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg) int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg)
{ {
dev = PIOS_MPU6000_alloc(); dev = PIOS_MPU6000_alloc(cfg);
if (dev == NULL) { if (dev == NULL) {
return -1; return -1;
} }
@ -129,9 +156,7 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
dev->cfg = cfg; dev->cfg = cfg;
/* Configure the MPU6000 Sensor */ /* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
PIOS_MPU6000_Config(cfg); PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Set up EXTI line */ /* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg); PIOS_EXTI_Init(cfg->exti_cfg);
@ -234,11 +259,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
return -1; return -1;
} }
// TODO: check that changing the SPI clock speed is safe
// to do here given that interrupts are enabled and the bus has
// not been claimed/is not claimed during this call
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
// update filter settings // update filter settings
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) { while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
; ;
@ -267,7 +287,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
dev->accel_range = accelRange; dev->accel_range = accelRange;
#endif #endif
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
return 0; return 0;
} }
@ -275,7 +294,7 @@ int32_t PIOS_MPU6000_ConfigureRanges(
* @brief Claim the SPI bus for the accel communications and select this chip * @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/ */
static int32_t PIOS_MPU6000_ClaimBus() static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi)
{ {
if (PIOS_MPU6000_Validate(dev) != 0) { if (PIOS_MPU6000_Validate(dev) != 0) {
return -1; return -1;
@ -283,17 +302,28 @@ static int32_t PIOS_MPU6000_ClaimBus()
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) { if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
return -2; return -2;
} }
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0; return 0;
} }
static void PIOS_MPU6000_SetSpeed(const bool fast)
{
if (fast) {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler);
} else {
PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler);
}
}
/** /**
* @brief Claim the SPI bus for the accel communications and select this chip * @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
* @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority * @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 * task has is now eligible to run, else unchanged
*/ */
static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken) static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken, bool fast_spi)
{ {
if (PIOS_MPU6000_Validate(dev) != 0) { if (PIOS_MPU6000_Validate(dev) != 0) {
return -1; return -1;
@ -301,6 +331,7 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) { if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -2; return -2;
} }
PIOS_MPU6000_SetSpeed(fast_spi);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0); PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0; return 0;
} }
@ -342,7 +373,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
{ {
uint8_t data; uint8_t data;
if (PIOS_MPU6000_ClaimBus() != 0) { if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1; return -1;
} }
@ -363,7 +394,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
*/ */
static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data) static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
{ {
if (PIOS_MPU6000_ClaimBus() != 0) { if (PIOS_MPU6000_ClaimBus(false) != 0) {
return -1; return -1;
} }
@ -393,7 +424,7 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 }; uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
uint8_t rec[7]; uint8_t rec[7];
if (PIOS_MPU6000_ClaimBus() != 0) { if (PIOS_MPU6000_ClaimBus(true) != 0) {
return -1; return -1;
} }
@ -460,16 +491,16 @@ float PIOS_MPU6000_GetAccelScale()
{ {
switch (dev->accel_range) { switch (dev->accel_range) {
case PIOS_MPU6000_ACCEL_2G: case PIOS_MPU6000_ACCEL_2G:
return GRAV / 16384.0f; return PIOS_CONST_MKS_GRAV_ACCEL_F / 16384.0f;
case PIOS_MPU6000_ACCEL_4G: case PIOS_MPU6000_ACCEL_4G:
return GRAV / 8192.0f; return PIOS_CONST_MKS_GRAV_ACCEL_F / 8192.0f;
case PIOS_MPU6000_ACCEL_8G: case PIOS_MPU6000_ACCEL_8G:
return GRAV / 4096.0f; return PIOS_CONST_MKS_GRAV_ACCEL_F / 4096.0f;
case PIOS_MPU6000_ACCEL_16G: case PIOS_MPU6000_ACCEL_16G:
return GRAV / 2048.0f; return PIOS_CONST_MKS_GRAV_ACCEL_F / 2048.0f;
} }
return 0; return 0;
} }
@ -504,7 +535,7 @@ static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
/* Interrupt Status register can be read at high SPI clock speed */ /* Interrupt Status register can be read at high SPI clock speed */
uint8_t data; uint8_t data;
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1; return -1;
} }
PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG)); PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
@ -525,29 +556,16 @@ static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
{ {
int32_t result = 0; int32_t result = 0;
/* Register writes must be at < 1MHz SPI clock. if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
* Speed can only be changed when SPI bus semaphore is held, but
* device chip select must not be enabled, so we use the direct
* SPI bus claim call here */
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
return -1; return -1;
} }
/* Reduce SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
/* Enable chip select */
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
/* Reset FIFO. */ /* Reset FIFO. */
if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) { if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
result = -2; result = -2;
} else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) { } else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
result = -2; result = -2;
} }
/* Disable chip select. */ PIOS_MPU6000_ReleaseBusISR(woken);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
/* Increase SPI clock speed. */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Release the SPI bus semaphore. */
PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
return result; return result;
} }
@ -562,7 +580,7 @@ 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_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
uint8_t mpu6000_rec_buf[3]; uint8_t mpu6000_rec_buf[3];
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) { if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
return -1; return -1;
} }
@ -604,6 +622,97 @@ bool PIOS_MPU6000_IRQHandler(void)
return false; 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);
}
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()
{
// 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
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
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
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
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);
BaseType_t higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
return higherPriorityTaskWoken == pdTRUE;
}
static bool PIOS_MPU6000_ReadSensor(bool *woken)
{
const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_SENSOR_FIRST_REG | 0x80 };
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;
}
static bool PIOS_MPU6000_ReadFifo(bool *woken)
{
/* Temporary fix for OP-1049. Expected to be superceded for next major release /* Temporary fix for OP-1049. Expected to be superceded for next major release
* by code changes for OP-1039. * by code changes for OP-1039.
* Read interrupt status register to check for FIFO overflow. Must be the * Read interrupt status register to check for FIFO overflow. Must be the
@ -611,128 +720,57 @@ bool PIOS_MPU6000_IRQHandler(void)
* any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in * any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
* interrupt config register) */ * interrupt config register) */
int32_t result; int32_t result;
if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(&woken)) < 0) {
return woken; if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
return false;
} }
if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) { if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
/* The FIFO has overflowed, so reset it, /* The FIFO has overflowed, so reset it,
* to enable sample sync to be recovered. * to enable sample sync to be recovered.
* If the reset fails, we are in trouble, but * If the reset fails, we are in trouble, but
* we keep trying on subsequent interrupts. */ * we keep trying on subsequent interrupts. */
PIOS_MPU6000_ResetFifoISR(&woken); PIOS_MPU6000_ResetFifoISR(woken);
/* Return and wait for the next new sample. */ /* Return and wait for the next new sample. */
return woken; return false;
} }
/* Usual case - FIFO has not overflowed. */ /* Usual case - FIFO has not overflowed. */
mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken); mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) { if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
return woken; return false;
} }
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return woken; return false;
} }
static uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 }; const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
static uint8_t mpu6000_rec_buf[1 + PIOS_MPU6000_SAMPLES_BYTES];
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(&woken); PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++; mpu6000_fails++;
return woken; return false;
} }
PIOS_MPU6000_ReleaseBusISR(&woken); PIOS_MPU6000_ReleaseBusISR(woken);
static struct pios_mpu6000_data data;
// In the case where extras samples backed up grabbed an extra // In the case where extras samples backed up grabbed an extra
if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) { if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
mpu6000_fifo_backup++; mpu6000_fifo_backup++;
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) { if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
return woken; return false;
} }
if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) { if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
PIOS_MPU6000_ReleaseBusISR(&woken); PIOS_MPU6000_ReleaseBusISR(woken);
mpu6000_fails++; mpu6000_fails++;
return woken; return false;
} }
PIOS_MPU6000_ReleaseBusISR(&woken); PIOS_MPU6000_ReleaseBusISR(woken);
} }
return true;
// 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
#if defined(PIOS_MPU6000_ACCEL)
// Currently we only support rotations on top so switch X/Y accordingly
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.gyro_y = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
data.gyro_x = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
break;
case PIOS_MPU6000_TOP_90DEG:
// -1 to bring it back to -32768 +32767 range
data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
data.gyro_y = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
data.gyro_x = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
data.gyro_y = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
data.gyro_x = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
break;
case PIOS_MPU6000_TOP_270DEG:
data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
data.gyro_y = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
data.gyro_x = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
break;
}
data.gyro_z = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]);
data.accel_z = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
#else /* if defined(PIOS_MPU6000_ACCEL) */
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
switch (dev->cfg->orientation) {
case PIOS_MPU6000_TOP_0DEG:
data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
break;
case PIOS_MPU6000_TOP_90DEG:
data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y
data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X
break;
case PIOS_MPU6000_TOP_180DEG:
data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]);
data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
break;
case PIOS_MPU6000_TOP_270DEG:
data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y
data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X
break;
}
data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]);
data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
#endif /* if defined(PIOS_MPU6000_ACCEL) */
signed portBASE_TYPE higherPriorityTaskWoken;
xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
mpu6000_irq++;
mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
return woken || higherPriorityTaskWoken == pdTRUE;
} }
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */
/** /**

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

@ -159,6 +159,9 @@ struct pios_mpu6000_cfg {
enum pios_mpu6000_range gyro_range; enum pios_mpu6000_range gyro_range;
enum pios_mpu6000_filter filter; enum pios_mpu6000_filter filter;
enum pios_mpu6000_orientation orientation; enum pios_mpu6000_orientation orientation;
SPIPrescalerTypeDef fast_prescaler;
SPIPrescalerTypeDef std_prescaler;
uint8_t max_downsample;
}; };
/* Public Functions */ /* Public Functions */

108
flight/pios/stm32f10x/pios_spi.c
View File

@ -44,6 +44,7 @@ static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_d
/* Should check device magic here */ /* Should check device magic here */
return true; return true;
} }
#define SPI_MAX_BLOCK_PIO 128
#if defined(PIOS_INCLUDE_FREERTOS) #if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_spi_dev *PIOS_SPI_alloc(void) static struct pios_spi_dev *PIOS_SPI_alloc(void)
@ -432,23 +433,7 @@ int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b)
return rx_byte; return rx_byte;
} }
/** static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
* Transfers a block of bytes via DMA.
* \param[in] spi SPI number (0 or 1)
* \param[in] send_buffer pointer to buffer which should be sent.<BR>
* If NULL, 0xff (all-one) will be sent.
* \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
* If NULL, received bytes will be discarded.
* \param[in] len number of bytes which should be transfered
* \param[in] callback pointer to callback function which will be executed
* from DMA channel interrupt once the transfer is finished.
* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will
* block until the transfer is finished.
* \return >= 0 if no error during transfer
* \return -1 if disabled SPI port selected
* \return -3 if function has been called during an ongoing DMA transfer
*/
int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
{ {
struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id; struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id;
@ -580,6 +565,95 @@ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint
return 0; return 0;
} }
/**
* Transfers a block of bytes via PIO.
*
* \param[in] spi_id SPI device handle
* \param[in] send_buffer pointer to buffer which should be sent.<BR>
* If NULL, 0xff (all-one) will be sent.
* \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
* If NULL, received bytes will be discarded.
* \param[in] len number of bytes which should be transfered
* \return >= 0 if no error during transfer
* \return -1 if disabled SPI port selected
* \return -3 if function has been called during an ongoing DMA transfer
*/
static int32_t SPI_PIO_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len)
{
struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id;
uint8_t b;
bool valid = PIOS_SPI_validate(spi_dev);
PIOS_Assert(valid)
/* Exit if ongoing transfer */
if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) {
return -3;
}
/* Make sure the RXNE flag is cleared by reading the DR register */
b = spi_dev->cfg->regs->DR;
while (len--) {
/* get the byte to send */
b = send_buffer ? *(send_buffer++) : 0xff;
/* Start the transfer */
spi_dev->cfg->regs->DR = b;
/* Wait until there is a byte to read */
while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) {
;
}
/* Read the rx'd byte */
b = spi_dev->cfg->regs->DR;
/* save the received byte */
if (receive_buffer) {
*(receive_buffer++) = b;
}
/* Wait until the TXE goes high */
while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) {
;
}
}
/* Wait for SPI transfer to have fully completed */
while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) {
;
}
return 0;
}
/**
* Transfers a block of bytes via PIO or DMA.
* \param[in] spi_id SPI device handle
* \param[in] send_buffer pointer to buffer which should be sent.<BR>
* If NULL, 0xff (all-one) will be sent.
* \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
* If NULL, received bytes will be discarded.
* \param[in] len number of bytes which should be transfered
* \param[in] callback pointer to callback function which will be executed
* from DMA channel interrupt once the transfer is finished.
* If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will
* block until the transfer is finished.
* \return >= 0 if no error during transfer
* \return -1 if disabled SPI port selected
* \return -3 if function has been called during an ongoing DMA transfer
*/
int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
{
if (callback || len > SPI_MAX_BLOCK_PIO) {
return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback);
}
return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len);
}
/** /**
* Check if a transfer is in progress * Check if a transfer is in progress
* \param[in] spi SPI number (0 or 1) * \param[in] spi SPI number (0 or 1)

12
flight/targets/boards/coptercontrol/board_hw_defs.c
View File

@ -260,7 +260,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_13, .GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
}, },
@ -268,7 +268,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_14, .GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING, .GPIO_Mode = GPIO_Mode_IN_FLOATING,
}, },
}, },
@ -276,7 +276,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_15, .GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
}, },
@ -360,7 +360,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_13, .GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
}, },
@ -368,7 +368,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_14, .GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_IN_FLOATING, .GPIO_Mode = GPIO_Mode_IN_FLOATING,
}, },
}, },
@ -376,7 +376,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
.gpio = GPIOB, .gpio = GPIOB,
.init = { .init = {
.GPIO_Pin = GPIO_Pin_15, .GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_10MHz, .GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF_PP, .GPIO_Mode = GPIO_Mode_AF_PP,
}, },
}, },

2
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -92,6 +92,8 @@
/* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_ETASV3 */
/* #define PIOS_INCLUDE_HCSR04 */ /* #define PIOS_INCLUDE_HCSR04 */
#define PIOS_SENSOR_RATE 500.0f
/* PIOS receiver drivers */ /* PIOS receiver drivers */
#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PPM

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

@ -119,18 +119,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg, .exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 16 for 500 Hz // Clock at 8 khz, downsampled by 8 for 1000 Hz
.Smpl_rate_div_no_dlp = 15, .Smpl_rate_div_no_dlp = 7,
// Clock at 1 khz, downsampled by 2 for 500 Hz // Clock at 1 khz, downsampled by 1 for 1000 Hz
.Smpl_rate_div_dlp = 1, .Smpl_rate_div_dlp = 0,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G, .accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG, .gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ, .filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG .orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 2
}; };
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */

3
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
View File

@ -91,6 +91,9 @@
// #define PIOS_INCLUDE_MPXV // #define PIOS_INCLUDE_MPXV
// #define PIOS_INCLUDE_ETASV3 // #define PIOS_INCLUDE_ETASV3
/* #define PIOS_INCLUDE_HCSR04 */ /* #define PIOS_INCLUDE_HCSR04 */
#define PIOS_SENSOR_RATE 500.0f
#define PIOS_INCLUDE_WS2811 #define PIOS_INCLUDE_WS2811
/* PIOS receiver drivers */ /* PIOS receiver drivers */

5
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -91,11 +91,12 @@
#define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_MS4525DO #define PIOS_INCLUDE_MS4525DO
/* #define PIOS_INCLUDE_HCSR04 */
#define PIOS_SENSOR_RATE 500.0f
#define PIOS_INCLUDE_WS2811 #define PIOS_INCLUDE_WS2811
/* #define PIOS_INCLUDE_HCSR04 */
/* PIOS receiver drivers */ /* PIOS receiver drivers */
#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PPM

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

@ -192,18 +192,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg, .exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 12 for 666Hz // Clock at 8 khz
.Smpl_rate_div_no_dlp = 11, .Smpl_rate_div_no_dlp = 0,
// with dlp on output rate is 500Hz // with dlp on output rate is 1000Hz
.Smpl_rate_div_dlp = 1, .Smpl_rate_div_dlp = 0,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G, .accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG, .gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ, .filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_180DEG .orientation = PIOS_MPU6000_TOP_180DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 16,
}; };
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */

2
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -89,6 +89,8 @@
#define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_ETASV3
/* #define PIOS_INCLUDE_HCSR04 */ /* #define PIOS_INCLUDE_HCSR04 */
#define PIOS_SENSOR_RATE 500.0f
/* PIOS receiver drivers */ /* PIOS receiver drivers */
#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PWM
#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PPM

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

@ -222,18 +222,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg, .exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 12 for 666Hz // Clock at 8 khz
.Smpl_rate_div_no_dlp = 11, .Smpl_rate_div_no_dlp = 0,
// with dlp on output rate is 500Hz // with dlp on output rate is 1000Hz
.Smpl_rate_div_dlp = 1, .Smpl_rate_div_dlp = 0,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,
.User_ctl = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C, .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C,
.Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
.accel_range = PIOS_MPU6000_ACCEL_8G, .accel_range = PIOS_MPU6000_ACCEL_8G,
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG, .gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
.filter = PIOS_MPU6000_LOWPASS_256_HZ, .filter = PIOS_MPU6000_LOWPASS_256_HZ,
.orientation = PIOS_MPU6000_TOP_0DEG .orientation = PIOS_MPU6000_TOP_0DEG,
.fast_prescaler = PIOS_SPI_PRESCALER_4,
.std_prescaler = PIOS_SPI_PRESCALER_64,
.max_downsample = 16,
}; };
#endif /* PIOS_INCLUDE_MPU6000 */ #endif /* PIOS_INCLUDE_MPU6000 */

3
flight/targets/boards/simposix/firmware/inc/pios_config.h
View File

@ -70,6 +70,9 @@
// #define PIOS_INCLUDE_MS5611 // #define PIOS_INCLUDE_MS5611
// #define PIOS_INCLUDE_HCSR04 // #define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define PIOS_FLASH_ON_ACCEL /* true for second revo */
#define PIOS_SENSOR_RATE 500.0f
#define FLASH_FREERTOS #define FLASH_FREERTOS
/* Com systems to include */ /* Com systems to include */
#define PIOS_INCLUDE_COM #define PIOS_INCLUDE_COM