Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

use pitot values only if healty

Browse source
This commit is contained in:
Roman Lut 2023-02-27 18:03:17 +01:00
parent 9c32094f5e
commit 936a7fd3a2
7 changed files with 12 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/fc/fc_tasks.c
View file

@ -200,7 +200,10 @@ void taskUpdatePitot(timeUs_t currentTimeUs)
} }
pitotUpdate(); pitotUpdate();
updatePositionEstimator_PitotTopic(currentTimeUs);
if ( pitotIsHealthy()) {
updatePositionEstimator_PitotTopic(currentTimeUs);
}
} }
#endif #endif

2
src/main/flight/pid.c
View file

@ -979,7 +979,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge
// yaw_rate = tan(roll_angle) * Gravity / forward_vel // yaw_rate = tan(roll_angle) * Gravity / forward_vel
#if defined(USE_PITOT) #if defined(USE_PITOT)
float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) ? getAirspeedEstimate() : pidProfile()->fixedWingReferenceAirspeed; float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy()? getAirspeedEstimate() : pidProfile()->fixedWingReferenceAirspeed;
#else #else
float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed; float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed;
#endif #endif

4
src/main/navigation/navigation_fixedwing.c
View file

@ -679,7 +679,9 @@ bool isFixedWingFlying(void)
{ {
float airspeed = 0.0f; float airspeed = 0.0f;
#ifdef USE_PITOT #ifdef USE_PITOT
airspeed = getAirspeedEstimate(); if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
airspeed = getAirspeedEstimate();
}
#endif #endif
bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f;

2
src/main/telemetry/ibus_shared.c
View file

@ -170,7 +170,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) {
return sendIbusMeasurement2(address, (uint16_t) (attitude.values.roll * 10)); //in ddeg -> cdeg, 1ddeg = 10cdeg return sendIbusMeasurement2(address, (uint16_t) (attitude.values.roll * 10)); //in ddeg -> cdeg, 1ddeg = 10cdeg
} else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_VSPEED) { //Speed cm/s } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_VSPEED) { //Speed cm/s
#ifdef USE_PITOT #ifdef USE_PITOT
if (sensors(SENSOR_PITOT)) return sendIbusMeasurement2(address, (uint16_t)getAirspeedEstimate()); //int32_t if (sensors(SENSOR_PITOT) && pitotIsHealthy()) return sendIbusMeasurement2(address, (uint16_t)getAirspeedEstimate()); //int32_t
else else
#endif #endif
return sendIbusMeasurement2(address, 0); return sendIbusMeasurement2(address, 0);

2
src/main/telemetry/ltm.c
View file

@ -189,7 +189,7 @@ void ltm_sframe(sbuf_t *dst)
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // current mAh (65535 mAh max) sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // current mAh (65535 mAh max)
sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023)); // scaled RSSI (uchar) sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023)); // scaled RSSI (uchar)
#if defined(USE_PITOT) #if defined(USE_PITOT)
sbufWriteU8(dst, sensors(SENSOR_PITOT) ? getAirspeedEstimate() / 100.0f : 0); // in m/s sbufWriteU8(dst, (sensors(SENSOR_PITOT) && pitotIsHealthy())? getAirspeedEstimate() / 100.0f : 0); // in m/s
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif

2
src/main/telemetry/mavlink.c
View file

@ -644,7 +644,7 @@ void mavlinkSendHUDAndHeartbeat(void)
#endif #endif
#if defined(USE_PITOT) #if defined(USE_PITOT)
if (sensors(SENSOR_PITOT)) { if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
mavAirSpeed = getAirspeedEstimate() / 100.0f; mavAirSpeed = getAirspeedEstimate() / 100.0f;
} }
#endif #endif

2
src/main/telemetry/smartport.c
View file

@ -550,7 +550,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break; break;
case FSSP_DATAID_ASPD : case FSSP_DATAID_ASPD :
#ifdef USE_PITOT #ifdef USE_PITOT
if (sensors(SENSOR_PITOT)) { if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1 smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1
*clearToSend = false; *clearToSend = false;
} }