mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Merge branch 'amorale/OP-1535_mpu_overhaul' into next
This commit is contained in:
Alessio Morale
commit
a9f35e2afe
@ -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.
|
||||
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
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#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
|
||||
@ -50,6 +51,14 @@
|
||||
#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
|
||||
@ -57,6 +66,11 @@ 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);
|
||||
@ -166,13 +180,22 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
|
||||
temp = PIOS_MS5611_GetTemperature();
|
||||
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
|
||||
// in case the temperature is outside of the calibrated range, uses the nearest extremes
|
||||
float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
|
||||
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
|
||||
press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
|
||||
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)) {
|
||||
|
||||
@ -64,8 +64,8 @@
|
||||
|
||||
#include "CoordinateConversions.h"
|
||||
#include <pios_notify.h>
|
||||
|
||||
|
||||
#include <mathmisc.h>
|
||||
#include <pios_constants.h>
|
||||
#include <pios_instrumentation_helper.h>
|
||||
|
||||
PERF_DEFINE_COUNTER(counterUpd);
|
||||
@ -82,10 +82,19 @@ PERF_DEFINE_COUNTER(counterAtt);
|
||||
#define STACK_SIZE_BYTES 540
|
||||
#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_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_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
@ -130,6 +139,10 @@ static bool apply_accel_temp = false;
|
||||
static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
|
||||
static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
|
||||
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
|
||||
static AccelGyroSettingsgyro_scaleData gyro_scale;
|
||||
@ -142,8 +155,7 @@ static volatile int32_t trim_accels[3];
|
||||
static volatile int32_t trim_samples;
|
||||
int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
|
||||
|
||||
#define GRAV 9.81f
|
||||
#define STD_CC_ACCEL_SCALE (GRAV * 0.004f)
|
||||
#define STD_CC_ACCEL_SCALE (PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f)
|
||||
/* 0.004f is gravity / LSB */
|
||||
#define STD_CC_ANALOG_GYRO_NEUTRAL 1665
|
||||
#define STD_CC_ANALOG_GYRO_GAIN 0.42f
|
||||
@ -222,12 +234,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
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;
|
||||
|
||||
if (cc3d) {
|
||||
@ -236,6 +242,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
|
||||
#endif
|
||||
} else {
|
||||
#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();
|
||||
#endif
|
||||
|
||||
@ -257,7 +268,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
|
||||
settingsUpdatedCb(AttitudeSettingsHandle());
|
||||
|
||||
PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||
|
||||
portTickType lastSysTime = xTaskGetTickCount();
|
||||
// Main task loop
|
||||
while (1) {
|
||||
FlightStatusData flightStatus;
|
||||
@ -317,6 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
|
||||
PERF_MEASURE_PERIOD(counterPeriod);
|
||||
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)
|
||||
|
||||
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) {
|
||||
gyros[0] += mpu6000_data.gyro_x;
|
||||
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
|
||||
ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
|
||||
}
|
||||
PERF_TRACK_VALUE(counterAccelSamples, count);
|
||||
|
||||
if (!count) {
|
||||
return -1; // Error, no data
|
||||
@ -488,21 +501,39 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
|
||||
accels[1] *= accel_scale.Y * invcount;
|
||||
accels[2] *= accel_scale.Z * 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) {
|
||||
gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
|
||||
gyros[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[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
|
||||
gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
|
||||
gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
|
||||
}
|
||||
|
||||
if (apply_accel_temp) {
|
||||
accels[0] -= accel_temp_coeff.X * ctemp;
|
||||
accels[1] -= accel_temp_coeff.Y * ctemp;
|
||||
accels[2] -= accel_temp_coeff.Z * ctemp;
|
||||
accel_temp_bias[0] = accel_temp_coeff.X * ctemp;
|
||||
accel_temp_bias[1] = accel_temp_coeff.Y * 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;
|
||||
// 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);
|
||||
|
||||
// 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]);
|
||||
if (accel_mag < 1.0e-3f) {
|
||||
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 (inv_accel_mag > 1e3f) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Account for filtered gravity vector magnitude
|
||||
float grot_mag;
|
||||
float inv_grot_mag;
|
||||
|
||||
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 {
|
||||
grot_mag = 1.0f;
|
||||
inv_grot_mag = 1.0f;
|
||||
}
|
||||
|
||||
if (grot_mag < 1.0e-3f) {
|
||||
if (inv_grot_mag > 1e3f) {
|
||||
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[1] *= invMag;
|
||||
accel_err[2] *= invMag;
|
||||
@ -645,21 +676,20 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
|
||||
}
|
||||
|
||||
// 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.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) {
|
||||
if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
q[3] = 0;
|
||||
} else {
|
||||
const float invQmag = 1.0f / qmag;
|
||||
q[0] = q[0] * invQmag;
|
||||
q[1] = q[1] * invQmag;
|
||||
q[2] = q[2] * invQmag;
|
||||
q[3] = q[3] * invQmag;
|
||||
q[0] = q[0] * inv_qmag;
|
||||
q[1] = q[1] * inv_qmag;
|
||||
q[2] = q[2] * inv_qmag;
|
||||
q[3] = q[3] * inv_qmag;
|
||||
}
|
||||
|
||||
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.Y = trim_accels[1] / trim_samples;
|
||||
// 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;
|
||||
AttitudeSettingsSet(&attitudeSettings);
|
||||
} else {
|
||||
|
||||
@ -58,7 +58,7 @@
|
||||
#include <accelgyrosettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <taskinfo.h>
|
||||
|
||||
#include <pios_math.h>
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
#include <pios_board_info.h>
|
||||
@ -66,11 +66,28 @@
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 1000
|
||||
#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
|
||||
// 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
|
||||
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 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] = {
|
||||
{ 0 }
|
||||
};
|
||||
@ -219,7 +243,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
vTaskDelay(10);
|
||||
}
|
||||
}
|
||||
|
||||
PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
|
||||
PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
bool error = false;
|
||||
@ -234,7 +259,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
#endif
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
|
||||
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
error = false;
|
||||
} else {
|
||||
@ -263,7 +288,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
count = 0;
|
||||
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) {
|
||||
// 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++;
|
||||
}
|
||||
|
||||
PERF_MEASURE_PERIOD(counterSensorPeriod);
|
||||
PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
|
||||
|
||||
if (gyro_samples == 0) {
|
||||
PIOS_MPU6000_ReadGyros(&mpu6000_data);
|
||||
error = true;
|
||||
@ -350,24 +378,43 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
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,
|
||||
accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
|
||||
accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
|
||||
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 (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) {
|
||||
rot_mult(R, accels_out, accels);
|
||||
accelSensorData.x = accels[0];
|
||||
@ -385,18 +432,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
(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,
|
||||
gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
|
||||
gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
|
||||
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 (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) {
|
||||
rot_mult(R, gyros_out, gyros);
|
||||
gyroSensorData.x = gyros[0];
|
||||
@ -437,8 +476,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
#ifdef PIOS_INCLUDE_WDG
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
|
||||
#endif
|
||||
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -42,7 +42,7 @@
|
||||
|
||||
#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_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
@ -55,7 +55,7 @@
|
||||
|
||||
#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_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
@ -52,7 +52,7 @@
|
||||
|
||||
#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_MAX 1.0f
|
||||
#define UPDATE_ALPHA 1.0e-2f
|
||||
|
||||
@ -47,7 +47,7 @@
|
||||
#define DT_ALPHA 1e-3f
|
||||
#define DT_MIN 1e-6f
|
||||
#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) \
|
||||
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
|
||||
|
||||
@ -40,7 +40,7 @@
|
||||
#define DT_ALPHA 1e-3f
|
||||
#define DT_MIN 1e-6f
|
||||
#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
|
||||
struct data {
|
||||
|
||||
@ -33,15 +33,13 @@
|
||||
|
||||
#ifdef PIOS_INCLUDE_MPU6000
|
||||
|
||||
#include "fifo_buffer.h"
|
||||
|
||||
#include <pios_constants.h>
|
||||
/* Global Variables */
|
||||
|
||||
enum pios_mpu6000_dev_magic {
|
||||
PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
|
||||
};
|
||||
|
||||
#define PIOS_MPU6000_MAX_DOWNSAMPLE 2
|
||||
struct mpu6000_dev {
|
||||
uint32_t spi_id;
|
||||
uint32_t slave_num;
|
||||
@ -53,29 +51,58 @@ 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;
|
||||
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
|
||||
static struct mpu6000_dev *dev;
|
||||
volatile bool mpu6000_configured = false;
|
||||
static mpu6000_data_t mpu6000_data;
|
||||
|
||||
// ! 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 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_GetReg(uint8_t address);
|
||||
|
||||
#define GRAV 9.81f
|
||||
|
||||
#ifdef PIOS_MPU6000_ACCEL
|
||||
#define PIOS_MPU6000_SAMPLES_BYTES 14
|
||||
#else
|
||||
#define PIOS_MPU6000_SAMPLES_BYTES 8
|
||||
#endif
|
||||
|
||||
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);
|
||||
/**
|
||||
* @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;
|
||||
|
||||
@ -86,7 +113,7 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
|
||||
|
||||
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) {
|
||||
vPortFree(mpu6000_dev);
|
||||
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)
|
||||
{
|
||||
dev = PIOS_MPU6000_alloc();
|
||||
dev = PIOS_MPU6000_alloc(cfg);
|
||||
if (dev == NULL) {
|
||||
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;
|
||||
|
||||
/* Configure the MPU6000 Sensor */
|
||||
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
|
||||
PIOS_MPU6000_Config(cfg);
|
||||
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
|
||||
|
||||
/* Set up EXTI line */
|
||||
PIOS_EXTI_Init(cfg->exti_cfg);
|
||||
@ -234,11 +259,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
|
||||
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
|
||||
while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
|
||||
;
|
||||
@ -267,7 +287,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
|
||||
|
||||
dev->accel_range = accelRange;
|
||||
#endif
|
||||
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -275,7 +294,7 @@ int32_t PIOS_MPU6000_ConfigureRanges(
|
||||
* @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
|
||||
*/
|
||||
static int32_t PIOS_MPU6000_ClaimBus()
|
||||
static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi)
|
||||
{
|
||||
if (PIOS_MPU6000_Validate(dev) != 0) {
|
||||
return -1;
|
||||
@ -283,17 +302,28 @@ static int32_t PIOS_MPU6000_ClaimBus()
|
||||
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
|
||||
return -2;
|
||||
}
|
||||
PIOS_MPU6000_SetSpeed(fast_spi);
|
||||
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 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
|
||||
* @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
|
||||
* 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) {
|
||||
return -1;
|
||||
@ -301,6 +331,7 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
|
||||
if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
|
||||
return -2;
|
||||
}
|
||||
PIOS_MPU6000_SetSpeed(fast_spi);
|
||||
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
|
||||
return 0;
|
||||
}
|
||||
@ -342,7 +373,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
|
||||
{
|
||||
uint8_t data;
|
||||
|
||||
if (PIOS_MPU6000_ClaimBus() != 0) {
|
||||
if (PIOS_MPU6000_ClaimBus(false) != 0) {
|
||||
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)
|
||||
{
|
||||
if (PIOS_MPU6000_ClaimBus() != 0) {
|
||||
if (PIOS_MPU6000_ClaimBus(false) != 0) {
|
||||
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 rec[7];
|
||||
|
||||
if (PIOS_MPU6000_ClaimBus() != 0) {
|
||||
if (PIOS_MPU6000_ClaimBus(true) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -460,16 +491,16 @@ float PIOS_MPU6000_GetAccelScale()
|
||||
{
|
||||
switch (dev->accel_range) {
|
||||
case PIOS_MPU6000_ACCEL_2G:
|
||||
return GRAV / 16384.0f;
|
||||
return PIOS_CONST_MKS_GRAV_ACCEL_F / 16384.0f;
|
||||
|
||||
case PIOS_MPU6000_ACCEL_4G:
|
||||
return GRAV / 8192.0f;
|
||||
return PIOS_CONST_MKS_GRAV_ACCEL_F / 8192.0f;
|
||||
|
||||
case PIOS_MPU6000_ACCEL_8G:
|
||||
return GRAV / 4096.0f;
|
||||
return PIOS_CONST_MKS_GRAV_ACCEL_F / 4096.0f;
|
||||
|
||||
case PIOS_MPU6000_ACCEL_16G:
|
||||
return GRAV / 2048.0f;
|
||||
return PIOS_CONST_MKS_GRAV_ACCEL_F / 2048.0f;
|
||||
}
|
||||
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 */
|
||||
uint8_t data;
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
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;
|
||||
|
||||
/* Register writes must be at < 1MHz SPI clock.
|
||||
* 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) {
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
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. */
|
||||
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;
|
||||
}
|
||||
/* Disable chip select. */
|
||||
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);
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
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_rec_buf[3];
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -604,6 +622,97 @@ bool PIOS_MPU6000_IRQHandler(void)
|
||||
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
|
||||
* by code changes for OP-1039.
|
||||
* 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
|
||||
* interrupt config register) */
|
||||
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) {
|
||||
/* 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);
|
||||
PIOS_MPU6000_ResetFifoISR(woken);
|
||||
/* Return and wait for the next new sample. */
|
||||
return woken;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 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) {
|
||||
return woken;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
|
||||
return woken;
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
static 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];
|
||||
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_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
|
||||
PIOS_MPU6000_ReleaseBusISR(&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 woken;
|
||||
return false;
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBusISR(&woken);
|
||||
|
||||
static struct pios_mpu6000_data data;
|
||||
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) != 0) {
|
||||
return woken;
|
||||
if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
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);
|
||||
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 woken;
|
||||
return false;
|
||||
}
|
||||
|
||||
PIOS_MPU6000_ReleaseBusISR(&woken);
|
||||
PIOS_MPU6000_ReleaseBusISR(woken);
|
||||
}
|
||||
|
||||
// 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;
|
||||
return true;
|
||||
}
|
||||
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 */
|
||||
|
||||
/**
|
||||
|
||||
@ -159,6 +159,9 @@ struct pios_mpu6000_cfg {
|
||||
enum pios_mpu6000_range gyro_range;
|
||||
enum pios_mpu6000_filter filter;
|
||||
enum pios_mpu6000_orientation orientation;
|
||||
SPIPrescalerTypeDef fast_prescaler;
|
||||
SPIPrescalerTypeDef std_prescaler;
|
||||
uint8_t max_downsample;
|
||||
};
|
||||
|
||||
/* Public Functions */
|
||||
|
||||
@ -44,6 +44,7 @@ static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_d
|
||||
/* Should check device magic here */
|
||||
return true;
|
||||
}
|
||||
#define SPI_MAX_BLOCK_PIO 128
|
||||
|
||||
#if defined(PIOS_INCLUDE_FREERTOS)
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
static int32_t SPI_DMA_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;
|
||||
|
||||
@ -580,6 +565,95 @@ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint
|
||||
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
|
||||
* \param[in] spi SPI number (0 or 1)
|
||||
|
||||
@ -260,7 +260,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
@ -268,7 +268,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
@ -276,7 +276,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
@ -360,7 +360,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
@ -368,7 +368,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
@ -376,7 +376,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_10MHz,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
|
||||
@ -92,6 +92,8 @@
|
||||
/* #define PIOS_INCLUDE_ETASV3 */
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
/* PIOS receiver drivers */
|
||||
#define PIOS_INCLUDE_PWM
|
||||
#define PIOS_INCLUDE_PPM
|
||||
|
||||
@ -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 = {
|
||||
.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,
|
||||
// Clock at 8 khz, downsampled by 16 for 500 Hz
|
||||
.Smpl_rate_div_no_dlp = 15,
|
||||
// Clock at 1 khz, downsampled by 2 for 500 Hz
|
||||
.Smpl_rate_div_dlp = 1,
|
||||
// Clock at 8 khz, downsampled by 8 for 1000 Hz
|
||||
.Smpl_rate_div_no_dlp = 7,
|
||||
// Clock at 1 khz, downsampled by 1 for 1000 Hz
|
||||
.Smpl_rate_div_dlp = 0,
|
||||
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
|
||||
.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,
|
||||
.accel_range = PIOS_MPU6000_ACCEL_8G,
|
||||
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
|
||||
.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 */
|
||||
|
||||
|
||||
@ -91,6 +91,9 @@
|
||||
// #define PIOS_INCLUDE_MPXV
|
||||
// #define PIOS_INCLUDE_ETASV3
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
#define PIOS_INCLUDE_WS2811
|
||||
|
||||
/* PIOS receiver drivers */
|
||||
|
||||
@ -91,11 +91,12 @@
|
||||
#define PIOS_INCLUDE_MPXV
|
||||
#define PIOS_INCLUDE_ETASV3
|
||||
#define PIOS_INCLUDE_MS4525DO
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
#define PIOS_INCLUDE_WS2811
|
||||
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
/* PIOS receiver drivers */
|
||||
#define PIOS_INCLUDE_PWM
|
||||
#define PIOS_INCLUDE_PPM
|
||||
|
||||
@ -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 = {
|
||||
.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,
|
||||
// Clock at 8 khz, downsampled by 12 for 666Hz
|
||||
.Smpl_rate_div_no_dlp = 11,
|
||||
// with dlp on output rate is 500Hz
|
||||
.Smpl_rate_div_dlp = 1,
|
||||
// Clock at 8 khz
|
||||
.Smpl_rate_div_no_dlp = 0,
|
||||
// with dlp on output rate is 1000Hz
|
||||
.Smpl_rate_div_dlp = 0,
|
||||
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
|
||||
.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,
|
||||
.accel_range = PIOS_MPU6000_ACCEL_8G,
|
||||
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
|
||||
.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 */
|
||||
|
||||
|
||||
@ -89,6 +89,8 @@
|
||||
#define PIOS_INCLUDE_ETASV3
|
||||
/* #define PIOS_INCLUDE_HCSR04 */
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
/* PIOS receiver drivers */
|
||||
#define PIOS_INCLUDE_PWM
|
||||
#define PIOS_INCLUDE_PPM
|
||||
|
||||
@ -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 = {
|
||||
.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,
|
||||
// Clock at 8 khz, downsampled by 12 for 666Hz
|
||||
.Smpl_rate_div_no_dlp = 11,
|
||||
// with dlp on output rate is 500Hz
|
||||
.Smpl_rate_div_dlp = 1,
|
||||
// Clock at 8 khz
|
||||
.Smpl_rate_div_no_dlp = 0,
|
||||
// with dlp on output rate is 1000Hz
|
||||
.Smpl_rate_div_dlp = 0,
|
||||
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
|
||||
.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,
|
||||
.accel_range = PIOS_MPU6000_ACCEL_8G,
|
||||
.gyro_range = PIOS_MPU6000_SCALE_2000_DEG,
|
||||
.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 */
|
||||
|
||||
|
||||
@ -70,6 +70,9 @@
|
||||
// #define PIOS_INCLUDE_MS5611
|
||||
// #define PIOS_INCLUDE_HCSR04
|
||||
#define PIOS_FLASH_ON_ACCEL /* true for second revo */
|
||||
|
||||
#define PIOS_SENSOR_RATE 500.0f
|
||||
|
||||
#define FLASH_FREERTOS
|
||||
/* Com systems to include */
|
||||
#define PIOS_INCLUDE_COM
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user