diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 238a67db93b..e4b8dc1d67a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1627,7 +1627,7 @@ static void loadMainState(timeUs_t currentTimeUs) #endif #ifdef USE_PITOT - blackboxCurrent->airSpeed = pitot.airSpeed; + blackboxCurrent->airSpeed = getAirspeedEstimate(); #endif #ifdef USE_RANGEFINDER diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8f0ad833318..ac28c1ca132 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1428,7 +1428,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_AIR_SPEED: #ifdef USE_PITOT - sbufWriteU32(dst, pitot.airSpeed); + sbufWriteU32(dst, getAirspeedEstimate()); #else sbufWriteU32(dst, 0); #endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 58fcb4f4198..f497ad424ff 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -997,15 +997,13 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge // yaw_rate = tan(roll_angle) * Gravity / forward_vel #if defined(USE_PITOT) - float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) ? - pitot.airSpeed : - pidProfile()->fixedWingReferenceAirspeed; + float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) ? getAirspeedEstimate() : pidProfile()->fixedWingReferenceAirspeed; #else float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed; #endif // Constrain to somewhat sane limits - 10km/h - 216km/h - airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); + airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); @@ -1042,8 +1040,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAngle) { static uint8_t lastFpvCamAngleDegrees = 0; - static float cosCameraAngle = 1.0; - static float sinCameraAngle = 0.0; + static float cosCameraAngle = 1.0f; + static float sinCameraAngle = 0.0f; if (lastFpvCamAngleDegrees != fpvCameraAngle) { lastFpvCamAngleDegrees = fpvCameraAngle; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7198262a229..343233743ad 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2632,11 +2632,12 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_SPEED: { #ifdef USE_PITOT + const float airspeed_estimate = getAirspeedEstimate(); buff[0] = SYM_AIR; - osdFormatVelocityStr(buff + 1, pitot.airSpeed, false, false); + osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); - if ((osdConfig()->airspeed_alarm_min != 0 && pitot.airSpeed < osdConfig()->airspeed_alarm_min) || - (osdConfig()->airspeed_alarm_max != 0 && pitot.airSpeed > osdConfig()->airspeed_alarm_max)) { + if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || + (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } #else @@ -3777,14 +3778,16 @@ static void osdUpdateStats(void) if (feature(FEATURE_GPS)) { value = osdGet3DSpeed(); + const float airspeed_estimate = getAirspeedEstimate(); + if (stats.max_3D_speed < value) stats.max_3D_speed = value; if (stats.max_speed < gpsSol.groundSpeed) stats.max_speed = gpsSol.groundSpeed; - if (stats.max_air_speed < pitot.airSpeed) - stats.max_air_speed = pitot.airSpeed; + if (stats.max_air_speed < airspeed_estimate) + stats.max_air_speed = airspeed_estimate; if (stats.max_distance < GPS_distanceToHome) stats.max_distance = GPS_distanceToHome; diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 43e3b9b481d..361dce12fc6 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -517,7 +517,7 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) case OSD_SIDEBAR_SCROLL_SPEED: { #if defined(USE_GPS) - int speed = osdGetSpeedFromSelectedSource(); + int16_t speed = osdGetSpeedFromSelectedSource(); switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 9daa1dd7489..a6a772ac61b 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -53,7 +53,7 @@ PG_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, .speedSource = SETTING_OSD_SPEED_SOURCE_DEFAULT ); -int osdGetSpeedFromSelectedSource(void) { +int16_t osdGetSpeedFromSelectedSource(void) { int speed = 0; switch (osdCommonConfig()->speedSource) { case OSD_SPEED_SOURCE_GROUND: @@ -64,7 +64,7 @@ int osdGetSpeedFromSelectedSource(void) { break; case OSD_SPEED_SOURCE_AIR: #ifdef USE_PITOT - speed = pitot.airSpeed; + speed = (int16_t)getAirspeedEstimate(); #endif break; } diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index ff349481823..0f700c445e4 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -45,7 +45,7 @@ typedef struct { PG_DECLARE(osdCommonConfig_t, osdCommonConfig); -int osdGetSpeedFromSelectedSource(void); +int16_t osdGetSpeedFromSelectedSource(void); #endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 269ea0cd7bb..7285f889ad9 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -691,7 +691,7 @@ void osdDJIFormatVelocityStr(char* buff) case OSD_SPEED_SOURCE_AIR: strcpy(sourceBuf, "AIR"); #ifdef USE_PITOT - vel = pitot.airSpeed; + vel = getAirspeedEstimate(); #endif break; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 511d92ef72d..7eb854e9ff7 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -660,12 +660,12 @@ bool isFixedWingAutoThrottleManuallyIncreased() bool isFixedWingFlying(void) { - float airspeed = 0; + float airspeed = 0.0f; #ifdef USE_PITOT - airspeed = pitot.airSpeed; + airspeed = getAirspeedEstimate(); #endif bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle; - bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250; + bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index bd082da8085..4088294841e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -341,7 +341,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) */ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs) { - posEstimator.pitot.airspeed = pitot.airSpeed; + posEstimator.pitot.airspeed = getAirspeedEstimate(); posEstimator.pitot.lastUpdateTime = currentTimeUs; } #endif diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 66497962117..8493db1a9e2 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -504,7 +504,7 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s #ifdef USE_PITOT - return constrain(pitot.airSpeed, 0, INT16_MAX); + return constrain(getAirspeedEstimate(), 0, INT16_MAX); #else return false; #endif diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 9f321b5d046..6dfd04b7504 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -58,6 +58,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTME #else #define PITOT_HARDWARE_DEFAULT PITOT_NONE #endif + PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, .pitot_hardware = SETTING_PITOT_HARDWARE_DEFAULT, .pitot_lpf_milli_hz = SETTING_PITOT_LPF_MILLI_HZ_DEFAULT, @@ -231,7 +232,7 @@ STATIC_PROTOTHREAD(pitotThread) pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; } else { performPitotCalibrationCycle(); - pitot.airSpeed = 0; + pitot.airSpeed = 0.0f; } #ifdef USE_SIMULATOR if (simulatorData.flags & SIMU_AIRSPEED) { @@ -248,7 +249,7 @@ void pitotUpdate(void) pitotThread(); } -int32_t pitotCalculateAirSpeed(void) +float getAirspeedEstimate(void) { return pitot.airSpeed; } diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index fffc5a9f1ba..4f9bccb046c 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -65,7 +65,7 @@ bool pitotInit(void); bool pitotIsCalibrationComplete(void); void pitotStartCalibration(void); void pitotUpdate(void); -int32_t pitotCalculateAirSpeed(void); +float getAirspeedEstimate(void); bool pitotIsHealthy(void); #endif diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index ffb333c72af..e8424515165 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -169,7 +169,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { 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 #ifdef USE_PITOT - if (sensors(SENSOR_PITOT)) return sendIbusMeasurement2(address, (uint16_t) (pitot.airSpeed)); //int32_t + if (sensors(SENSOR_PITOT)) return sendIbusMeasurement2(address, (uint16_t)getAirspeedEstimate()); //int32_t else #endif return sendIbusMeasurement2(address, 0); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index b94730f936e..282c9be4fd2 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -189,7 +189,7 @@ void ltm_sframe(sbuf_t *dst) sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // current mAh (65535 mAh max) sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023)); // scaled RSSI (uchar) #if defined(USE_PITOT) - sbufWriteU8(dst, sensors(SENSOR_PITOT) ? pitot.airSpeed / 100.0f : 0); // in m/s + sbufWriteU8(dst, sensors(SENSOR_PITOT) ? getAirspeedEstimate() / 100.0f : 0); // in m/s #else sbufWriteU8(dst, 0); #endif diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index b13a9f04971..421ff59dd21 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -645,7 +645,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(USE_PITOT) if (sensors(SENSOR_PITOT)) { - mavAirSpeed = pitot.airSpeed / 100.0f; + mavAirSpeed = getAirspeedEstimate() / 100.0f; } #endif diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 766dea19010..1ea659864bd 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -551,7 +551,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear case FSSP_DATAID_ASPD : #ifdef USE_PITOT if (sensors(SENSOR_PITOT)) { - smartPortSendPackage(id, pitot.airSpeed * 0.194384449f); // cm/s to knots*1 + smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1 *clearToSend = false; } #endif