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(); }