[sensors] airspeed and incidence sensors use ABI

- state interface is not updated directly, it should go through air_data
  or eventually some INS filter
- NPS is updated as well
- tas_from_eas (air_data) takes at least the altitude into account (not
  temperature) when direct measurements of P and T are not availble

conf/abi.xml

@@ -166,6 +166,12 @@
       <field name="vz" type="float" unit="m/s"/>
     </message>
 
+    <message name="INCIDENCE" id="25">
+      <field name="flag" type="uint8_t">bit 1: AoA is updated, bit 2: sideslip is updated</field>
+      <field name="aoa" type="float" unit="rad">Angle of attack</field>
+      <field name="sideslip" type="float" unit="rad">Sideslip angle</field>
+    </message>
+
   </msg_class>
 
 </protocol>

sw/airborne/math/pprz_isa.h

@@ -65,6 +65,11 @@ extern "C" {
 
 static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT *PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY);
 
+/** Convert temperature from Kelvin to Celsius */
+#define CelsiusOfKelvin(_t) (_t - 274.15f)
+/** Convert temperature from Celsius to Kelvin */
+#define KelvinOfCelsius(_t) (_t + 274.15f)
+
 /**
  * Get absolute altitude from pressure (using simplified equation).
  * Referrence pressure is standard pressure at sea level
@@ -165,6 +170,17 @@ static inline float pprz_isa_ref_pressure_of_height_full(float pressure, float h
   return pressure / pow(Trel, expo);
 }
 
+/**
+ * Get ISA temperature from a MSL altitude
+ *
+ * @param alt AMSL altitude
+ * @return temperature in ISA condition
+ */
+static inline float pprz_isa_temperature_of_altitude(float alt)
+{
+  return PPRZ_ISA_SEA_LEVEL_TEMP - PPRZ_ISA_TEMP_LAPS_RATE * alt;
+}
+
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

sw/airborne/modules/air_data/air_data.c

@@ -66,6 +66,13 @@ static abi_event temperature_ev;
 #endif
 static abi_event airspeed_ev;
 
+/** ABI binding for incidence angles
+ */
+#ifndef AIR_DATA_INCIDENCE_ID
+#define AIR_DATA_INCIDENCE_ID ABI_BROADCAST
+#endif
+static abi_event incidence_ev;
+
 /** Default factor to convert estimated airspeed (EAS) to true airspeed (TAS) */
 #ifndef AIR_DATA_TAS_FACTOR
 #define AIR_DATA_TAS_FACTOR 1.0
@@ -162,6 +169,20 @@ static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float eas)
   }
 }
 
+static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
+{
+  if (bit_is_set(flag, 0)) {
+    // update angle of attack
+    air_data.aoa = aoa;
+    stateSetAngleOfAttack_f(aoa);
+  }
+  if (bit_is_set(flag, 1)) {
+    // update sideslip angle
+    air_data.sideslip = sideslip;
+    stateSetSideslip_f(sideslip);
+  }
+}
+
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
@@ -224,6 +245,7 @@ void air_data_init(void)
   AbiBindMsgBARO_DIFF(AIR_DATA_BARO_DIFF_ID, &pressure_diff_ev, pressure_diff_cb);
   AbiBindMsgTEMPERATURE(AIR_DATA_TEMPERATURE_ID, &temperature_ev, temperature_cb);
   AbiBindMsgAIRSPEED(AIR_DATA_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
+  AbiBindMsgINCIDENCE(AIR_DATA_INCIDENCE_ID, &incidence_ev, incidence_cb);
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BARO_RAW, send_baro_raw);
@@ -300,7 +322,7 @@ float get_tas_factor(float p, float t)
    * sqrt(rho0 / rho) = sqrt((p0 * T) / (p * T0))
    * convert input temp to Kelvin
    */
-  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * (t + 274.15)) /
+  return sqrtf((PPRZ_ISA_SEA_LEVEL_PRESSURE * KelvinOfCelsius(t)) /
                (p * PPRZ_ISA_SEA_LEVEL_TEMP));
 }
 
@@ -315,6 +337,20 @@ float get_tas_factor(float p, float t)
  */
 float tas_from_eas(float eas)
 {
+  // update tas factor if requested
+  if (air_data.calc_tas_factor) {
+    if (air_data.pressure > 0.f && air_data.temperature > -900.f) {
+      // compute air density from pressure and temperature
+      air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature);
+    }
+    else {
+      // compute air density from altitude in ISA condition
+      const float z = air_data_get_amsl();
+      const float p = pprz_isa_pressure_of_altitude(z);
+      const float t = pprz_isa_temperature_of_altitude(z);
+      air_data.tas_factor = get_tas_factor(p, CelsiusOfKelvin(t));
+    }
+  }
   return air_data.tas_factor * eas;
 }
 

sw/airborne/modules/sensors/airspeed_adc.c

@@ -28,6 +28,7 @@
 #include BOARD_CONFIG
 #include "generated/airframe.h"
 #include "state.h"
+#include "subsystems/abi.h"
 
 #ifndef USE_AIRSPEED_ADC
 #define USE_AIRSPEED_ADC TRUE
@@ -85,6 +86,8 @@ void airspeed_adc_update(void)
   airspeed_adc.airspeed = sim_air_speed;
 #endif //SITL
 
+  AbiSendMsgAIRSPEED(AIRSPEED_ADC_ID, airspeed_adc.airspeed);
+
 #if USE_AIRSPEED_ADC
   stateSetAirspeed_f(airspeed_adc.airspeed);
 #endif

sw/airborne/modules/sensors/aoa_adc.c

@@ -30,6 +30,7 @@
 
 #include "modules/sensors/aoa_adc.h"
 #include "generated/airframe.h"
+#include "subsystems/abi.h"
 #include "state.h"
 
 // Messages
@@ -84,6 +85,9 @@ void aoa_adc_update(void)
   prev_aoa = aoa_adc.angle;
 
 #ifdef USE_AOA
+  uint8_t flag = 1;
+  float foo = 0.f;
+  AbiSendMsgINCIDENCE(AOA_ADC_ID, flag, aoa_adc.angle, foo);
   stateSetAngleOfAttack_f(aoa_adc.angle);
 #endif
 

sw/airborne/modules/sensors/aoa_pwm.c

@@ -36,6 +36,7 @@
 #include "mcu_periph/pwm_input.h"
 #include "pprzlink/messages.h"
 #include "subsystems/datalink/downlink.h"
+#include "subsystems/abi.h"
 #include "generated/airframe.h"
 
 #if LOG_AOA
@@ -164,6 +165,9 @@ void aoa_pwm_init(void)
 
 /* update, log and send */
 void aoa_pwm_update(void) {
+#if USE_AOA || USE_SIDESLIP
+  uint8_t flag = 0;
+#endif
   static float prev_aoa = 0.0f;
   // raw duty cycle in usec
   uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL);
@@ -178,7 +182,7 @@ void aoa_pwm_update(void) {
   prev_aoa = aoa_pwm.angle;
 
 #if USE_AOA
-  stateSetAngleOfAttack_f(aoa_pwm.angle);
+  SetBit(flag, 0);
 #endif
 
 #if USE_SIDESLIP
@@ -195,7 +199,11 @@ void aoa_pwm_update(void) {
   ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle;
   prev_ssa = ssa_pwm.angle;
 
-  stateSetSideslip_f(ssa_pwm.angle);
+  SetBit(flag, 1);
+#endif
+
+#if USE_AOA || USE_SIDESLIP
+  AbiSendMsgINCIDENCE(AOA_PWM_ID, flag, aoa_pwm.angle, ssa_pwm.angle);
 #endif
 
 #if SEND_SYNC_AOA

sw/airborne/subsystems/abi_sender_ids.h

@@ -88,6 +88,32 @@
 #define MS45XX_SENDER_ID 40
 #endif
 
+/*
+ * IDs of airspeed sensors (message 14)
+ */
+#ifndef AIRSPEED_NPS_ID
+#define AIRSPEED_NPS_ID 1
+#endif
+
+#ifndef AIRSPEED_ADC_ID
+#define AIRSPEED_ADC_ID 2
+#endif
+
+/*
+ * IDs of Incidence angles (message 24)
+ */
+#ifndef AOA_ADC_ID
+#define AOA_ADC_ID 1
+#endif
+
+#ifndef AOA_PWM_ID
+#define AOA_PWM_ID 2
+#endif
+
+#ifndef INCIDENCE_NPS_ID
+#define INCIDENCE_NPS_ID 20
+#endif
+
 /*
  * IDs of AGL measurment modules that can be loaded (sonars,...) (message 2)
  */
@@ -130,6 +156,7 @@
 #ifndef AGL_RAY_SENSOR_GAZEBO_ID
 #define AGL_RAY_SENSOR_GAZEBO_ID 10
 #endif
+
 /*
  * IDs of magnetometer sensors (including IMUs with mag)
  */
@@ -281,7 +308,7 @@
 #endif
 
 /*
- * IDs of OPTICFLOW estimates (message 12)
+ * IDs of OPTICFLOW estimates (message 11)
  */
 #ifndef FLOW_OPTICFLOW_ID
 #define FLOW_OPTICFLOW_ID 1

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -140,7 +140,7 @@ void nps_autopilot_run_step(double time)
 
 #if USE_AIRSPEED || USE_NPS_AIRSPEED
   if (nps_sensors_airspeed_available()) {
-    stateSetAirspeed_f((float)sensors.airspeed.value);
+    AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
     Fbw(event_task);
     Ap(event_task);
   }
@@ -167,22 +167,44 @@ void nps_autopilot_run_step(double time)
   }
 #endif
 
-#if USE_NPS_AOA
+  // standalone angle of attack
+#if USE_NPS_AOA && !NPS_SYNC_INCIDENCE
   if (nps_sensors_aoa_available()) {
-    stateSetAngleOfAttack_f((float)sensors.aoa.value);
+    AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 1, (float)sensors.aoa.value, 0.f);
     Fbw(event_task);
     Ap(event_task);
   }
 #endif
 
-#if USE_NPS_SIDESLIP
+  // standalone sideslip
+#if USE_NPS_SIDESLIP && !NPS_SYNC_INCIDENCE
   if (nps_sensors_sideslip_available()) {
-    stateSetSideslip_f((float)sensors.sideslip.value);
+    AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 2, 0.f, (float)sensors.sideslip.value);
     Fbw(event_task);
     Ap(event_task);
   }
 #endif
 
+  // synchronized angle of attack and sideslip
+#if NPS_SYNC_INCIDENCE && USE_NPS_AOA && USE_NPS_SIDESLIP
+  static uint8_t flag = 0;
+  if (nps_sensors_aoa_available()) {
+    SetBit(flag, 0);
+  }
+  if (nps_sensors_sideslip_available()) {
+    SetBit(flag, 1);
+  }
+  if (flag == 3) {
+    // both sensors are updated
+    AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 3, (float)sensors.aoa.value, (float)sensors.sideslip.value);
+    Fbw(event_task);
+    Ap(event_task);
+    flag = 0;
+  }
+#endif
+
+
+
   if (nps_bypass_ahrs) {
     sim_overwrite_ahrs();
   }