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 30 Packages Projects Releases Wiki Activity

[abi] send GEO_MAG via ABI

Browse Source
This commit is contained in:
Felix Ruess
2015-03-03 14:10:39 +01:00
parent 9c61c34ab2
commit 9b95038631
12 changed files with 49 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+4 -1
View File
@@ -46,7 +46,10 @@
<field name="q_b2i_f" type="struct FloatQuat *"/>
</message>
<message name="GEO_MAG" id="9">
<field name="h" type="struct FloatVect3 *" unit="1.0"/>
</message>
</msg_class>
</protocol>
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 0
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/rotorcraft/ins_float_invariant.makefile
-2
View File
@@ -7,8 +7,6 @@ USE_MAGNETOMETER ?= 1
INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant_wrapper.h\"
INS_CFLAGS += -DUSE_AHRS_ALIGNER
INS_CFLAGS += -DUSE_AHRS
# for geo mag
INS_CFLAGS += -DAHRS_FLOAT
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
INS_CFLAGS += -DUSE_MAGNETOMETER
conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
+1 -1
View File
@@ -8,7 +8,7 @@
USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
+1 -1
View File
@@ -7,7 +7,7 @@
USE_MAGNETOMETER ?= 1
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
+1 -1
View File
@@ -5,7 +5,7 @@
USE_MAGNETOMETER ?= 1
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
sw/airborne/modules/geo_mag/geo_mag.c
+12 -10
View File
@@ -29,9 +29,14 @@
#include "math/pprz_geodetic_wmm2010.h"
#include "math/pprz_algebra_double.h"
#include "subsystems/gps.h"
#include "subsystems/abi.h"
// for kill_throttle check
#include "autopilot.h"
#include "subsystems/ahrs.h"
#ifndef GEO_MAG_SENDER_ID
#define GEO_MAG_SENDER_ID 1
#endif
bool_t geo_mag_calc_flag;
struct GeoMag geo_mag;
@@ -72,16 +77,13 @@ void geo_mag_event(void)
mag_calc(1, latitude, longitude, alt, nmax, gha,
&geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
double_vect3_normalize(&geo_mag.vect);
// copy to ahrs
#ifdef AHRS_FLOAT
VECT3_COPY(DefaultAhrsImpl.mag_h, geo_mag.vect);
#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(DefaultAhrsImpl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x),
MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
// send as normalized float vector via ABI
struct FloatVect3 h = { .x = geo_mag.vect.x,
.y = geo_mag.vect.y,
.z = geo_mag.vect.z };
float_vect3_normalize(&h);
AbiSendMsgGEO_MAG(GEO_MAG_SENDER_ID, &h);
geo_mag.ready = TRUE;
}
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+7
View File
@@ -67,6 +67,7 @@ static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
@@ -153,6 +154,11 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_fc_set_body_to_imu_quat(q_b2i_f);
}
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
memcpy(&ahrs_fc.mag_h, h, sizeof(struct FloatVect3));
}
void ahrs_fc_register(void)
{
ahrs_register_impl(ahrs_fc_init, ahrs_fc_update_gps);
@@ -165,6 +171,7 @@ void ahrs_fc_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_FC_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+7
View File
@@ -51,6 +51,7 @@ static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
@@ -111,6 +112,11 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_mlkf_set_body_to_imu_quat(q_b2i_f);
}
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
memcpy(&ahrs_mlkf.mag_h, h, sizeof(struct FloatVect3));
}
void ahrs_mlkf_register(void)
{
ahrs_register_impl(ahrs_mlkf_init, NULL);
@@ -123,6 +129,7 @@ void ahrs_mlkf_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
+8
View File
@@ -89,6 +89,7 @@ static abi_event accel_ev;
static abi_event mag_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
static abi_event geo_mag_ev;
static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
@@ -175,6 +176,12 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ahrs_icq_set_body_to_imu_quat(q_b2i_f);
}
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y),
MAG_BFP_OF_REAL(h->z));
}
void ahrs_icq_register(void)
{
ahrs_register_impl(ahrs_icq_init, ahrs_icq_update_gps);
@@ -187,6 +194,7 @@ void ahrs_icq_register(void)
AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_IMU_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "AHRS_QUAT_INT", send_quat);
sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
+6
View File
@@ -76,6 +76,7 @@ static abi_event baro_ev;
static abi_event mag_ev;
static abi_event gyro_ev;
static abi_event aligner_ev;
static abi_event geo_mag_ev;
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
@@ -115,6 +116,10 @@ static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
}
}
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
memcpy(&ins_float_inv.mag_h, h, sizeof(struct FloatVect3));
}
void ins_float_inv_register(void)
@@ -126,6 +131,7 @@ void ins_float_inv_register(void)
AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);