diff --git a/conf/abi.xml b/conf/abi.xml
index f6287a43d7..e4369562c1 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -166,6 +166,12 @@
+
+ bit 1: AoA is updated, bit 2: sideslip is updated
+ Angle of attack
+ Sideslip angle
+
+
diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h
index a79ab1c301..3b0bb6e0a3 100644
--- a/sw/airborne/math/pprz_isa.h
+++ b/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
diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c
index eacbda6d90..d1e34a1d1f 100644
--- a/sw/airborne/modules/air_data/air_data.c
+++ b/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;
}
diff --git a/sw/airborne/modules/sensors/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c
index 588b7703cc..d52b08dca2 100644
--- a/sw/airborne/modules/sensors/airspeed_adc.c
+++ b/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
diff --git a/sw/airborne/modules/sensors/aoa_adc.c b/sw/airborne/modules/sensors/aoa_adc.c
index 8f9c75cd45..c1147eb78b 100644
--- a/sw/airborne/modules/sensors/aoa_adc.c
+++ b/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
diff --git a/sw/airborne/modules/sensors/aoa_pwm.c b/sw/airborne/modules/sensors/aoa_pwm.c
index b6897e9308..14f8918ebe 100644
--- a/sw/airborne/modules/sensors/aoa_pwm.c
+++ b/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
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index 088014b890..ed92b8f0dc 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/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
diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c
index 891be4b945..d95c28317f 100644
--- a/sw/simulator/nps/nps_autopilot_fixedwing.c
+++ b/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();
}