Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[ahrs] start using ABI for imu data

Browse Source
This commit is contained in:
Felix Ruess
2014-10-13 22:24:04 +02:00
parent 6b3c8cb7f8
commit 1d80eb2d7b
30 changed files with 587 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+16
View File
@@ -16,6 +16,22 @@
<field name="distance" type="float" unit="m"/>
</message>
<message name="IMU_GYRO_INT32" id="4">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates"/>
</message>
<message name="IMU_ACCEL_INT32" id="5">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="accel" type="struct Int32Vect3"/>
</message>
<message name="IMU_MAG_INT32" id="6">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="mag" type="struct Int32Vect3"/>
</message>
</msg_class>
conf/modules/ahrs_chimu_spi.xml
+1 -1
View File
@@ -20,7 +20,7 @@
<file name="imu_chimu.c"/>
<file name="ahrs.c" dir="subsystems"/>
<raw>
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
</raw>
</makefile>
</module>
conf/modules/ahrs_chimu_uart.xml
+1 -1
View File
@@ -24,7 +24,7 @@
<file name="imu_chimu.c"/>
<file name="ahrs.c" dir="subsystems"/>
<raw>
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_module.h\"
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ahrs_chimu.h\"
</raw>
</makefile>
</module>
conf/settings/estimation/ahrs_float_mlkf.xml
+3 -3
View File
@@ -4,9 +4,9 @@
<dl_settings>
<dl_settings NAME="AHRS">
<dl_setting var="ahrs_impl.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
<dl_setting var="ahrs_impl.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
<dl_setting var="ahrs_impl.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
<dl_setting var="ahrs_mlkf.mag_noise.x" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_x" param="AHRS_MAG_NOISE_X"/>
<dl_setting var="ahrs_mlkf.mag_noise.y" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_y" param="AHRS_MAG_NOISE_Y"/>
<dl_setting var="ahrs_mlkf.mag_noise.z" min="0.0" step="0.02" max="1.0" module="subsystems/ahrs/ahrs_float_mlkf" shortname="mag_noise_z" param="AHRS_MAG_NOISE_Z"/>
</dl_settings>
</dl_settings>
sw/airborne/arch/sim/sim_ahrs.c
-5
View File
@@ -15,7 +15,6 @@ float sim_psi; ///< in radians
float sim_p; ///< in radians/s
float sim_q; ///< in radians/s
float sim_r; ///< in radians/s
bool_t ahrs_sim_available;
// Updates from Ocaml sim
@@ -24,8 +23,6 @@ value provide_attitude(value phi, value theta, value psi) {
sim_theta = Double_val(theta);
sim_psi = - Double_val(psi) + M_PI/2.;
ahrs_sim_available = TRUE;
return Val_unit;
}
@@ -34,8 +31,6 @@ value provide_rates(value p, value q, value r) {
sim_q = Double_val(q);
sim_r = Double_val(r);
ahrs_sim_available = TRUE;
return Val_unit;
}
sw/airborne/boards/ardrone/actuators_at.c
+5 -5
View File
@@ -41,21 +41,21 @@ void actuators_set(pprz_t commands[]) {
float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
//Starting engine
if(thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
if(thrust > 0 && (ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED))
at_com_send_ref(REF_TAKEOFF);
//Check emergency or stop engine
if((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0)
if((ahrs_ardrone2.state & ARDRONE_EMERGENCY_MASK) != 0)
at_com_send_ref(REF_EMERGENCY);
else if(thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
else if(thrust < -0.9 && !(ahrs_ardrone2.control_state == CTRL_DEFAULT || ahrs_ardrone2.control_state == CTRL_INIT || ahrs_ardrone2.control_state == CTRL_LANDED))
at_com_send_ref(0);
//Calibration
if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
if((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING))
at_com_send_calib(0);
//Moving
if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
if((ahrs_ardrone2.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_ardrone2.control_state == CTRL_FLYING || ahrs_ardrone2.control_state == CTRL_HOVERING))
at_com_send_pcmd(1, thrust, roll, pitch, yaw);
//Keep alive (FIXME)
sw/airborne/firmwares/fixedwing/main_ap.c
+29 -49
View File
@@ -128,6 +128,11 @@ PRINT_CONFIG_VAR(MODULES_FREQUENCY)
PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#endif
#define __DefaultAhrsRegister(_x) _x ## _register()
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
#if USE_AHRS && USE_IMU
#ifdef AHRS_PROPAGATE_FREQUENCY
@@ -137,10 +142,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
#endif
#endif
#define __DefaultAhrsRegister(_x) _x ## _register()
#define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x)
#define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl)
static inline void on_gyro_event( void );
static inline void on_accel_event( void );
static inline void on_mag_event( void );
@@ -202,9 +203,13 @@ void init_ap( void ) {
#endif
#if USE_AHRS
#if defined SITL && !USE_NPS && !USE_INFRARED
ahrs_sim_init();
#else
ahrs_init();
DefaultAhrsRegister();
#endif
#endif
#if USE_AHRS && USE_IMU
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
@@ -602,9 +607,8 @@ void sensors_task( void ) {
#endif // USE_IMU
//FIXME: this is just a kludge
#if USE_AHRS && defined SITL && !USE_NPS
// dt is not really used in ahrs_sim
ahrs_propagate(&imu.gyro, 1./PERIODIC_FREQUENCY);
#if USE_AHRS && defined SITL && !USE_NPS && !USE_INFRARED
update_ahrs_from_sim();
#endif
#if USE_GPS
@@ -726,51 +730,43 @@ static inline void on_gps_solution( void ) {
#if USE_AHRS
#if USE_IMU
static inline void on_accel_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
static inline void on_accel_event(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// 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_CORRECT_FREQUENCY for AHRS accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
const float dt = 1./AHRS_CORRECT_FREQUENCY;
#endif
imu_scale_accel(&imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_accel(&imu.accel, dt);
}
AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
}
static inline void on_gyro_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
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 AHRS/INS propagation.")
PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
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
ahrs_timeout_counter = 0;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
// Run aligner on raw data as it also makes averages.
if (ahrs.status == AHRS_REGISTERED) {
if (ahrs.status != AHRS_RUNNING) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
@@ -781,8 +777,6 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
}
#endif
ahrs_propagate(&imu.gyro_prev, dt);
#if defined SITL && USE_NPS
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
@@ -796,25 +790,11 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
static inline void on_mag_event(void)
{
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// 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_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
imu_scale_mag(&imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag(&imu.mag, dt);
}
AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
#endif
}
sw/airborne/firmwares/rotorcraft/main.c
+24 -45
View File
@@ -69,7 +69,9 @@ PRINT_CONFIG_MSG_VALUE("USE_BARO_BOARD is TRUE, reading onboard baro: ", BARO_BO
#include "firmwares/rotorcraft/guidance.h"
#include "subsystems/ahrs.h"
#if USE_AHRS_ALIGNER
#include "subsystems/ahrs/ahrs_aligner.h"
#endif
#include "subsystems/ins.h"
#include "state.h"
@@ -161,7 +163,9 @@ STATIC_INLINE void main_init( void ) {
baro_init();
#endif
imu_init();
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ahrs_init();
ins_init();
@@ -313,61 +317,53 @@ STATIC_INLINE void main_event( void ) {
}
static inline void on_accel_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// dt between this and last callback
float dt = (float)(now_ts - last_ts) / 1e6;
last_ts = now_ts;
#else
PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.")
PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
#endif
imu_scale_accel(&imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_accel(&imu.accel, dt);
}
AbiSendMsgIMU_ACCEL_INT32(1, &now_ts, &imu.accel);
}
static inline void on_gyro_event( void ) {
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// 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 AHRS/INS propagation.")
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);
if (ahrs.status == AHRS_REGISTERED) {
AbiSendMsgIMU_GYRO_INT32(1, &now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
if (ahrs.status != AHRS_RUNNING) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) {
if (ahrs_align(&ahrs_aligner.lp_gyro, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag)) {
ahrs.status = AHRS_RUNNING;
}
}
return;
}
else {
ahrs_propagate(&imu.gyro_prev, dt);
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
ins_propagate(dt);
}
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
ins_propagate(dt);
#ifdef USE_VEHICLE_INTERFACE
vi_notify_imu_available();
#endif
@@ -384,27 +380,10 @@ static inline void on_gps_event(void) {
static inline void on_mag_event(void) {
imu_scale_mag(&imu);
#if USE_MAGNETOMETER
#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.")
// timestamp in usec when last callback was received
static uint32_t last_ts = 0;
// current timestamp
uint32_t now_ts = get_sys_time_usec();
// 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_MAG_CORRECT_FREQUENCY for AHRS mag update.")
PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
#endif
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag(&imu.mag, dt);
}
#endif // USE_MAGNETOMETER
AbiSendMsgIMU_MAG_INT32(1, &now_ts, &imu.mag);
#ifdef USE_VEHICLE_INTERFACE
vi_notify_mag_available();
sw/airborne/modules/ins/ahrs_chimu.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2014 Felix Ruess
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file modules/ins/ahrs_chimu.h
*/
#ifndef AHRS_CHIMU_H
#define AHRS_CHIMU_H
#include "modules/ins/ins_module.h"
#include "subsystems/ahrs.h"
#define DefaultAhrsImpl ahrs_chimu
extern void ahrs_chimu_register(void);
extern void ahrs_chimu_init(struct OrientationReps* body_to_imu);
#endif
sw/airborne/modules/ins/ahrs_chimu_spi.c
+12 -12
View File
@@ -26,6 +26,7 @@
#include "ins_module.h"
#include "imu_chimu.h"
#include "ahrs_chimu.h"
#include "led.h"
@@ -34,7 +35,14 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
void ahrs_init(void)
void ahrs_chimu_update_gps(void);
void ahrs_chimu_register(void)
{
ahrs_register_impl(ahrs_chimu_init, NULL, ahrs_chimu_update_gps);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
{
ahrs.status = AHRS_UNINIT;
@@ -68,10 +76,6 @@ void ahrs_init(void)
InsSend(rate,12);
}
void ahrs_align(void)
{
ahrs.status = AHRS_RUNNING;
}
void parse_ins_msg( void )
{
@@ -98,6 +102,8 @@ void parse_ins_msg( void )
0.
}; // FIXME rate r
stateSetBodyRates_f(&rates);
//FIXME
ahrs.status = AHRS_RUNNING;
}
else if(CHIMU_DATA.m_MsgID==0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
@@ -109,7 +115,7 @@ void parse_ins_msg( void )
}
void ahrs_update_gps(void)
void ahrs_chimu_update_gps(void)
{
// Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
@@ -130,9 +136,3 @@ void ahrs_update_gps(void)
// Downlink Send
}
void ahrs_propagate(float dt __attribute__((unused))) {
}
void ahrs_update_accel(float dt __attribute__((unused))) {
}
sw/airborne/modules/ins/ahrs_chimu_uart.c
+7 -9
View File
@@ -30,7 +30,12 @@ CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
INS_FORMAT ins_pitch_neutral;
void ahrs_init(void)
void ahrs_chimu_register(void)
{
ahrs_register_impl(ahrs_chimu_init, NULL, NULL);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
{
ahrs.status = AHRS_UNINIT;
@@ -61,10 +66,6 @@ void ahrs_init(void)
CHIMU_Checksum(rate,12);
InsSend(rate,12);
}
void ahrs_align(void)
{
ahrs.status = AHRS_RUNNING;
}
void parse_ins_msg( void )
@@ -86,6 +87,7 @@ void parse_ins_msg( void )
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
ahrs.status = AHRS_RUNNING;
#if CHIMU_DOWNLINK_IMMEDIATE
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
#endif
@@ -94,7 +96,3 @@ void parse_ins_msg( void )
}
}
}
void ahrs_update_gps( void )
{
}
sw/airborne/subsystems/ahrs.c
+40 -27
View File
@@ -27,22 +27,28 @@
#include "subsystems/ahrs.h"
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
struct Ahrs ahrs;
void ahrs_register_impl(AhrsInit init, AhrsAlign align, AhrsPropagate propagate,
AhrsUpdateAccel update_acc, AhrsUpdateMag update_mag,
void ahrs_register_impl(AhrsInit init, AhrsAlign align,
AhrsUpdateGps update_gps)
{
ahrs.init = init;
ahrs.align = align;
ahrs.propagate = propagate;
ahrs.update_accel = update_acc;
ahrs.update_mag = update_mag;
ahrs.update_gps = update_gps;
// TODO: remove hacks
#if !USE_IMU
struct OrientationReps body_to_imu;
struct FloatEulers eulers_zero = {0., 0., 0.};
orientationSetEulers_f(&body_to_imu, &eulers_zero);
ahrs.init(&body_to_imu);
#elif !defined SITL || USE_NPS
/* call init function of implementation */
ahrs.init(&imu.body_to_imu);
#endif
ahrs.status = AHRS_REGISTERED;
}
@@ -52,9 +58,6 @@ void ahrs_init(void)
ahrs.status = AHRS_UNINIT;
ahrs.init = NULL;
ahrs.align = NULL;
ahrs.propagate = NULL;
ahrs.update_accel = NULL;
ahrs.update_mag = NULL;
ahrs.update_gps = NULL;
}
@@ -67,26 +70,7 @@ bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
return FALSE;
}
void ahrs_propagate(struct Int32Rates* gyro, float dt)
{
if (ahrs.propagate != NULL && ahrs.status == AHRS_RUNNING) {
ahrs.propagate(gyro, dt);
}
}
void ahrs_update_accel(struct Int32Vect3* accel, float dt)
{
if (ahrs.update_accel != NULL && ahrs.status == AHRS_RUNNING) {
ahrs.update_accel(accel, dt);
}
}
void ahrs_update_mag(struct Int32Vect3* mag, float dt)
{
if (ahrs.update_mag != NULL && ahrs.status == AHRS_RUNNING) {
ahrs.update_mag(mag, dt);
}
}
void ahrs_update_gps(void)
{
@@ -94,3 +78,32 @@ void ahrs_update_gps(void)
ahrs.update_gps();
}
}
/*
* REMOVE ME! keep temporarily for some test firmware
*/
void ahrs_propagate(struct Int32Rates* gyro)
{
if (ahrs.status == AHRS_RUNNING) {
uint32_t stamp = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(1, &stamp, gyro);
}
}
void ahrs_update_accel(struct Int32Vect3* accel)
{
if (ahrs.status == AHRS_RUNNING) {
uint32_t stamp = get_sys_time_usec();
AbiSendMsgIMU_ACCEL_INT32(1, &stamp, accel);
}
}
void ahrs_update_mag(struct Int32Vect3* mag)
{
if (ahrs.status == AHRS_RUNNING) {
uint32_t stamp = get_sys_time_usec();
AbiSendMsgIMU_MAG_INT32(1, &stamp, mag);
}
}
sw/airborne/subsystems/ahrs.h
+10 -14
View File
@@ -44,9 +44,9 @@
typedef void (*AhrsInit)(struct OrientationReps* body_to_imu);
typedef bool_t (*AhrsAlign)(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
typedef void (*AhrsPropagate)(struct Int32Rates* gyro, float dt);
typedef void (*AhrsUpdateAccel)(struct Int32Vect3* accel, float dt);
typedef void (*AhrsUpdateMag)(struct Int32Vect3* mag, float dt);
//typedef void (*AhrsPropagate)(struct Int32Rates* gyro, float dt);
//typedef void (*AhrsUpdateAccel)(struct Int32Vect3* accel, float dt);
//typedef void (*AhrsUpdateMag)(struct Int32Vect3* mag, float dt);
typedef void (*AhrsUpdateGps)(void);
//typedef void (*AhrsUpdateGps)(struct Gps* gps);
@@ -57,17 +57,16 @@ struct Ahrs {
/* function pointers to actual implementation, set by ahrs_register_impl */
AhrsInit init;
AhrsAlign align;
AhrsPropagate propagate;
AhrsUpdateAccel update_accel;
AhrsUpdateMag update_mag;
//AhrsPropagate propagate;
//AhrsUpdateAccel update_accel;
//AhrsUpdateMag update_mag;
AhrsUpdateGps update_gps;
};
/** global AHRS state */
extern struct Ahrs ahrs;
extern void ahrs_register_impl(AhrsInit init, AhrsAlign align, AhrsPropagate propagate,
AhrsUpdateAccel update_acc, AhrsUpdateMag update_mag,
extern void ahrs_register_impl(AhrsInit init, AhrsAlign align,
AhrsUpdateGps update_gps);
/** AHRS initialization. Called at startup.
@@ -85,23 +84,20 @@ extern bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel
/** Propagation. Usually integrates the gyro rates to angles.
* Calls implementation if registered.
* @param gyro pointer to gyro measurement
* @param dt time difference since last propagation in seconds
*/
extern void ahrs_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_propagate(struct Int32Rates* gyro);
/** Update AHRS state with accerleration measurements.
* Calls implementation if registered.
* @param accel pointer to accelerometer measurement
* @param dt time difference since last update in seconds
*/
extern void ahrs_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_update_accel(struct Int32Vect3* accel);
/** Update AHRS state with magnetometer measurements.
* Calls implementation if registered.
* @param mag pointer to magnetometer measurement
* @param dt time difference since last update in seconds
*/
extern void ahrs_update_mag(struct Int32Vect3* mag, float dt);
extern void ahrs_update_mag(struct Int32Vect3* mag);
/** Update AHRS state with GPS measurements.
* Calls implementation if registered.
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+32 -37
View File
@@ -42,7 +42,7 @@
#include "subsystems/gps/gps_ardrone2.h"
#endif
struct AhrsARDrone ahrs_impl;
struct AhrsARDrone ahrs_ardrone2;
struct AhrsAligner ahrs_aligner;
unsigned char buffer[4096]; //Packet buffer
@@ -51,23 +51,28 @@ unsigned char buffer[4096]; //Packet buffer
static void send_ahrs_ad2(void) {
DOWNLINK_SEND_AHRS_ARDRONE2(DefaultChannel, DefaultDevice,
&ahrs_impl.state,
&ahrs_impl.control_state,
&ahrs_impl.eulers.phi,
&ahrs_impl.eulers.theta,
&ahrs_impl.eulers.psi,
&ahrs_impl.speed.x,
&ahrs_impl.speed.y,
&ahrs_impl.speed.z,
&ahrs_impl.accel.x,
&ahrs_impl.accel.y,
&ahrs_impl.accel.z,
&ahrs_impl.altitude,
&ahrs_impl.battery);
&ahrs_ardrone2.state,
&ahrs_ardrone2.control_state,
&ahrs_ardrone2.eulers.phi,
&ahrs_ardrone2.eulers.theta,
&ahrs_ardrone2.eulers.psi,
&ahrs_ardrone2.speed.x,
&ahrs_ardrone2.speed.y,
&ahrs_ardrone2.speed.z,
&ahrs_ardrone2.accel.x,
&ahrs_ardrone2.accel.y,
&ahrs_ardrone2.accel.z,
&ahrs_ardrone2.altitude,
&ahrs_ardrone2.battery);
}
#endif
void ahrs_init(void) {
void ahrs_ardrone2_register(void)
{
ahrs_register_impl(ahrs_ardrone2_init, NULL, NULL);
}
void ahrs_ardrone2_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
init_at_com();
//Set navdata_demo to FALSE and flat trim the ar drone
@@ -81,10 +86,6 @@ void ahrs_init(void) {
#endif
}
void ahrs_align(void) {
}
#ifdef ARDRONE2_DEBUG
static void dump(const void *_b, size_t s) {
const unsigned char *b = _b;
@@ -100,7 +101,7 @@ static void dump(const void *_b, size_t s) {
}
#endif
void ahrs_propagate(float dt __attribute__((unused))) {
void ahrs_ardrone2_propagate(void) {
int l;
//Recieve the main packet
@@ -122,7 +123,7 @@ void ahrs_propagate(float dt __attribute__((unused))) {
#endif
//Set the state
ahrs_impl.state = main_packet->ardrone_state;
ahrs_ardrone2.state = main_packet->ardrone_state;
//Init the option
navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
@@ -144,15 +145,15 @@ void ahrs_propagate(float dt __attribute__((unused))) {
navdata_demo = (navdata_demo_t*) navdata_option;
//Set the AHRS state
ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
ahrs_impl.eulers.phi = navdata_demo->phi;
ahrs_impl.eulers.theta = navdata_demo->theta;
ahrs_impl.eulers.psi = navdata_demo->psi;
ahrs_impl.speed.x = navdata_demo->vx / 1000;
ahrs_impl.speed.y = navdata_demo->vy / 1000;
ahrs_impl.speed.z = navdata_demo->vz / 1000;
ahrs_impl.altitude = navdata_demo->altitude / 10;
ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
ahrs_ardrone2.eulers.phi = navdata_demo->phi;
ahrs_ardrone2.eulers.theta = navdata_demo->theta;
ahrs_ardrone2.eulers.psi = navdata_demo->psi;
ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;
//Set the ned to body eulers
struct FloatEulers angles;
@@ -168,7 +169,7 @@ void ahrs_propagate(float dt __attribute__((unused))) {
navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
//Set the AHRS accel state
INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
break;
#ifdef USE_GPS_ARDRONE2
case 27: //NAVDATA_GPS
@@ -194,9 +195,3 @@ void ahrs_propagate(float dt __attribute__((unused))) {
navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
}
}
void ahrs_aligner_init(void) {
}
void ahrs_aligner_run(void) {
}
sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
+8 -1
View File
@@ -34,6 +34,7 @@
#include "subsystems/ahrs/ahrs_aligner.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_geodetic_float.h"
struct AhrsARDrone {
uint32_t state; // ARDRONE_STATES
@@ -45,6 +46,12 @@ struct AhrsARDrone {
uint32_t battery; // in percentage
struct Int32Quat ltp_to_imu_quat;
};
extern struct AhrsARDrone ahrs_impl;
extern struct AhrsARDrone ahrs_ardrone2;
#define DefaultAhrsImpl ahrs_ardrone2
extern void ahrs_ardrone2_register(void);
extern void ahrs_ardrone2_init(struct OrientationReps* body_to_imu);
extern void ahrs_ardrone2_propagate(void);
#endif /* AHRS_ARDRONE2_H */
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+57 -2
View File
@@ -38,6 +38,7 @@
#include "subsystems/gps.h"
#endif
#include "state.h"
#include "subsystems/abi.h"
//#include "../../test/pprz_algebra_print.h"
@@ -89,6 +90,50 @@ static inline void compute_body_orientation_and_rates(void);
struct AhrsFloatCmpl ahrs_fc;
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_FC_IMU_ID
#define AHRS_FC_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
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;
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
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;
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
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;
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -141,8 +186,7 @@ static void send_rmat(void) {
void ahrs_fc_register(void)
{
ahrs_register_impl(ahrs_fc_init, ahrs_fc_align, ahrs_fc_propagate,
ahrs_fc_update_accel, ahrs_fc_update_mag, ahrs_fc_update_gps);
ahrs_register_impl(ahrs_fc_init, ahrs_fc_align, ahrs_fc_update_gps);
}
void ahrs_fc_init(struct OrientationReps* body_to_imu)
@@ -150,6 +194,8 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu)
/* save body_to_imu pointer */
ahrs_fc.body_to_imu = body_to_imu;
ahrs_fc.status = AHRS_FC_UNINIT;
ahrs_fc.ltp_vel_norm_valid = FALSE;
ahrs_fc.heading_aligned = FALSE;
@@ -180,6 +226,13 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu)
ahrs_fc.accel_cnt = 0;
ahrs_fc.mag_cnt = 0;
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
@@ -211,6 +264,8 @@ bool_t ahrs_fc_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0);
ahrs_fc.status = AHRS_FC_RUNNING;
return TRUE;
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+7
View File
@@ -32,6 +32,11 @@
#include "std.h"
enum AhrsFCStatus {
AHRS_FC_UNINIT,
AHRS_FC_RUNNING
};
struct AhrsFloatCmpl {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
@@ -62,6 +67,8 @@ struct AhrsFloatCmpl {
uint16_t mag_cnt; ///< number of propagations since last mag update
struct OrientationReps* body_to_imu;
enum AhrsFCStatus status;
};
extern struct AhrsFloatCmpl ahrs_fc;
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+55 -4
View File
@@ -36,6 +36,7 @@
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "subsystems/abi.h"
#if USE_GPS
#include "subsystems/gps.h"
@@ -92,6 +93,46 @@ float imu_health = 0.;
#endif
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_DCM_IMU_ID
#define AHRS_DCM_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
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;
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t* stamp __attribute__((unused)),
const struct Int32Vect3* accel)
{
if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
ahrs_dcm_update_accel((struct Int32Vect3*)accel);
}
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t* stamp __attribute__((unused)),
const struct Int32Vect3* mag)
{
if (ahrs_dcm.status == AHRS_DCM_RUNNING) {
ahrs_dcm_update_mag((struct Int32Vect3*)mag);
}
}
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{
for (int i=0; i<3; i++) {
@@ -103,8 +144,7 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
void ahrs_dcm_register(void)
{
ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_align, ahrs_dcm_propagate,
ahrs_dcm_update_accel, ahrs_dcm_update_mag, ahrs_dcm_update_gps);
ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_align, ahrs_dcm_update_gps);
}
void ahrs_dcm_init(struct OrientationReps* body_to_imu)
@@ -112,6 +152,8 @@ void ahrs_dcm_init(struct OrientationReps* body_to_imu)
/* save body_to_imu pointer */
ahrs_dcm.body_to_imu = body_to_imu;
ahrs_dcm.status = AHRS_DCM_UNINIT;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(ahrs_dcm.body_to_imu),
sizeof(struct FloatEulers));
@@ -126,6 +168,13 @@ void ahrs_dcm_init(struct OrientationReps* body_to_imu)
ahrs_dcm.gps_course = 0;
ahrs_dcm.gps_course_valid = FALSE;
ahrs_dcm.gps_age = 100;
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_DCM_IMU_ID, &mag_ev, mag_cb);
}
bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
@@ -149,6 +198,8 @@ bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);
ahrs_dcm.status = AHRS_DCM_RUNNING;
return TRUE;
}
@@ -209,7 +260,7 @@ void ahrs_dcm_update_gps(void)
}
void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused)))
void ahrs_dcm_update_accel(struct Int32Vect3* accel)
{
ACCELS_FLOAT_OF_BFP(accel_float, *accel);
@@ -240,7 +291,7 @@ PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
}
void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused)))
void ahrs_dcm_update_mag(struct Int32Vect3* mag)
{
#if USE_MAGNETOMETER
#warning MAGNETOMETER FEEDBACK NOT TESTED YET
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+9 -2
View File
@@ -27,6 +27,11 @@
#include <inttypes.h>
#include "math/pprz_algebra_float.h"
enum AhrsDCMStatus {
AHRS_DCM_UNINIT,
AHRS_DCM_RUNNING
};
struct AhrsFloatDCM {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
@@ -41,6 +46,8 @@ struct AhrsFloatDCM {
uint8_t gps_age;
struct OrientationReps* body_to_imu;
enum AhrsDCMStatus status;
};
extern struct AhrsFloatDCM ahrs_dcm;
@@ -77,8 +84,8 @@ extern void ahrs_dcm_init(struct OrientationReps* body_to_imu);
extern bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
extern void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt);
extern void ahrs_dcm_update_accel(struct Int32Vect3* accel);
extern void ahrs_dcm_update_mag(struct Int32Vect3* mag);
extern void ahrs_dcm_update_gps(void);
#endif // AHRS_FLOAT_DCM_H
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+56 -4
View File
@@ -42,6 +42,8 @@
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"
#include "subsystems/abi.h"
//#include <stdio.h>
#ifndef AHRS_MAG_NOISE_X
@@ -67,10 +69,49 @@ static void send_geo_mag(void) {
}
#endif
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_MLKF_IMU_ID
#define AHRS_MLKF_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
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;
}
static void accel_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t* stamp __attribute__((unused)),
const struct Int32Vect3* accel)
{
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
ahrs_mlkf_update_accel((struct Int32Vect3*)accel);
}
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t* stamp __attribute__((unused)),
const struct Int32Vect3* mag)
{
if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
ahrs_mlkf_update_mag((struct Int32Vect3*)mag);
}
}
void ahrs_mlkf_register(void)
{
ahrs_register_impl(ahrs_mlkf_init, ahrs_mlkf_align, ahrs_mlkf_propagate,
ahrs_mlkf_update_accel, ahrs_mlkf_update_mag, NULL);
ahrs_register_impl(ahrs_mlkf_init, ahrs_mlkf_align, NULL);
}
void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
@@ -78,6 +119,8 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
/* save body_to_imu pointer */
ahrs_mlkf.body_to_imu = body_to_imu;
ahrs_mlkf.status = AHRS_MLKF_UNINIT;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_mlkf.ltp_to_imu_quat, orientationGetQuat_f(ahrs_mlkf.body_to_imu),
sizeof(struct FloatQuat));
@@ -102,6 +145,13 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
VECT3_ASSIGN(ahrs_mlkf.mag_noise, AHRS_MAG_NOISE_X, AHRS_MAG_NOISE_Y, AHRS_MAG_NOISE_Z);
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
#endif
@@ -122,6 +172,8 @@ bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_mlkf.gyro_bias, bias0);
ahrs_mlkf.status = AHRS_MLKF_RUNNING;
return TRUE;
}
@@ -131,7 +183,7 @@ void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt) {
set_body_state_from_quat();
}
void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused))) {
void ahrs_mlkf_update_accel(struct Int32Vect3* accel) {
struct FloatVect3 imu_g;
ACCELS_FLOAT_OF_BFP(imu_g, *accel);
const float alpha = 0.92;
@@ -145,7 +197,7 @@ void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt __attribute__((un
}
void ahrs_mlkf_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused))) {
void ahrs_mlkf_update_mag(struct Int32Vect3* mag) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, *mag);
update_state(&ahrs_mlkf.mag_h, &imu_h, &ahrs_mlkf.mag_noise);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+9 -2
View File
@@ -36,6 +36,11 @@
#include "math/pprz_orientation_conversion.h"
#include "subsystems/ahrs.h"
enum AhrsMlkfStatus {
AHRS_MLKF_UNINIT,
AHRS_MLKF_RUNNING
};
struct AhrsMlkf {
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
@@ -52,6 +57,8 @@ struct AhrsMlkf {
/** pointer to body_to_imu rotation */
struct OrientationReps* body_to_imu;
enum AhrsMlkfStatus status;
};
extern struct AhrsMlkf ahrs_mlkf;
@@ -63,7 +70,7 @@ extern void ahrs_mlkf_init(struct OrientationReps* body_to_imu);
extern bool_t ahrs_mlkf_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
extern void ahrs_mlkf_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_mlkf_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_mlkf_update_mag(struct Int32Vect3* mag, float dt);
extern void ahrs_mlkf_update_accel(struct Int32Vect3* accel);
extern void ahrs_mlkf_update_mag(struct Int32Vect3* mag);
#endif /* AHRS_FLOAT_MLKF_H */
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+27 -26
View File
@@ -36,9 +36,26 @@
#include "subsystems/gps.h"
#include "state.h"
#include "subsystems/abi.h"
float heading;
/** ABI binding for gyro data.
* Used for gyro ABI messages.
*/
#ifndef AHRS_INFRARED_GYRO_ID
#define AHRS_INFRARED_GYRO_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* gyro)
{
stateSetBodyRates_i((struct Int32Rates*)gyro);
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -55,40 +72,25 @@ static void send_status(void) {
}
#endif
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
void ahrs_infrared_register(void)
{
ahrs_register_impl(ahrs_infrared_init, NULL, ahrs_infrared_update_gps);
}
void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
ahrs.status = AHRS_RUNNING;
heading = 0.;
AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
#endif
}
void ahrs_align(void) {
//TODO set gyro bias if used
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(float dt __attribute__((unused))) {
struct FloatRates body_rate = { 0., 0., 0. };
#ifdef ADC_CHANNEL_GYRO_P
body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
#endif
#ifdef ADC_CHANNEL_GYRO_Q
body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
#endif
#ifdef ADC_CHANNEL_GYRO_R
body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
#endif
stateSetBodyRates_f(&body_rate);
}
void ahrs_update_gps(void) {
void ahrs_infrared_update_gps(void) {
float hspeed_mod_f = gps.gspeed / 100.;
float course_f = gps.course / 1e7;
@@ -120,4 +122,3 @@ void ahrs_update_infrared(void) {
stateSetNedToBodyEulers_f(&att);
}
sw/airborne/subsystems/ahrs/ahrs_infrared.h
+7 -1
View File
@@ -29,9 +29,15 @@
#ifndef AHRS_INFRARED_H
#define AHRS_INFRARED_H
#include "subsystems/ahrs.h"
#include "std.h"
#include "subsystems/ahrs.h"
#include "math/pprz_orientation_conversion.h"
extern void ahrs_infrared_register(void);
extern void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused)));
extern void ahrs_update_infrared(void);
extern void ahrs_infrared_update_gps(void);
#define DefaultAhrsImpl ahrs_infrared
#endif /* AHRS_INFRARED_H */
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+57 -2
View File
@@ -31,6 +31,7 @@
#include "ahrs_int_cmpl_euler.h"
#include "state.h"
#include "subsystems/abi.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
@@ -59,6 +60,50 @@ static inline void set_body_state_from_euler(void);
while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
}
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_ICE_IMU_ID
#define AHRS_ICE_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
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_propagate((struct Int32Rates*)gyro, dt);
}
last_stamp = *stamp;
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
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;
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
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;
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -100,8 +145,7 @@ static void send_bias(void) {
void ahrs_ice_register(void)
{
ahrs_register_impl(ahrs_ice_init, ahrs_ice_align, ahrs_ice_propagate,
ahrs_ice_update_accel, ahrs_ice_update_mag, NULL);
ahrs_register_impl(ahrs_ice_init, ahrs_ice_align, NULL);
}
void ahrs_ice_init(struct OrientationReps* body_to_imu)
@@ -109,6 +153,8 @@ void ahrs_ice_init(struct OrientationReps* body_to_imu)
/* save body_to_imu pointer */
ahrs_ice.body_to_imu = body_to_imu;
ahrs_ice.status = AHRS_ICE_UNINIT;
/* set ltp_to_imu so that body is zero */
memcpy(&ahrs_ice.ltp_to_imu_euler, orientationGetEulers_i(ahrs_ice.body_to_imu),
sizeof(struct Int32Eulers));
@@ -119,6 +165,13 @@ void ahrs_ice_init(struct OrientationReps* body_to_imu)
ahrs_ice.mag_offset = AHRS_MAG_OFFSET;
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_ICE_IMU_ID, &mag_ev, mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter);
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
@@ -144,6 +197,8 @@ bool_t ahrs_ice_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
ahrs_ice.status = AHRS_ICE_RUNNING;
return TRUE;
}
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+7
View File
@@ -34,6 +34,11 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
enum AhrsICEStatus {
AHRS_ICE_UNINIT,
AHRS_ICE_RUNNING
};
struct AhrsIntCmplEuler {
struct Int32Rates gyro_bias;
struct Int32Rates imu_rate;
@@ -46,6 +51,8 @@ struct AhrsIntCmplEuler {
float mag_offset;
struct OrientationReps* body_to_imu;
enum AhrsICEStatus status;
};
extern struct AhrsIntCmplEuler ahrs_ice;
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+58 -3
View File
@@ -32,7 +32,7 @@
#include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/abi.h"
#include "state.h"
#if USE_GPS
@@ -109,6 +109,51 @@ static inline void set_body_state_from_quat(void);
static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3* mag, float dt);
static inline void ahrs_icq_update_mag_2d(struct Int32Vect3* mag, float dt);
/** ABI binding for IMU data.
* Used for gyro, accel and mag ABI messages.
*/
#ifndef AHRS_ICQ_IMU_ID
#define AHRS_ICQ_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
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;
}
static void accel_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* accel)
{
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;
}
static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Vect3* mag)
{
#if USE_MAGNETOMETER
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;
#endif
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -156,8 +201,7 @@ static void send_geo_mag(void) {
void ahrs_icq_register(void)
{
ahrs_register_impl(ahrs_icq_init, ahrs_icq_align, ahrs_icq_propagate,
ahrs_icq_update_accel, ahrs_icq_update_mag, ahrs_icq_update_gps);
ahrs_register_impl(ahrs_icq_init, ahrs_icq_align, ahrs_icq_update_gps);
}
@@ -166,6 +210,8 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) {
/* save body_to_imu pointer */
ahrs_icq.body_to_imu = body_to_imu;
ahrs_icq.status = AHRS_ICQ_UNINIT;
ahrs_icq.ltp_vel_norm_valid = FALSE;
ahrs_icq.heading_aligned = FALSE;
@@ -201,6 +247,13 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) {
ahrs_icq.accel_cnt = 0;
ahrs_icq.mag_cnt = 0;
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_IMU_ID, &mag_ev, mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
@@ -235,6 +288,8 @@ bool_t ahrs_icq_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro);
INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28);
ahrs_icq.status = AHRS_ICQ_RUNNING;
return TRUE;
}
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.h
+7
View File
@@ -35,6 +35,11 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
enum AhrsICQStatus {
AHRS_ICQ_UNINIT,
AHRS_ICQ_RUNNING
};
/**
* Ahrs implementation specifc values
*/
@@ -92,6 +97,8 @@ struct AhrsIntCmplQuat {
uint16_t mag_cnt; ///< number of propagations since last mag update
struct OrientationReps* body_to_imu;
enum AhrsICQStatus status; ///< status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
};
extern struct AhrsIntCmplQuat ahrs_icq;
sw/airborne/subsystems/ahrs/ahrs_sim.c
+2 -25
View File
@@ -30,6 +30,7 @@
#include "subsystems/ahrs/ahrs_sim.h"
#include "math/pprz_algebra_float.h"
#include "generated/airframe.h"
#include "state.h"
extern float sim_phi;
extern float sim_theta;
@@ -37,7 +38,6 @@ extern float sim_psi;
extern float sim_p;
extern float sim_q;
extern float sim_r;
extern bool_t ahrs_sim_available;
// for sim of e.g. Xsens ins
#ifndef INS_ROLL_NEUTRAL_DEFAULT
@@ -61,31 +61,8 @@ void update_ahrs_from_sim(void) {
}
void ahrs_init(void) {
void ahrs_sim_init(void) {
//ahrs_float.status = AHRS_UNINIT;
// set to running for now
ahrs.status = AHRS_RUNNING;
ahrs_sim_available = FALSE;
}
void ahrs_align(void)
{
/* Currently not really simulated
* body and imu have the same frame and always set to true value from sim
*/
update_ahrs_from_sim();
ahrs.status = AHRS_RUNNING;
}
void ahrs_propagate(float dt __attribute__((unused))) {
if (ahrs_sim_available) {
update_ahrs_from_sim();
ahrs_sim_available = FALSE;
}
}
sw/airborne/subsystems/ahrs/ahrs_sim.h
+1 -2
View File
@@ -32,11 +32,10 @@
#include "subsystems/ahrs.h"
#include "std.h"
extern bool_t ahrs_sim_available;
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
extern void update_ahrs_from_sim(void);
extern void ahrs_sim_init(void);
#endif /* AHRS_SIM_H */
sw/airborne/subsystems/ins/ins_ardrone2.c
+3 -3
View File
@@ -92,8 +92,8 @@ void ins_reset_altitude_ref( void ) {
void ins_propagate(float __attribute__((unused)) dt) {
/* untilt accels and speeds */
float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_impl.accel);
float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_impl.speed);
float_rmat_transp_vmult(&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.accel);
float_rmat_transp_vmult(&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), &ahrs_ardrone2.speed);
//Add g to the accelerations
ins_impl.ltp_accel.z += 9.81;
@@ -105,7 +105,7 @@ void ins_propagate(float __attribute__((unused)) dt) {
//Don't set the height if we use the one from the gps
#if !USE_GPS_HEIGHT
//Set the height and save the position
ins_impl.ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
ins_impl.ltp_pos.z = -(ahrs_ardrone2.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
stateSetPositionNed_i(&ins_impl.ltp_pos);
#endif
}