[abi] send GEO_MAG via ABI

paparazzi · Mar 10, 2015 · 9b95038 · 9b95038
1 parent 9c61c34
commit 9b95038
Show file tree

Hide file tree

Showing 12 changed files with 49 additions and 18 deletions.
diff --git a/conf/abi.xml b/conf/abi.xml
@@ -46,7 +46,10 @@
       <field name="q_b2i_f" type="struct FloatQuat *"/>
     </message>
 
-  </msg_class>
+    <message name="GEO_MAG" id="9">
+      <field name="h" type="struct FloatVect3 *" unit="1.0"/>
+    </message>
 
+  </msg_class>
 
 </protocol>
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
@@ -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))

diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
@@ -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))

diff --git a/conf/firmwares/subsystems/rotorcraft/ins_float_invariant.makefile b/conf/firmwares/subsystems/rotorcraft/ins_float_invariant.makefile
@@ -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

diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
@@ -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))

diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
@@ -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))

diff --git a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
@@ -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))

diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -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;
   }

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
@@ -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);

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
@@ -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);

diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
@@ -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);

diff --git a/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c b/sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
@@ -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);