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

Add remaining flight time/distance calculations and OSD items

Browse source
This commit is contained in:
Michel Pastor 2018-06-04 16:07:51 +02:00
parent 46ef27db85
commit 83148a04d2
19 changed files with 401 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -74,6 +74,7 @@ COMMON_SRC = \
flight/mixer.c \
flight/pid.c \
flight/pid_autotune.c \
flight/rth_estimator.c \
flight/servos.c \
flight/wind_estimator.c \
io/beeper.c \

3
src/main/build/debug.h
View file

@ -16,6 +16,8 @@
*/
#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#define DEBUG16_VALUE_COUNT 4
extern int16_t debug[DEBUG16_VALUE_COUNT];
@ -63,6 +65,7 @@ typedef enum {
DEBUG_SAG_COMP_VOLTAGE,
DEBUG_VIBE,
DEBUG_CRUISE,
DEBUG_REM_FLIGHT_TIME,
DEBUG_COUNT
} debugType_e;

2
src/main/cms/cms_menu_osd.c
View file

@ -157,6 +157,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("ON TIME", OSD_ONTIME),
OSD_ELEMENT_ENTRY("FLY TIME", OSD_FLYTIME),
OSD_ELEMENT_ENTRY("ON/FLY TIME", OSD_ONTIME_FLYTIME),
OSD_ELEMENT_ENTRY("REM. TIME", OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH),
OSD_ELEMENT_ENTRY("REM. DIST", OSD_REMAINING_DISTANCE_BEFORE_RTH),
OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME),
OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE),
OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME),

2
src/main/common/maths.h
View file

@ -153,7 +153,9 @@ float cos_approx(float x);
float atan2_approx(float y, float x);
float acos_approx(float x);
#define tan_approx(x) (sin_approx(x) / cos_approx(x))
#define asin_approx(x) (M_PIf / 2 - acos_approx(x))
#else
#define asin_approx(x) asinf(x)
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
#define atan2_approx(y,x) atan2f(y,x)

10
src/main/fc/fc_core.c
View file

@ -94,6 +94,7 @@ enum {
#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro
timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
static timeUs_t flightTime = 0;
float dT;
@ -704,6 +705,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
if (ARMING_FLAG(ARMED) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))) {
flightTime += cycleTime;
}
#ifdef USE_ASYNC_GYRO_PROCESSING
if (getAsyncMode() == ASYNC_MODE_NONE) {
taskGyro(currentTimeUs);
@ -825,3 +830,8 @@ void taskUpdateRxMain(timeUs_t currentTimeUs)
processRx(currentTimeUs);
isRXDataNew = true;
}
// returns seconds
float getFlightTime() {
return (float)(flightTime / 1000) / 1000;
}

4
src/main/fc/fc_core.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "common/time.h"
typedef enum disarmReason_e {
DISARM_NONE = 0,
DISARM_TIMEOUT = 1,
@ -29,6 +31,7 @@ typedef enum disarmReason_e {
DISARM_REASON_COUNT
} disarmReason_t;
void handleInflightCalibrationStickPosition(void);
void disarm(disarmReason_t disarmReason);
@ -36,3 +39,4 @@ void tryArm(void);
disarmReason_t getDisarmReason(void);
bool isCalibrating(void);
float getFlightTime();

21
src/main/fc/settings.yaml
View file

@ -67,7 +67,7 @@ tables:
- name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE"]
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -536,6 +536,17 @@ groups:
field: voltageSource
table: bat_voltage_source
type: uint8_t
- name: cruise_power
field: cruise_power
min: 0
max: 4294967295
- name: idle_power
field: idle_power
min: 0
max: 65535
- name: rth_energy_margin
min: 0
max: 100
- name: PG_BATTERY_PROFILES
type: batteryProfile_t
@ -1311,6 +1322,10 @@ groups:
field: fw.loiter_radius
min: 0
max: 10000
- name: nav_fw_cruise_speed
field: fw.cruise_speed
min: 0
max: 65535
- name: nav_fw_land_dive_angle
field: fw.land_dive_angle
@ -1531,6 +1546,10 @@ groups:
min: 0
max: 1
- name: osd_estimations_wind_compensation
field: estimations_wind_compensation
type: bool
- name: PG_SYSTEM_CONFIG
type: systemConfig_t
headers: ["fc/config.h"]

13
src/main/fc/stats.c
View file

@ -30,8 +30,14 @@ PG_RESET_TEMPLATE(statsConfig_t, statsConfig,
static uint32_t arm_millis;
static uint32_t arm_distance_cm;
#ifdef USE_ADC
static uint32_t arm_mWhDrawn;
static uint32_t flyingEnergy; // energy drawn during flying up to last disarm (ARMED) mWh
uint32_t getFlyingEnergy() {
return flyingEnergy;
}
#endif
void statsOnArm(void)
@ -51,8 +57,11 @@ void statsOnDisarm(void)
statsConfigMutable()->stats_total_time += dt; //[s]
statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100; //[m]
#ifdef USE_ADC
if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))
statsConfigMutable()->stats_total_energy += getMWhDrawn() - arm_mWhDrawn;
if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
const uint32_t energy = getMWhDrawn() - arm_mWhDrawn;
statsConfigMutable()->stats_total_energy += energy;
flyingEnergy += energy;
}
#endif
writeEEPROM();
}

1
src/main/fc/stats.h
View file

@ -11,6 +11,7 @@ typedef struct statsConfig_s {
uint8_t stats_enabled;
} statsConfig_t;
uint32_t getFlyingEnergy();
void statsOnArm(void);
void statsOnDisarm(void);

201
src/main/flight/rth_estimator.c Normal file
View file

@ -0,0 +1,201 @@
#include "build/debug.h"
#include "common/maths.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/wind_estimator.h"
#include "navigation/navigation.h"
#include "sensors/battery.h"
#include <stdint.h>
#if defined(USE_ADC) && defined(USE_GPS)
/* INPUTS:
* - forwardSpeed (same unit as horizontalWindSpeed)
* - heading degrees
* - horizontalWindSpeed (same unit as forwardSpeed)
* - windHeading degrees
* OUTPUT:
* returns degrees
*/
static float windDriftCompensationAngle(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
return RADIANS_TO_DEGREES(asin_approx(-horizontalWindSpeed * sin_approx(DEGREES_TO_RADIANS(windHeading - heading)) / forwardSpeed));
}
/* INPUTS:
* - forwardSpeed (same unit as horizontalWindSpeed)
* - heading degrees
* - horizontalWindSpeed (same unit as forwardSpeed)
* - windHeading degrees
* OUTPUT:
* returns (same unit as forwardSpeed and horizontalWindSpeed)
*/
static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading)));
}
/* INPUTS:
* - heading degrees
* - horizontalWindSpeed
* - windHeading degrees
* OUTPUT:
* returns same unit as horizontalWindSpeed
*/
static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) {
return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading));
}
/* INPUTS:
* - forwardSpeed (same unit as horizontalWindSpeed)
* - heading degrees
* - horizontalWindSpeed (same unit as forwardSpeed)
* - windHeading degrees
* OUTPUT:
* returns (same unit as forwardSpeed and horizontalWindSpeed)
*/
static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) {
return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading);
}
// returns degrees
static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) {
return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle;
}
// altitudeChange is in meters
// idle_power and cruise_power are in deciWatt
// output is in Watt
static float estimateRTHAltitudeChangePower(float altitudeChange) {
uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle;
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
}
// altitudeChange is in m
// verticalWindSpeed is in m/s
// cruise_speed is in cm/s
// output is in seconds
static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) {
// Assuming increase in throttle keeps air speed at cruise speed
const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed;
return altitudeChange / estimatedVerticalSpeed;
}
// altitudeChange is in m
// horizontalWindSpeed is in m/s
// windHeading is in degrees
// verticalWindSpeed is in m/s
// cruise_speed is in cm/s
// output is in meters
static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) {
// Assuming increase in throttle keeps air speed at cruise speed
float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading);
return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed;
}
// altitudeChange is in m
// verticalWindSpeed is in m/s
// output is in Wh
static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) {
return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600;
}
// returns distance in m
// *heading is in degrees
static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed);
if (altitudeChange > 0) {
float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome);
float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff);
float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff);
*heading = RADIANS_TO_DEGREES(atan2_approx(triangleAltitude, triangleAltitudeToReturnStart));
return sqrt(sq(triangleAltitude) + sq(triangleAltitudeToReturnStart));
} else {
*heading = GPS_directionToHome;
return GPS_distanceToHome - estimatedAltitudeChangeGroundDistance;
}
}
// returns mWh
static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
// Fixed wing only for now
if (!STATE(FIXED_WING))
return -1;
if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && navigationPositionEstimateIsHealthy() && (batteryMetersConfig()->cruise_power > 0) && (ARMING_FLAG(ARMED)) && ((!STATE(FIXED_WING)) || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) && (navConfig()->fw.cruise_speed > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && batteryWasFullWhenPluggedIn() && isEstimatedWindSpeedValid() && isImuHeadingValid()))
return -1;
uint16_t windHeading; // centidegrees
const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s
const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading);
const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100;
const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100;
float RTH_heading; // degrees
const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading);
const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees);
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100));
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100));
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100));
DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100));
if (RTH_speed <= 0)
return -2; // wind is too strong
const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds
const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh
const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh
const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh
if (remaining_energy_before_rth < 0) // No energy left = No time left
return 0;
return remaining_energy_before_rth;
}
// returns seconds
int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount);
// error: return error code directly
if (remainingEnergyBeforeRTH < 0)
return remainingEnergyBeforeRTH;
const int32_t averagePower = calculateAveragePower();
if (averagePower == 0)
return -1;
const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower;
if (time_before_rth > 0x7FFFFFFF) // int32 overflow
return -1;
return time_before_rth;
}
// returns meters
int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
// error: return error code directly
if (remainingFlightTimeBeforeRTH < 0)
return remainingFlightTimeBeforeRTH;
return remainingFlightTimeBeforeRTH * calculateAverageSpeed();
}
#endif

5
src/main/flight/rth_estimator.h Normal file
View file

@ -0,0 +1,5 @@
#if defined(USE_ADC) && defined(USE_GPS)
int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount);
int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount);
#endif

1
src/main/flight/wind_estimator.h
View file

@ -23,6 +23,7 @@
#endif
#endif
#include "common/axis.h"
#include "common/time.h"
bool isEstimatedWindSpeedValid(void);

79
src/main/io/osd.c
View file

@ -67,9 +67,11 @@
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "fc/fc_tasks.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/rth_estimator.h"
#include "flight/wind_estimator.h"
#include "navigation/navigation.h"
@ -116,7 +118,6 @@
x; \
})
static timeUs_t flyTime = 0;
static unsigned currentLayout = 0;
static int layoutOverride = -1;
@ -160,7 +161,7 @@ static displayPort_t *osdDisplayPort;
#define AH_SIDEBAR_WIDTH_POS 7
#define AH_SIDEBAR_HEIGHT_POS 3
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2);
PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 3);
static int digitCount(int32_t value)
{
@ -460,7 +461,7 @@ static inline void osdFormatOnTime(char *buff)
static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
{
uint32_t seconds = flyTime / 1000000;
uint32_t seconds = getFlightTime();
osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H);
if (attr && osdConfig()->time_alarm > 0) {
if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) {
@ -1233,6 +1234,60 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH:
{
static timeUs_t updatedTimestamp = 0;
/*static int32_t updatedTimeSeconds = 0;*/
timeUs_t currentTimeUs = micros();
static int32_t timeSeconds = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
updatedTimestamp = currentTimeUs;
}
if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
buff[0] = SYM_FLY_M;
strcpy(buff + 1, "--:--");
updatedTimestamp = 0;
} else if (timeSeconds == -2) {
// Wind is too strong to come back with cruise throttle
buff[0] = SYM_FLY_M;
buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
buff[3] = ':';
buff[6] = '\0';
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else {
osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
if (timeSeconds == 0)
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
}
break;
case OSD_REMAINING_DISTANCE_BEFORE_RTH:;
static timeUs_t updatedTimestamp = 0;
timeUs_t currentTimeUs = micros();
static int32_t distanceMeters = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
updatedTimestamp = currentTimeUs;
}
buff[0] = SYM_TRIP_DIST;
if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
buff[1] = SYM_DIST_M;
strcpy(buff + 2, "---");
} else if (distanceMeters == -2) {
// Wind is too strong to come back with cruise throttle
buff[0] = SYM_DIST_M;
buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL;
buff[5] = '\0';
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} else {
osdFormatDistanceSymbol(buff + 1, distanceMeters * 100);
if (distanceMeters == 0)
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
break;
case OSD_FLYMODE:
{
char *p = "ACRO";
@ -1875,6 +1930,9 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
elementIndex = OSD_TRIP_DIST;
}
if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
elementIndex = OSD_ITEM_COUNT;
}
}
if (!feature(FEATURE_GPS)) {
if (elementIndex == OSD_GPS_SPEED) {
@ -1952,6 +2010,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
@ -1995,6 +2055,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->units = OSD_UNIT_METRIC;
osdConfig->main_voltage_decimals = 1;
osdConfig->attitude_angle_decimals = 0;
osdConfig->estimations_wind_compensation = true;
}
static void osdSetNextRefreshIn(uint32_t timeMs) {
@ -2196,7 +2258,7 @@ static void osdShowStats(void)
}
displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
uint32_t flySeconds = flyTime / 1000000;
uint16_t flySeconds = getFlightTime();
uint16_t flyMinutes = flySeconds / 60;
flySeconds %= 60;
uint16_t flyHours = flyMinutes / 60;
@ -2241,8 +2303,6 @@ static void osdShowArmed(void)
static void osdRefresh(timeUs_t currentTimeUs)
{
static timeUs_t lastTimeUs = 0;
if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu)) {
displayClearScreen(osdDisplayPort);
armState = ARMING_FLAG(ARMED);
@ -2263,13 +2323,6 @@ static void osdRefresh(timeUs_t currentTimeUs)
armState = ARMING_FLAG(ARMED);
}
if (ARMING_FLAG(ARMED)) {
timeUs_t deltaT = currentTimeUs - lastTimeUs;
flyTime += deltaT;
}
lastTimeUs = currentTimeUs;
if (resumeRefreshAt) {
// If we already reached he time for the next refresh,
// or THR is high or PITCH is high, resume refreshing.

4
src/main/io/osd.h
View file

@ -85,6 +85,8 @@ typedef enum {
OSD_WIND_SPEED_VERTICAL,
OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
OSD_REMAINING_DISTANCE_BEFORE_RTH,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -136,6 +138,8 @@ typedef struct osdConfig_s {
uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
} osdConfig_t;
PG_DECLARE(osdConfig_t, osdConfig);

42
src/main/navigation/navigation.c
View file

@ -124,6 +124,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_climb_angle = 20,
.max_dive_angle = 15,
.cruise_throttle = 1400,
.cruise_speed = 0, // cm/s
.max_throttle = 1700,
.min_throttle = 1200,
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
@ -1893,6 +1894,22 @@ static void updateHomePositionCompatibility(void)
GPS_directionToHome = posControl.homeDirection / 100;
}
float RTHAltitude() {
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
return(posControl.actualState.abs.pos.z);
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude);
case NAV_RTH_MAX_ALT:
return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude);
}
}
/*-----------------------------------------------------------
* Reset home position to current position
*-----------------------------------------------------------*/
@ -1900,24 +1917,7 @@ static void updateDesiredRTHAltitude(void)
{
if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
break;
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
break;
case NAV_RTH_MAX_ALT:
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
break;
}
posControl.homeWaypointAbove.pos.z = RTHAltitude();
}
}
else {
@ -3053,6 +3053,11 @@ int32_t navigationGetHomeHeading(void)
return posControl.homePosition.yaw;
}
// returns m/s
float calculateAverageSpeed() {
return (float)getTotalTravelDistance() / (getFlightTime() * 100);
}
#else // NAV
#ifdef USE_GPS
@ -3115,6 +3120,7 @@ int32_t getTotalTravelDistance(void)
{
return lrintf(GPS_totalTravelDistance);
}
#endif
#endif // NAV

6
src/main/navigation/navigation.h
View file

@ -152,6 +152,7 @@ typedef struct navConfig_s {
uint8_t max_climb_angle; // Fixed wing max banking angle (deg)
uint8_t max_dive_angle; // Fixed wing max banking angle (deg)
uint16_t cruise_throttle; // Cruise throttle
uint16_t cruise_speed; // Speed at cruise throttle (cm/s), used for time/distance left before RTH
uint16_t min_throttle; // Minimum allowed throttle in auto mode
uint16_t max_throttle; // Maximum allowed throttle in auto mode
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
@ -309,6 +310,8 @@ void resetWaypointList(void);
bool loadNonVolatileWaypointList(void);
bool saveNonVolatileWaypointList(void);
float RTHAltitude();
/* Geodetic functions */
typedef enum {
GEO_ALT_ABSOLUTE,
@ -339,6 +342,9 @@ bool navigationIsFlyingAutonomousMode(void);
bool navigationRTHAllowsLanding(void);
bool isNavLaunchEnabled(void);
bool isFixedWingLaunchDetected(void);
float calculateAverageSpeed();
/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles

34
src/main/sensors/battery.c
View file

@ -21,6 +21,8 @@
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/filter.h"
@ -32,7 +34,14 @@
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/runtime_config.h"
#include "fc/stats.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "navigation/navigation.h"
#include "config/feature.h"
@ -44,7 +53,6 @@
#include "io/beeper.h"
#include "build/debug.h"
#define ADCVREF 3300 // in mV (3300 = 3.3V)
@ -119,7 +127,11 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
.offset = CURRENT_METER_OFFSET
},
.voltageSource = BAT_VOLTAGE_RAW
.voltageSource = BAT_VOLTAGE_RAW,
.cruise_power = 0,
.idle_power = 0,
.rth_energy_margin = 5
);
@ -459,6 +471,14 @@ void powerMeterUpdate(timeUs_t timeDelta)
mWhDrawn = mWhDrawnRaw / (3600 * 100);
}
// calculate true power including heat losses in power supply (battery + wires)
// power is in cW (0.01W)
// batteryWarningVoltage is in cV (0.01V)
int32_t heatLossesCompensatedPower(int32_t power)
{
return power + sq(power * 100 / batteryWarningVoltage) * powerSupplyImpedance / 100000;
}
void sagCompensatedVBatUpdate(timeUs_t currentTime)
{
static timeUs_t recordTimestamp = 0;
@ -510,3 +530,13 @@ uint8_t calculateBatteryPercentage(void)
void batteryDisableProfileAutoswitch(void) {
profileAutoswitchDisable = true;
}
// returns cW (0.01W)
int32_t calculateAveragePower() {
return (int64_t)mWhDrawn * 360 / getFlightTime();
}
// returns mWh / meter
int32_t calculateAverageEfficiency() {
return getFlyingEnergy() * 100 / getTotalTravelDistance();
}

7
src/main/sensors/battery.h
View file

@ -74,6 +74,10 @@ typedef struct batteryMetersConfig_s {
batVoltageSource_e voltageSource;
uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW)
uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW)
uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH
} batteryMetersConfig_t;
typedef struct batteryProfile_s {
@ -144,3 +148,6 @@ void powerMeterUpdate(timeUs_t timeDelta);
uint8_t calculateBatteryPercentage(void);
float calculateThrottleCompensationFactor(void);
int32_t calculateAveragePower();
int32_t calculateAverageEfficiency();
int32_t heatLossesCompensatedPower(int32_t power);