Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 07:56:34 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[ahrs] send aligner data via ABI

Browse Source
This commit is contained in:
Felix Ruess
2014-11-05 00:10:26 +01:00
parent f1f447372e
commit 750d06aa7a
17 changed files with 97 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+7
View File
@@ -35,6 +35,13 @@
<field name="mag" type="struct Int32Vect3"/>
</message>
<message name="IMU_LOWPASSED" id="7">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates"/>
<field name="accel" type="struct Int32Vect3"/>
<field name="mag" type="struct Int32Vect3"/>
</message>
</msg_class>
sw/airborne/firmwares/fixedwing/main_ap.c
+1 -3
View File
@@ -767,9 +767,7 @@ static inline void on_gyro_event(void)
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;
}
ahrs.status = AHRS_RUNNING;
}
return;
}
sw/airborne/firmwares/rotorcraft/main.c
+1 -3
View File
@@ -348,9 +348,7 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
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;
}
ahrs.status = AHRS_RUNNING;
}
return;
}
sw/airborne/modules/ins/ahrs_chimu_spi.c
+1 -1
View File
@@ -39,7 +39,7 @@ void ahrs_chimu_update_gps(void);
void ahrs_chimu_register(void)
{
ahrs_register_impl(ahrs_chimu_init, NULL, ahrs_chimu_update_gps);
ahrs_register_impl(ahrs_chimu_init, ahrs_chimu_update_gps);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
sw/airborne/modules/ins/ahrs_chimu_uart.c
+1 -1
View File
@@ -32,7 +32,7 @@ INS_FORMAT ins_pitch_neutral;
void ahrs_chimu_register(void)
{
ahrs_register_impl(ahrs_chimu_init, NULL, NULL);
ahrs_register_impl(ahrs_chimu_init, NULL);
}
void ahrs_chimu_init(struct OrientationReps* body_to_imu __attribute__((unused)))
sw/airborne/subsystems/ahrs.c
+1 -15
View File
@@ -32,11 +32,9 @@
struct Ahrs ahrs;
void ahrs_register_impl(AhrsInit init, AhrsAlign align,
AhrsUpdateGps update_gps)
void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps)
{
ahrs.init = init;
ahrs.align = align;
ahrs.update_gps = update_gps;
// TODO: remove hacks
@@ -57,21 +55,9 @@ void ahrs_init(void)
{
ahrs.status = AHRS_UNINIT;
ahrs.init = NULL;
ahrs.align = NULL;
ahrs.update_gps = NULL;
}
bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag)
{
if (ahrs.align != NULL) {
return ahrs.align(lp_gyro, lp_accel, lp_mag);
}
return FALSE;
}
void ahrs_update_gps(void)
{
if (ahrs.update_gps != NULL && ahrs.status == AHRS_RUNNING) {
sw/airborne/subsystems/ahrs.h
+1 -37
View File
@@ -42,13 +42,7 @@
#endif
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 (*AhrsUpdateGps)(void);
//typedef void (*AhrsUpdateGps)(struct Gps* gps);
/** Attitude and Heading Reference System state */
struct Ahrs {
@@ -56,49 +50,19 @@ struct Ahrs {
/* function pointers to actual implementation, set by ahrs_register_impl */
AhrsInit init;
AhrsAlign align;
//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,
AhrsUpdateGps update_gps);
extern void ahrs_register_impl(AhrsInit init, AhrsUpdateGps update_gps);
/** AHRS initialization. Called at startup.
* Initialized the global AHRS struct.
*/
extern void ahrs_init(void);
/** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases.
* Calls implementation if registered.
* @return TRUE if ahrs is aligned
*/
extern bool_t ahrs_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
/** Propagation. Usually integrates the gyro rates to angles.
* Calls implementation if registered.
* @param gyro pointer to gyro measurement
*/
extern void ahrs_propagate(struct Int32Rates* gyro);
/** Update AHRS state with accerleration measurements.
* Calls implementation if registered.
* @param accel pointer to accelerometer measurement
*/
extern void ahrs_update_accel(struct Int32Vect3* accel);
/** Update AHRS state with magnetometer measurements.
* Calls implementation if registered.
* @param mag pointer to magnetometer measurement
*/
extern void ahrs_update_mag(struct Int32Vect3* mag);
/** Update AHRS state with GPS measurements.
* Calls implementation if registered.
* Reads the global #gps data struct.
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+5
View File
@@ -31,6 +31,8 @@
#include <stdlib.h> /* for abs() */
#include "subsystems/imu.h"
#include "led.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
struct AhrsAligner ahrs_aligner;
@@ -129,6 +131,9 @@ void ahrs_aligner_run(void) {
#ifdef AHRS_ALIGNER_LED
LED_ON(AHRS_ALIGNER_LED);
#endif
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_LOWPASSED(ABI_BROADCAST, &now_ts, &ahrs_aligner.lp_gyro,
&ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
}
}
sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+1 -2
View File
@@ -43,7 +43,6 @@
#endif
struct AhrsARDrone ahrs_ardrone2;
struct AhrsAligner ahrs_aligner;
unsigned char buffer[4096]; //Packet buffer
#if PERIODIC_TELEMETRY
@@ -69,7 +68,7 @@ static void send_ahrs_ad2(void) {
void ahrs_ardrone2_register(void)
{
ahrs_register_impl(ahrs_ardrone2_init, NULL, NULL);
ahrs_register_impl(ahrs_ardrone2_init, NULL);
}
void ahrs_ardrone2_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
sw/airborne/subsystems/ahrs/ahrs_ardrone2.h
-1
View File
@@ -31,7 +31,6 @@
#define AHRS_ARDRONE2_H
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_geodetic_float.h"
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+15 -1
View File
@@ -101,6 +101,8 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
@@ -134,6 +136,17 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* st
last_stamp = *stamp;
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
const struct Int32Vect3* lp_mag)
{
if (ahrs_fc.status != AHRS_FC_RUNNING) {
ahrs_fc_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
(struct Int32Vect3*)lp_mag);
}
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -186,7 +199,7 @@ static void send_rmat(void) {
void ahrs_fc_register(void)
{
ahrs_register_impl(ahrs_fc_init, ahrs_fc_align, ahrs_fc_update_gps);
ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps);
}
void ahrs_fc_init(struct OrientationReps* body_to_imu)
@@ -232,6 +245,7 @@ void ahrs_fc_init(struct OrientationReps* body_to_imu)
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);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+15 -1
View File
@@ -103,6 +103,8 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
@@ -132,6 +134,17 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
const struct Int32Vect3* lp_mag)
{
if (ahrs_dcm.status != AHRS_DCM_RUNNING) {
ahrs_dcm_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
(struct Int32Vect3*)lp_mag);
}
}
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{
@@ -144,7 +157,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_update_gps);
ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_update_gps);
}
void ahrs_dcm_init(struct OrientationReps* body_to_imu)
@@ -175,6 +188,7 @@ void ahrs_dcm_init(struct OrientationReps* body_to_imu)
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);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
}
bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+16 -1
View File
@@ -80,6 +80,8 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
@@ -109,9 +111,21 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)),
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
const struct Int32Vect3* lp_mag)
{
if (ahrs_mlkf.status != AHRS_MLKF_RUNNING) {
ahrs_mlkf_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
(struct Int32Vect3*)lp_mag);
}
}
void ahrs_mlkf_register(void)
{
ahrs_register_impl(ahrs_mlkf_init, ahrs_mlkf_align, NULL);
ahrs_register_impl(ahrs_mlkf_init, NULL);
}
void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
@@ -151,6 +165,7 @@ void ahrs_mlkf_init(struct OrientationReps* body_to_imu) {
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);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+1 -1
View File
@@ -74,7 +74,7 @@ static void send_status(void) {
void ahrs_infrared_register(void)
{
ahrs_register_impl(ahrs_infrared_init, NULL, ahrs_infrared_update_gps);
ahrs_register_impl(ahrs_infrared_init, ahrs_infrared_update_gps);
}
void ahrs_infrared_init(struct OrientationReps* body_to_imu __attribute__((unused))) {
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+15 -1
View File
@@ -71,6 +71,8 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
@@ -104,6 +106,17 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* st
last_stamp = *stamp;
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
const struct Int32Vect3* lp_mag)
{
if (ahrs_ice.status != AHRS_ICE_RUNNING) {
ahrs_ice_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
(struct Int32Vect3*)lp_mag);
}
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -145,7 +158,7 @@ static void send_bias(void) {
void ahrs_ice_register(void)
{
ahrs_register_impl(ahrs_ice_init, ahrs_ice_align, NULL);
ahrs_register_impl(ahrs_ice_init, NULL);
}
void ahrs_ice_init(struct OrientationReps* body_to_imu)
@@ -171,6 +184,7 @@ void ahrs_ice_init(struct OrientationReps* body_to_imu)
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);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+15 -1
View File
@@ -119,6 +119,8 @@ static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* stamp,
const struct Int32Rates* gyro)
{
@@ -154,6 +156,17 @@ static void mag_cb(uint8_t __attribute__((unused)) sender_id, const uint32_t* st
#endif
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
const uint32_t* stamp __attribute__((unused)),
const struct Int32Rates* lp_gyro, const struct Int32Vect3* lp_accel,
const struct Int32Vect3* lp_mag)
{
if (ahrs_icq.status != AHRS_ICQ_RUNNING) {
ahrs_icq_align((struct Int32Rates*)lp_gyro, (struct Int32Vect3*)lp_accel,
(struct Int32Vect3*)lp_mag);
}
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -201,7 +214,7 @@ static void send_geo_mag(void) {
void ahrs_icq_register(void)
{
ahrs_register_impl(ahrs_icq_init, ahrs_icq_align, ahrs_icq_update_gps);
ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps);
}
@@ -253,6 +266,7 @@ void ahrs_icq_init(struct OrientationReps* body_to_imu) {
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);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
sw/airborne/test/subsystems/test_ahrs.c
+1 -3
View File
@@ -100,9 +100,7 @@ static inline void on_gyro_event(void) {
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;
}
ahrs.status = AHRS_RUNNING;
}
return;
}