From aa4faaab9efe38e8326d30f520c04b3e0d9e2179 Mon Sep 17 00:00:00 2001
From: Xavier Gibert <xaviergibert@hotmail.es>
Date: Thu, 7 Jun 2012 15:23:57 +0200
Subject: [PATCH] Solved rotorcraft problems

---
 conf/messages/downlink.xml                    | 50 +++++++++++--------
 conf/telemetry/default_rotorcraft.xml         | 10 ++--
 sw/airborne/firmwares/fixedwing/ap_downlink.h |  4 +-
 sw/airborne/firmwares/rotorcraft/telemetry.h  | 15 ++++--
 4 files changed, 49 insertions(+), 30 deletions(-)

diff --git a/conf/messages/downlink.xml b/conf/messages/downlink.xml
index 13062e16239..c0a71058ec4 100644
--- a/conf/messages/downlink.xml
+++ b/conf/messages/downlink.xml
@@ -117,7 +117,7 @@
 		<field name="yaw"       type="int16" />
 	</message>
 
-	<message name="BOOZ2_CMD" id="27">
+	<message name="ROTORCRAFT_CMD" id="27">
 		<field name="cmd_roll"   type="int32"/>
 		<field name="cmd_pitch"  type="int32"/>
 		<field name="cmd_yaw"    type="int32"/>
@@ -139,7 +139,7 @@
 		<field name="pitch" type="int8"/>
 	</message>
 
-	<message name="BOOZ2_RADIO_CONTROL" id="31">
+	<message name="ROTORCRAFT_RADIO_CONTROL" id="31">
 		<field name="roll"     type="int16" unit="pprz"/>
 		<field name="pitch"    type="int16" unit="pprz"/>
 		<field name="yaw"      type="int16" unit="pprz"/>
@@ -394,15 +394,22 @@
 		<field name="psi"   type="float" unit="rad" alt_unit="deg"/>
 	</message>
 
-	<!-- Missing AHRS_EULER_INT -->
+	<message name="AHRS_EULER_INT" id="66">
+    <field name="imu_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+    <field name="imu_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+    <field name="imu_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+    <field name="body_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+    <field name="body_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+    <field name="body_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
+  </message>
 
-	<message name="AHRS_MEASUREMENT_EULER" id="66">
+	<message name="AHRS_MEASUREMENT_EULER" id="67">
 		<field name="phi"   type="float" unit="rad" alt_unit="deg"/>
 		<field name="theta" type="float" unit="rad" alt_unit="deg"/>
 		<field name="psi"   type="float" unit="rad" alt_unit="deg"/>
 	</message>
 
-	<message name="AHRS_DEBUG_QUAT" id="67">
+	<message name="AHRS_DEBUG_QUAT" id="68">
 		<field name="jqi"   type="float"/>
 		<field name="jqx"   type="float"/>
 		<field name="jqy"   type="float"/>
@@ -417,7 +424,7 @@
 		<field name="mqz"   type="float"/>
 	</message>
 
-	<message name="AHRS_LKF" id="68">
+	<message name="AHRS_LKF" id="69">
 		<field name="phi"   type="float" unit="rad"   alt_unit="deg"/>
 		<field name="theta" type="float" unit="rad"   alt_unit="deg"/>
 		<field name="psi"   type="float" unit="rad"   alt_unit="deg"/>
@@ -436,7 +443,7 @@
 		<field name="mz"    type="float" />
 	</message>
 
-	<message name="AHRS_LKF_DEBUG" id="69">
+	<message name="AHRS_LKF_DEBUG" id="70">
 		<field name="phi_err"   type="float" unit="rad"   alt_unit="deg"/>
 		<field name="theta_err" type="float" unit="rad"   alt_unit="deg"/>
 		<field name="psi_err"   type="float" unit="rad"   alt_unit="deg"/>
@@ -454,7 +461,7 @@
 		<field name="br_cov"    type="float" />
 	</message>
 
-	<message name="AHRS_LKF_ACC_DBG" id="70">
+	<message name="AHRS_LKF_ACC_DBG" id="71">
 		<field name="qi_err"    type="float" />
 		<field name="qx_err"    type="float" />
 		<field name="qy_err"    type="float" />
@@ -464,7 +471,7 @@
 		<field name="br_err"    type="float" unit="rad/s" alt_unit="deg/s"/>
 	</message>
 
-	<message name="AHRS_LKF_MAG_DBG" id="71">
+	<message name="AHRS_LKF_MAG_DBG" id="72">
 		<field name="qi_err"    type="float" />
 		<field name="qx_err"    type="float" />
 		<field name="qy_err"    type="float" />
@@ -475,7 +482,7 @@
 	</message>
 
 
-	<message name="BOOZ2_AHRS_EULER" id="72">
+	<message name="BOOZ2_AHRS_EULER" id="73">
 		<field name="imu_phi"    type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
 		<field name="imu_theta"  type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
 		<field name="imu_psi"    type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
@@ -484,7 +491,7 @@
 		<field name="body_psi"   type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
 	</message>
 
-	<message name="BOOZ2_AHRS_QUAT" id="73">
+	<message name="BOOZ2_AHRS_QUAT" id="74">
 		<field name="imu_qi"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
 		<field name="imu_qx"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
 		<field name="imu_qy"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
@@ -495,7 +502,7 @@
 		<field name="body_qz" type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
 	</message>
 
-	<message name="BOOZ2_AHRS_RMAT" id="74">
+	<message name="BOOZ2_AHRS_RMAT" id="75">
 		<field name="imu_m00"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
 		<field name="imu_m01"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
 		<field name="imu_m02"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
@@ -517,7 +524,7 @@
 	</message>
 
 
-	<message name="BOOZ_AHRS_BIAS" id="75">
+	<message name="BOOZ_AHRS_BIAS" id="76">
 		<field name="bp"     type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
 		<field name="bq"     type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
 		<field name="br"     type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
@@ -1022,14 +1029,15 @@
 	</message>
 
 	<message name="FILTER_ALIGNER" id="162">
-		<field name="lp_gp"  type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="lp_gq"  type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="lp_gr"  type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="gp"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="gq"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="gr"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
-		<field name="noise"  type="int32"/>
-		<field name="cnt"    type="int32"/>
+    <field name="lp_gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="lp_gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="lp_gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
+    <field name="noise" type="int32"/>
+    <field name="cnt" type="int32"/>
+    <field name="status" type="uint8" values="UNINIT|RUNNING|LOCKED"/>
 	</message>
 
 	<message name="FILTER_Q" id="163">
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index 14a48ead4fc..3cf8d2e1526 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -12,7 +12,7 @@
       <message name="ROTORCRAFT_FP"          period="0.25"/>
       <message name="ALIVE"                  period="2.1"/>
       <message name="INS_REF"                period="5.1"/>
-      <message name="MISION_STATUS"          period="1.6"/>
+      <message name="MISSION_STATUS"          period="1.6"/>
       <message name="WP_MOVED"               period="1.3"/>
       <message name="BOOZ2_CAM"              period="1."/>
       <message name="GPS_INT"                period=".25"/>
@@ -42,9 +42,9 @@
       <message name="ROTORCRAFT_STATUS"      period="1.2"/>
       <message name="DL_VALUE"               period="0.5"/>
       <message name="ALIVE"                  period="2.1"/>
-      <message name="IMU_GYRO_SCALED"        period=".075"/>
-      <message name="IMU_ACCEL_SCALED"       period=".075"/>
-      <message name="IMU_MAG_SCALED"         period=".1"/>
+      <message name="IMU_GYRO_INT"           period=".075"/>
+      <message name="IMU_ACCEL_INT"          period=".075"/>
+      <message name="IMU_MAG_INT"            period=".1"/>
     </mode>
 
     <mode name="ahrs">
@@ -99,7 +99,7 @@
       <!--<message name="STAB_ATTITUDE_REF" period=".4"/>-->
       <message name="ROTORCRAFT_FP"         period="0.8"/>
       <message name="ROTORCRAFT_STATUS"     period="1.2"/>
-      <message name="MISION_STATUS" period="1.6"/>
+      <message name="MISSION_STATUS" period="1.6"/>
 	  <message name="HFF_GPS"               period=".03"/>
       <message name="INS_REF"               period="5.1"/>
     </mode>
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
index 1a3cc779cf6..d19464839ab 100644
--- a/sw/airborne/firmwares/fixedwing/ap_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h
@@ -81,7 +81,9 @@
 	DOWNLINK_SEND_MISSION_STATUS(_trans, _dev, &estimator_flight_time, &nav_block, &block_time, &nav_stage, &stage_time, &nb_sec, &gps.fix, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count, &horizontal_mode);\
 })
 
-#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) SEND_MISSION_STATUS(_trans, _dev) 
+#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) Downlink({ \
+  SEND_MISSION_STATUS(_trans, _dev); \
+})
 
 #ifdef MCU_SPI_LINK
 #define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) DOWNLINK_SEND_DEBUG_MCU_LINK(_trans, _dev, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 1e423b9ea90..4f6f34350f8 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -51,6 +51,12 @@
 // I2C Error counters
 #include "mcu_periph/i2c.h"
 
+#if defined DOWNLINK
+#define Downlink(x) x
+#else
+#define Downlink(x) {}
+#endif
+
 #define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM)
 
 #include "subsystems/ins.h"
@@ -63,8 +69,8 @@
 	uint16_t vsup = electrical.vsupply; \
 	int16_t amps = (int16_t) (electrical.current/10); \
 	int16_t pwr = (int16_t) (vsup*amps); \
-	int16_t e = energy; \
-	DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &amps, &pwr, &e, &v_ctl_throttle_slewed);\
+	int16_t e = 0; \
+	DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &amps, &pwr, &e, 0);\
 })
 
 #define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) {			\
@@ -91,7 +97,10 @@
 }
 #endif /*USE_GPS */
 
-#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) SEND_MISSION_STATUS(_trans, _dev) 
+#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) Downlink({ \
+  SEND_MISSION_STATUS(_trans, _dev); \
+})
+
 
 #ifdef RADIO_CONTROL
 #define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)