[airframe] Airframe update, missing default telemetry & fix reading a…

…ngle
paparazzi · Oct 16, 2023 · 515cb91 · 515cb91
1 parent 2b89019
commit 515cb91
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 4 deletions.
diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
@@ -255,14 +255,20 @@
 
     <section name="IMU" prefix="IMU_">
         <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-3,3,13}, .scale={{15146,60067,2567},{1551,6124,263}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-26,9,2}, .scale={{32863,18346,55169},{6721,3745,11294}}}}"/>
-        <define name="MAG_CALIB"   value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-292,-95}, .scale={{33136,8764,8709},{59233,15761,15343}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->
+        <define name="MAG_CALIB"   value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-95,-292}, .scale={{33136,8709,8764},{59233,15343,15761}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->
 
         <!-- Define axis in hover frame -->
-        <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+        <define name="BODY_TO_IMU_PHI"   value="180." unit="deg"/>
         <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
         <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
     </section>
 
+    <section name="MAG_RM3100" prefix="RM3100_">
+      <define name="MAG_TO_IMU_PHI" value="-90.0"/>
+      <define name="MAG_TO_IMU_THETA" value="0.0"/>
+      <define name="MAG_TO_IMU_PSI" value="0.0"/>
+    </section>
+
     <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
         <!-- Limits -->
         <define name="SP_MAX_PHI"           value="45." unit="deg" />

diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
@@ -45,8 +45,10 @@
       <message name="HYBRID_GUIDANCE"          period="0.4"/>
       <message name="ESC"                      period="0.5"/>
       <message name="AHRS_REF_QUAT"            period="0.1"/>
-      <message name="INS_FLOW_INFO"	       period="0.1"/> 
+      <message name="INS_FLOW_INFO"            period="0.1"/>
       <message name="GPS_RTK"                  period="0.1"/>
+      <message name="AIRSPEED_RAW"             period="0.1"/>
+      <message name="ROTATING_WING_STATE"      period="0.2"/>
     </mode>
 
     <mode name="calibration">
@@ -82,6 +84,7 @@
       <message name="IMU_MAG_RAW"       period=".05"/>
       <message name="BARO_RAW"          period=".1"/>
       <message name="ARDRONE_NAVDATA"   period=".05"/>
+      <message name="AIRSPEED_RAW"      period="0.1"/>
     </mode>
 
     <mode name="scaled_sensors">
@@ -149,6 +152,7 @@
     <mode name="vel_guidance" key_press="q">
       <message name="ALIVE" period="0.9"/>
       <message name="HYBRID_GUIDANCE" period="0.062"/>
+      <message name="GUIDANCE_INDI_HYBRID"     period="0.1"/>
       <!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
       <message name="ROTORCRAFT_FP" period="0.8"/>
       <message name="ROTORCRAFT_STATUS" period="1.2"/>
@@ -242,6 +246,7 @@
       <message name="ACTUATORS"                period="0.002"/>
       <message name="GPS_RTK"                  period="0.1"/>
       <message name="IMU_HEATER"               period="1.0"/>
+      <message name="AIRSPEED_RAW"             period="0.01"/>
     </mode>
   </process>
 

diff --git a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c b/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c
@@ -121,7 +121,7 @@ static abi_event wing_position_ev;
 static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
 {
   for (int i=0; i<num_act; i++){
-    if (pos_msg[i].set.position && (pos_msg[i].idx =  SERVO_ROTATION_MECH))
+    if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH))
     {
       // Get wing rotation angle from sensor
       eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;