Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ahrs] add possibility for fixed dt again

Browse Source
This commit is contained in:
Felix Ruess
2014-11-10 16:57:03 +01:00
parent 31570c1871
commit e90e4c40a7
6 changed files with 118 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/main.c
+12 -13
View File
@@ -325,19 +325,6 @@ static inline void on_gyro_event( void ) {
// current timestamp
uint32_t now_ts = get_sys_time_usec();
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
@@ -356,6 +343,18 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// dt between this and last callback in seconds
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
#endif
ins_propagate(dt);
#ifdef USE_VEHICLE_INTERFACE
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+32
View File
@@ -106,34 +106,66 @@ static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_fc_propagate((struct Int32Rates*)gyro, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_cmpl propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_fc_propagate((struct Int32Rates*)gyro, dt);
}
#endif
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_fc_update_accel((struct Int32Vect3*)accel, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
ahrs_fc_update_accel((struct Int32Vect3*)accel, dt);
}
#endif
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_fc.status == AHRS_FC_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_fc_update_mag((struct Int32Vect3*)mag, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
if (ahrs_fc.status == AHRS_FC_RUNNING) {
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
ahrs_fc_update_mag((struct Int32Vect3*)mag, dt);
}
#endif
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+11
View File
@@ -108,12 +108,23 @@ static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_dcm.status == AHRS_DCM_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_dcm_propagate((struct Int32Rates*)gyro, dt);
}
#endif
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+12
View File
@@ -85,12 +85,24 @@ static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_mlkf_propagate((struct Int32Rates*)gyro, dt);
}
#endif
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+20
View File
@@ -87,23 +87,43 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* s
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler propagation.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_ice_update_accel((struct Int32Vect3*)accel, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_euler propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_ice.status == AHRS_ICE_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_ice_propagate((struct Int32Rates*)gyro, dt);
}
#endif
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_euler accel update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_ice.status == AHRS_ICE_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_ice_update_mag((struct Int32Vect3*)mag, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
if (ahrs_ice.status == AHRS_ICE_RUNNING) {
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
ahrs_ice_update_accel((struct Int32Rates*)accel, dt);
}
#endif
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+31
View File
@@ -124,35 +124,66 @@ static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat propagation.")
/* timestamp in usec when last callback was received */
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_icq_propagate((struct Int32Rates*)gyro, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS int_cmpl_quat propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
ahrs_icq_propagate((struct Int32Rates*)gyro, dt);
}
#endif
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_icq_update_accel((struct Int32Vect3*)accel, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
ahrs_icq_update_accel((struct Int32Rates*)accel, dt);
}
#endif
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.")
static uint32_t last_stamp = 0;
if (last_stamp > 0 && ahrs_icq.status == AHRS_ICQ_RUNNING) {
float dt = (float)(*stamp - last_stamp) * 1e-6;
ahrs_icq_update_mag((struct Int32Vect3*)mag, dt);
}
last_stamp = *stamp;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
ahrs_icq_update_mag((struct Int32Vect3*)mag, dt);
}
#endif
#endif
}