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

OP-1535 - Add board specific sensor update rate constant

Browse Source
This commit is contained in:
Alessio Morale 2014-10-14 05:22:36 +02:00
parent f2fc5f5616
commit 5d074063d1
13 changed files with 55 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,8 +53,11 @@
// Interval in number of sample to recalculate temp bias
#define TEMP_CALIB_INTERVAL 10
// 5Hz @120Hz update
#define TEMP_ALPHA 0.9979f
// 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
@ -181,7 +184,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
baro_temperature = temp;
}
baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp;
baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
if (tempCorrectionEnabled && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;

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

@ -82,18 +82,19 @@ PERF_DEFINE_COUNTER(counterAtt);
#define STACK_SIZE_BYTES 540
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
// Attitude module runs at 500Hz
#define SENSOR_PERIOD 2
// 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
// 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));
// LPF 5Hz at 500Hz rate
#define TEMP_ALPHA 0.999504f
#define UPDATE_EXPECTED (1.0f / 500.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
@ -327,7 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
PERF_MEASURE_PERIOD(counterPeriod);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_PERIOD_MS);
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS);
}
}
@ -465,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;
@ -492,19 +493,19 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
}
float invcount = 1.0f / count;
PERF_TIMED_SECTION_START(counterUpd);
gyros[0] *= gyro_scale.X * invcount;
gyros[1] *= gyro_scale.Y * invcount;
gyros[2] *= gyro_scale.Z * invcount;
gyros[0] *= gyro_scale.X * invcount;
gyros[1] *= gyro_scale.Y * invcount;
gyros[2] *= gyro_scale.Z * invcount;
accels[0] *= accel_scale.X * invcount;
accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount;
accels[0] *= accel_scale.X * invcount;
accels[1] *= accel_scale.Y * invcount;
accels[2] *= accel_scale.Z * invcount;
temp *= invcount;
if(isnan(temperature)){
if (isnan(temperature)) {
temperature = temp;
}
temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp;
temperature = temp_alpha * (temp - temperature) + temperature;
if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
temp_calibration_count = TEMP_CALIB_INTERVAL;

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

@ -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,12 +66,16 @@
// 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 5Hz at 500Hz rate
#define TEMP_ALPHA 0.999504f
// 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
@ -255,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 {
@ -284,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
@ -379,9 +383,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyro_temperature = gyroSensorData.temperature;
}
accel_temperature = TEMP_ALPHA * accel_temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature;
gyro_temperature = TEMP_ALPHA * gyro_temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature;
gyroSensorData.temperature = gyro_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) {
@ -472,7 +476,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
#endif
vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
}
}

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

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

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

@ -54,7 +54,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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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