1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +03:00

Merge branch 'master' into MrD_Wank-to-Wake-launch-idle

This commit is contained in:
Darren Lines 2022-12-09 21:31:03 +00:00
commit 15cee7c808
76 changed files with 1120 additions and 417 deletions

View file

@ -6,7 +6,7 @@ on: pull_request
jobs:
build:
runs-on: ubuntu-18.04
runs-on: ubuntu-latest
strategy:
matrix:
id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
@ -45,7 +45,7 @@ jobs:
test:
needs: [build]
runs-on: ubuntu-18.04
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
- name: Install dependencies

View file

@ -11,7 +11,7 @@ on:
jobs:
settings_md:
runs-on: ubuntu-18.04
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3

View file

@ -311,6 +311,25 @@ set battery_capacity_warning = 300
set battery_capacity_critical = 150
```
#### Change control profile based on battery profile
You can change the control profile, automatically, based on the battery profile. This allows for fine tuning of each power choice.
```
feature BAT_PROF_AUTOSWITCH
battery_profile 1
set bat_cells = 3
set controlrate_profile = 1
battery_profile 2
set bat_cells = 4
set controlrate_profile = 2
```
## Remaining flight time and flight distance estimation
The estimated remaining flight time and flight distance estimations can be displayed on the OSD (for fixed wing only for the moment). They are calculated from the GPS distance from home, remaining battery capacity and average power draw. They are taking into account the requested altitude change and heading to home change after altitude change following the switch to RTH. They are also taking into account the estimated wind if `osd_estimations_wind_compensation` is set to `ON`. When the timer and distance indicator reach 0 they will blink and you need to go home in a straight line manually or by engaging RTH. You should be left with at least `rth_energy_margin`% of battery left when arriving home if the cruise speed and power are set correctly (see bellow).

View file

@ -111,7 +111,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
| `servo` | Configure servos |
| `set` | Change setting with name=value or blank or * for list |
| `smix` | Custom servo mixer |
| `status` | Show status |
| `status` | Show status. Error codes can be looked up [here](https://github.com/iNavFlight/inav/wiki/%22Something%22-is-disabled----Reasons) |
| `tasks` | Show task stats |
| `temp_sensor` | List or configure temperature sensor(s). See [temperature sensors documentation](Temperature-sensors.md) for more information. |
| `version` | Show version |

View file

@ -80,6 +80,12 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` |
| 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
| 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
| 47 | EDGE | `Operand A` is activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |
| 48 | DELAY | This will return `true` when `Operand A` is true, and the delay time in `Operand B` [ms] has been exceeded. |
| 49 | TIMER | `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
| 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater. |
| 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. |
### Operands
| Operand Type | Name | Notes |

View file

@ -9,7 +9,7 @@ As many can attest, multirotors and RC models in general can be very dangerous,
Please consult the [Cli](Cli.md), [Controls](Controls.md), [Failsafe](Failsafe.md) and [Modes](Modes.md)
pages for further important information.
You are highly advised to use the Receiver tab in the CleanFlight Configurator, making sure your Rx channel
You are highly advised to use the Receiver tab in the INAV Configurator, making sure your Rx channel
values are centered at 1500 (1520 for Futaba RC) with minimum & maximums of 1000 and 2000 (respectively)
are reached when controls are operated. Failure to configure these ranges properly can create
problems, such as inability to arm (because you can't reach the endpoints) or immediate activation of

View file

@ -3812,6 +3812,16 @@ If set to ON, waypoints will be automatically loaded from EEPROM to the FC durin
---
### nav_wp_max_safe_distance
First waypoint in the mission should be closer than this value [m]. A value of 0 disables this check.
| Default | Min | Max |
| --- | --- | --- |
| 100 | 0 | 1500 |
---
### nav_wp_mission_restart
Sets restart behaviour for a WP mission when interrupted mid mission. START from first WP, RESUME from last active WP or SWITCH between START and RESUME each time WP Mode is reselected ON. SWITCH effectively allows resuming once only from a previous mid mission waypoint after which the mission will restart from the first waypoint.
@ -3842,16 +3852,6 @@ Waypoint radius [cm]. Waypoint would be considered reached if machine is within
---
### nav_wp_safe_distance
First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check.
| Default | Min | Max |
| --- | --- | --- |
| 10000 | | 65000 |
---
### opflow_hardware
Selection of OPFLOW hardware.
@ -3912,6 +3912,16 @@ Max pitch, in degrees, for OSD artificial horizon
---
### osd_ahi_pitch_interval
Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD)
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 30 |
---
### osd_ahi_reverse_roll
Switches the artificial horizon in the OSD to instead be a bank indicator, by reversing the direction of its movement.

View file

@ -0,0 +1,16 @@
# Board - MATEKSYS F722-PX/WPX/HD
## Vendor Information / specification
http://www.mateksys.com/?portfolio=f722-px
http://www.mateksys.com/?portfolio=f722-wpx
http://www.mateksys.com/?portfolio=f722-hd
## Firmware
Three firmware variants are available.
* `inav_x.y.z_MATEKF722PX.hex`
* `inav_x.y.z_MATEKF722WPX.hex`
* `inav_x.y.z_MATEKF722PX_PINIO.hex`
The WPX vairant is for the MATEK F722-WPX wing flight controller. The PX variants are for the MATEK F722-PX and MATEK F722-HD flight controllers. The PINIO variant adds USER 3 PINIO as a replacement to UART 5 TX and USER 4 PINIO as a replacement to UART 5 RX.

View file

@ -71,7 +71,6 @@ More target options:
* Integrated current meter
* Uses target **OMNIBUSF4PRO**
* Omnibus F4 Pro clones (Banggood, AliExpress, eBay, etc.) use **OMNIBUSF4PRO_LEDSTRIPM5** target (LED strip on M5 pin instead of incorrectly wired dedicated connection)
* If you want to power your servos from the servo rail you will have to bridge the 5v to another 5v pad from the board or take it from the esc
### Omnibus F4 Pro Corner
@ -195,9 +194,24 @@ SmartPort / FPort is possible without a hardware inverter by using one of the OM
* [Omnibus F4 Pro](https://inavflight.com/shop/p/OMNIBUSF4PRO)
* [Omnibus F4 Nano V6](https://inavflight.com/shop/s/bg/1320256)
## Powering servos and FC (fixed wing)
These boards have a set of diodes which allow you to power servos
and the flight controller in three different ways, without back EMF
from the servos damaging the electronics. Most commonly an ESC
connected to the servo rail provides 5V power for the servos.
If your opto-isolated ESC doesn't provide 5V and you have servos,
connect a 5 BEC to the servo rail (any of the servo outputs 1-4).
The BEC can also power the board electronics, through a protective diode.
Do NOT bridge any other 5V pad to the servo rail, or connect servos
to any other 5V pad on the board. The back EMF from a a servo can
destroy the chips on the board.
# Wiring diagrams for Omnibus F4 Pro
Following diagrams applies to _Pro_ version with integrated current meter and JST connectors only
Following diagrams apply to _Pro_ version with integrated current meter and JST connectors only
## Board layout

View file

@ -71,5 +71,6 @@ typedef enum {
DEBUG_AUTOTUNE,
DEBUG_RATE_DYNAMICS,
DEBUG_LANDING,
DEBUG_POS_EST,
DEBUG_COUNT
} debugType_e;

View file

@ -195,7 +195,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
OSD_SETTING_ENTRY("WP ENFORCE ALTITUDE", SETTING_NAV_WP_ENFORCE_ALTITUDE),
OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_MAX_SAFE_DISTANCE),
#ifdef USE_MULTI_MISSION
OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
#endif

View file

@ -215,6 +215,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("3D SPEED", OSD_3D_SPEED),
OSD_ELEMENT_ENTRY("PLUS CODE", OSD_PLUS_CODE),
OSD_ELEMENT_ENTRY("AZIMUTH", OSD_AZIMUTH),
OSD_ELEMENT_ENTRY("GRD COURSE", OSD_GROUND_COURSE),
OSD_ELEMENT_ENTRY("X TRACK ERR", OSD_CROSS_TRACK_ERROR),
#endif // GPS
OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),

View file

@ -131,7 +131,7 @@ bool mpuTemperatureReadScratchpad(gyroDev_t *gyro, int16_t * data)
if (ctx->lastReadStatus) {
// Convert to degC*10: degC = raw / 340 + 36.53
*data = int16_val(data, 0) / 34 + 365;
*data = int16_val(ctx->tempRaw, 0) / 34 + 365;
return true;
}

View file

@ -171,9 +171,13 @@
#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right
#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left
#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right
#define SYM_FLIGHT_MINS_REMAINING 0xDA // 218 Flight time (mins) remaining
#define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining
#define SYM_GROUND_COURSE 0xDC // 220 Ground course
#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6
#define SYM_LOGO_START 0x101 // 257 to 297, INAV logo
#define SYM_LOGO_WIDTH 10
#define SYM_LOGO_HEIGHT 4
#define SYM_AH_LEFT 0x12C // 300 Arrow left
@ -220,6 +224,7 @@
#define SYM_HOME_DIST 0x165 // 357 DIST
#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center
#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing
#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4

View file

@ -212,10 +212,10 @@ static void updateArmingStatus(void)
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
if (throttleStickIsLow()) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
}
}
@ -630,13 +630,13 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool throttleIsLow = throttleStickIsLow();
// When armed and motors aren't spinning, do beeps periodically
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
static bool armedBeeperOn = false;
if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
@ -645,7 +645,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
processRcStickPositions(throttleStatus);
processRcStickPositions(throttleIsLow);
processAirmode();
updateActivatedModes();
@ -759,7 +759,7 @@ void processRx(timeUs_t currentTimeUs)
pidResetErrorAccumulators();
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) {
ENABLE_STATE(ANTI_WINDUP);
@ -778,7 +778,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
if (throttleStatus == THROTTLE_LOW) {
if (throttleIsLow) {
if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) {
if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
ENABLE_STATE(ANTI_WINDUP);
@ -802,7 +802,7 @@ void processRx(timeUs_t currentTimeUs)
else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
DISABLE_STATE(ANTI_WINDUP);
//This case applies only to MR when Airmode management is throttle threshold activated
if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
pidResetErrorAccumulators();
}
}

View file

@ -84,9 +84,11 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXOSDALT3, .boxName = "OSD ALT 3", .permanentId = 44 },
{ .boxId = BOXNAVCOURSEHOLD, .boxName = "NAV COURSE HOLD", .permanentId = 45 },
{ .boxId = BOXBRAKING, .boxName = "MC BRAKING", .permanentId = 46 },
{ .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 },
{ .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 },
{ .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 },
{ .boxId = BOXUSER1, .boxName = "USER1", .permanentId = BOX_PERMANENT_ID_USER1 }, // 47
{ .boxId = BOXUSER2, .boxName = "USER2", .permanentId = BOX_PERMANENT_ID_USER2 }, // 48
{ .boxId = BOXUSER3, .boxName = "USER3", .permanentId = BOX_PERMANENT_ID_USER3 }, // 57
{ .boxId = BOXUSER4, .boxName = "USER4", .permanentId = BOX_PERMANENT_ID_USER4 }, // 58
{ .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 49 },
{ .boxId = BOXMSPRCOVERRIDE, .boxName = "MSP RC OVERRIDE", .permanentId = 50 },
{ .boxId = BOXPREARM, .boxName = "PREARM", .permanentId = 51 },
{ .boxId = BOXTURTLE, .boxName = "TURTLE", .permanentId = 52 },
@ -94,8 +96,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 },
{ .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 },
{ .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 },
{ .boxId = BOXLOITERDIRCHN, .boxName = "LOITER CHANGE", .permanentId = 57 },
{ .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 58 },
{ .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
};
@ -319,6 +320,7 @@ void initActiveBoxIds(void)
ADD_ACTIVE_BOX(BOXUSER1);
ADD_ACTIVE_BOX(BOXUSER2);
ADD_ACTIVE_BOX(BOXUSER3);
ADD_ACTIVE_BOX(BOXUSER4);
#endif
#if defined(USE_OSD) && defined(OSD_LAYOUT_COUNT)
@ -397,6 +399,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)), BOXUSER1);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)), BOXUSER2);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER3)), BOXUSER3);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER4)), BOXUSER4);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)), BOXLOITERDIRCHN);
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE);

View file

@ -574,7 +574,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
navigationUsePIDs();
break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &currentBatteryProfileMutable->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
break;
#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
case ADJUSTMENT_VTX_POWER_LEVEL:

View file

@ -111,22 +111,25 @@ bool isRollPitchStickDeflected(uint8_t deadband)
throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
{
int value;
if (type == THROTTLE_STATUS_TYPE_RC) {
value = rxGetChannelValue(THROTTLE);
} else {
int value = rxGetChannelValue(THROTTLE); // THROTTLE_STATUS_TYPE_RC
if (type == THROTTLE_STATUS_TYPE_COMMAND) {
value = rcCommand[THROTTLE];
}
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW;
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
bool midThrottle = value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband);
if ((feature(FEATURE_REVERSIBLE_MOTORS) && midThrottle) || (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))) {
return THROTTLE_LOW;
}
return THROTTLE_HIGH;
}
bool throttleStickIsLow(void)
{
return calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
int16_t throttleStickMixedValue(void)
{
int16_t throttleValue;
@ -181,7 +184,7 @@ static void updateRcStickPositions(void)
rcStickPositions = tmp;
}
void processRcStickPositions(throttleStatus_e throttleStatus)
void processRcStickPositions(bool isThrottleLow)
{
static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@ -211,7 +214,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
if (!isThrottleLow) {
tryArm();
return;
}
@ -227,7 +230,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
if (armingConfig()->disarm_kill_switch || isThrottleLow) {
disarm(DISARM_SWITCH);
}
}

View file

@ -112,6 +112,7 @@ bool isRollPitchStickDeflected(uint8_t deadband);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
int16_t throttleStickMixedValue(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);
void processRcStickPositions(bool isThrottleLow);
bool throttleStickIsLow(void);
int32_t getRcStickDeflection(int32_t axis);

View file

@ -75,7 +75,8 @@ typedef enum {
BOXPLANWPMISSION = 46,
BOXSOARING = 47,
BOXUSER3 = 48,
BOXCHANGEMISSION = 49,
BOXUSER4 = 49,
BOXCHANGEMISSION = 50,
CHECKBOX_ITEM_COUNT
} boxId_e;

View file

@ -91,7 +91,7 @@ tables:
values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING"]
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
- name: aux_operator
values: ["OR", "AND"]
enum: modeActivationOperator_e
@ -1048,12 +1048,6 @@ groups:
default_value: 1000
min: PWM_RANGE_MIN
max: PWM_RANGE_MAX
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fwMinThrottleDownPitchAngle
min: 0
max: 450
- name: nav_mc_hover_thr
description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
default_value: 1500
@ -1833,11 +1827,6 @@ groups:
field: fixedWingItermThrowLimit
min: FW_ITERM_THROW_LIMIT_MIN
max: FW_ITERM_THROW_LIMIT_MAX
- name: fw_loiter_direction
description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick."
default_value: "RIGHT"
field: loiter_direction
table: direction
- name: fw_reference_airspeed
description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present."
default_value: 1500
@ -2354,11 +2343,12 @@ groups:
field: general.waypoint_enforce_altitude
min: 0
max: 2000
- name: nav_wp_safe_distance
description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check."
default_value: 10000
- name: nav_wp_max_safe_distance
description: "First waypoint in the mission should be closer than this value [m]. A value of 0 disables this check."
default_value: 100
field: general.waypoint_safe_distance
max: 65000
min: 0
max: 1500
- name: nav_wp_mission_restart
description: "Sets restart behaviour for a WP mission when interrupted mid mission. START from first WP, RESUME from last active WP or SWITCH between START and RESUME each time WP Mode is reselected ON. SWITCH effectively allows resuming once only from a previous mid mission waypoint after which the mission will restart from the first waypoint."
default_value: "RESUME"
@ -2674,6 +2664,12 @@ groups:
field: fw.pitch_to_throttle_smooth
min: 0
max: 9
- name: fw_min_throttle_down_pitch
description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)"
default_value: 0
field: fw.minThrottleDownPitchAngle
min: 0
max: 450
- name: nav_fw_pitch2thr_threshold
description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]"
default_value: 50
@ -2686,6 +2682,11 @@ groups:
field: fw.loiter_radius
min: 0
max: 30000
- name: fw_loiter_direction
description: "Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick."
default_value: "RIGHT"
field: fw.loiter_direction
table: direction
- name: nav_fw_cruise_speed
description: "Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s"
default_value: 0
@ -3429,6 +3430,13 @@ groups:
default_value: 3
description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
- name: osd_ahi_pitch_interval
field: ahi_pitch_interval
min: 0
max: 30
default_value: 0
description: Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD)
- name: osd_home_position_arm_screen
type: bool
default_value: ON

View file

@ -387,7 +387,7 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
if (!throttleStickIsLow()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (!receivingRxDataAndNotFailsafeMode) {

View file

@ -626,11 +626,8 @@ motorStatus_e getMotorStatus(void)
}
const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
const bool throttleStickLow =
(calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);
if (throttleStickLow && fixedWingOrAirmodeNotActive) {
if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
// If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
// and either on a plane or on a quad with inactive airmode - stop motor
@ -652,7 +649,6 @@ motorStatus_e getMotorStatus(void)
return MOTOR_STOPPED_USER;
}
}
}
return MOTOR_RUNNING;

View file

@ -171,7 +171,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
static EXTENDED_FASTRAM float fixedWingLevelTrim;
static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 4);
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 5);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
@ -277,7 +277,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
.fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
.navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
.navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT,
.navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT,
@ -596,7 +595,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
// Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, currentBatteryProfile->fwMinThrottleDownPitchAngle);
angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
}
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming

View file

@ -133,7 +133,6 @@ typedef struct pidProfile_s {
float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point
uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
float navVelXyDTermLpfHz;
uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity

View file

@ -30,8 +30,6 @@
FILE_COMPILE_FOR_SPEED
//#define MSP_DISPLAYPORT_STATS
#if defined(USE_OSD) && defined(USE_MSP_OSD)
#include "common/utils.h"
@ -39,10 +37,16 @@ FILE_COMPILE_FOR_SPEED
#include "common/time.h"
#include "common/bitarray.h"
#include "cms/cms.h"
#include "drivers/display.h"
#include "drivers/display_font_metadata.h"
#include "drivers/osd_symbols.h"
#include "fc/rc_modes.h"
#include "io/osd.h"
#include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
@ -58,7 +62,7 @@ FILE_COMPILE_FOR_SPEED
typedef enum { // defines are from hdzero code
SD_3016,
HD_5018,
HD_3016,
HD_3016, // Special HDZERO mode that just sends the centre 30x16 of the 50x18 canvas to the VRX
HD_6022 // added to support DJI wtfos 60x22 grid
} resolutionType_e;
@ -87,43 +91,25 @@ static timeMs_t vtxHeartbeat;
// DJIWTF screen size
#define DJI_COLS 60
#define DJI_ROWS 22
// set COLS and ROWS to largest size available
#define COLS DJI_COLS
#define ROWS DJI_ROWS
// set screen size
#define SCREENSIZE (ROWS*COLS)
static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
static uint8_t screen[SCREENSIZE];
static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen
static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen
static bool screenCleared;
static uint8_t screenRows;
static uint8_t screenCols;
static uint8_t screenRows, screenCols;
static videoSystem_e osdVideoSystem;
extern uint8_t cliMode;
#ifdef MSP_DISPLAYPORT_STATS
static uint32_t dataSent;
static uint8_t resetCount;
#endif
static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
{
UNUSED(displayPort);
int sent = 0;
if (!cliMode && vtxActive) {
sent = mspSerialPushPort(cmd, subcmd, len, &mspPort, MSP_V1);
}
#ifdef MSP_DISPLAYPORT_STATS
dataSent += sent;
#endif
return sent;
}
static void checkVtxPresent(void)
{
if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) {
@ -131,25 +117,51 @@ static void checkVtxPresent(void)
}
}
static int setHdMode(displayPort_t *displayPort)
static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len)
{
uint8_t subSubcmd = 0;
switch(osdVideoSystem)
{
case VIDEO_SYSTEM_DJIWTF:
subSubcmd = HD_6022;
break;
case VIDEO_SYSTEM_HDZERO:
subSubcmd = HD_5018;
break;
default:
subSubcmd = SD_3016;
break;
}
UNUSED(displayPort);
checkVtxPresent();
uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, subSubcmd }; // font selection, mode (SD/HD)
int sent = 0;
if (!cliMode && vtxActive) {
sent = mspSerialPushPort(cmd, subcmd, len, &mspPort, MSP_V1);
}
return sent;
}
static uint8_t determineHDZeroOsdMode(void)
{
if (cmsInMenu) {
return HD_5018;
}
// Check if all visible widgets are in the center 30x16 chars of the canvas.
int activeLayout = osdGetActiveLayout(NULL);
osd_items_e index = 0;
do {
index = osdIncElementIndex(index);
uint16_t pos = osdLayoutsConfig()->item_pos[activeLayout][index];
if (OSD_VISIBLE(pos)) {
uint8_t elemPosX = OSD_X(pos);
uint8_t elemPosY = OSD_Y(pos);
if (!osdItemIsFixed(index) && (elemPosX < 10 || elemPosX > 39 || elemPosY == 0 || elemPosY == 17)) {
return HD_5018;
}
}
} while (index > 0);
return HD_3016;
}
static int setDisplayMode(displayPort_t *displayPort)
{
if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
currentOsdMode = determineHDZeroOsdMode(); // Can change between layouts
}
uint8_t subcmd[] = { MSP_SET_OPTIONS, 0, currentOsdMode }; // Font selection, mode (SD/HD)
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
@ -164,9 +176,17 @@ static int clearScreen(displayPort_t *displayPort)
{
uint8_t subcmd[] = { MSP_CLEAR_SCREEN };
if (!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) { // OSD is off
output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
subcmd[0] = MSP_DRAW_SCREEN;
vtxReset = true;
}
else {
init();
setHdMode(displayPort);
setDisplayMode(displayPort);
screenCleared = true;
}
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
@ -174,7 +194,7 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
{
UNUSED(displayPort);
uint16_t pos = (row * screenCols) + col;
uint16_t pos = (row * COLS) + col;
if (pos >= SCREENSIZE) {
return false;
}
@ -210,7 +230,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
UNUSED(displayPort);
UNUSED(attr);
return setChar((row * screenCols) + col, c);
return setChar((row * COLS) + col, c);
}
static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string, textAttributes_t attr)
@ -218,47 +238,13 @@ static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, con
UNUSED(displayPort);
UNUSED(attr);
uint16_t pos = (row * screenCols) + col;
uint16_t pos = (row * COLS) + col;
while (*string) {
setChar(pos++, *string++);
}
return 0;
}
#ifdef MSP_DISPLAYPORT_STATS
static void printStats(displayPort_t *displayPort, uint32_t updates)
{
static timeMs_t lastTime;
static uint32_t maxDataSent, maxBufferUsed, maxUpdates;
timeMs_t currentTime = millis();
char lineBuffer[100];
if (updates > maxUpdates) {
maxUpdates = updates; // updates sent per displayWrite
}
uint32_t bufferUsed = TX_BUFFER_SIZE - serialTxBytesFree(mspPort.port);
if (bufferUsed > maxBufferUsed) {
maxBufferUsed = bufferUsed; // serial buffer used after displayWrite
}
uint32_t diff = (currentTime - lastTime);
if (diff > 1000) { // Data sampled in 1 second
if (dataSent > maxDataSent) {
maxDataSent = dataSent; // bps (max 11520 allowed)
}
dataSent = 0;
lastTime = currentTime;
}
tfp_sprintf(lineBuffer, "R:%2d %4ld %5ld(%5ld) U:%2ld(%2ld) B:%3ld(%4ld,%4ld)", resetCount, (millis()-vtxHeartbeat),
dataSent, maxDataSent, updates, maxUpdates, bufferUsed, maxBufferUsed, mspPort.port->txBufferSize);
writeString(displayPort, 0, 17, lineBuffer, 0);
}
#endif
/**
* Write only changed characters to the VTX
*/
@ -266,7 +252,11 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
{
static uint8_t counter = 0;
if ((counter++ % DRAW_FREQ_DENOM) == 0) {
if ((!cmsInMenu && IS_RC_MODE_ACTIVE(BOXOSD)) || (counter++ % DRAW_FREQ_DENOM)) {
return 0;
}
uint8_t subcmd[COLS + 4];
uint8_t updateCount = 0;
subcmd[0] = MSP_WRITE_STRING;
@ -275,9 +265,9 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
while (next >= 0) {
// Look for sequential dirty characters on the same line for the same font page
int pos = next;
uint8_t row = pos / screenCols;
uint8_t col = pos % screenCols;
int endOfLine = row * screenCols + screenCols;
uint8_t row = pos / COLS;
uint8_t col = pos % COLS;
int endOfLine = row * COLS + screenCols;
bool page = bitArrayGet(fontPage, pos);
uint8_t len = 4;
@ -302,32 +292,23 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
if (screenCleared) {
screenCleared = false;
}
subcmd[0] = MSP_DRAW_SCREEN;
output(displayPort, MSP_DISPLAYPORT, subcmd, 1);
}
#ifdef MSP_DISPLAYPORT_STATS
printStats(displayPort, updateCount);
#endif
checkVtxPresent();
if (vtxReset) {
#ifdef MSP_DISPLAYPORT_STATS
resetCount++;
#endif
clearScreen(displayPort);
vtxReset = false;
}
}
return 0;
return 0;
}
static void resync(displayPort_t *displayPort)
{
displayPort->rows = screenRows;
displayPort->cols = screenCols;
setHdMode(displayPort);
}
static int screenSize(const displayPort_t *displayPort)
@ -432,25 +413,25 @@ bool mspOsdSerialInit(void)
displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
{
if (mspOsdSerialInit()) {
init();
displayInit(&mspOsdDisplayPort, &mspOsdVTable);
osdVideoSystem = videoSystem;
switch (videoSystem)
{
switch(videoSystem) {
case VIDEO_SYSTEM_AUTO:
case VIDEO_SYSTEM_PAL:
currentOsdMode = SD_3016;
screenRows = PAL_ROWS;
screenCols = PAL_COLS;
break;
case VIDEO_SYSTEM_NTSC:
currentOsdMode = SD_3016;
screenRows = NTSC_ROWS;
screenCols = NTSC_COLS;
break;
case VIDEO_SYSTEM_HDZERO:
currentOsdMode = HD_5018;
screenRows = HDZERO_ROWS;
screenCols = HDZERO_COLS;
break;
case VIDEO_SYSTEM_DJIWTF:
currentOsdMode = HD_6022;
screenRows = DJI_ROWS;
screenCols = DJI_COLS;
break;
@ -458,6 +439,10 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
break;
}
osdVideoSystem = videoSystem;
init();
displayInit(&mspOsdDisplayPort, &mspOsdVTable);
return &mspOsdDisplayPort;
}
return NULL;
@ -466,11 +451,11 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
/*
* Intercept MSP processor.
* VTX sends an MSP command every 125ms or so.
* VTX will have be marked as not ready if no commands received within VTX_TIMEOUT.
* VTX will have be marked as not ready if no commands received within VTX_TIMEOUT ms.
*/
static mspResult_e processMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
if (vtxSeen && !vtxActive) {
if ((vtxSeen && !vtxActive) || (cmd->cmd == MSP_EEPROM_WRITE)) {
vtxReset = true;
}

View file

@ -1779,6 +1779,19 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_GROUND_COURSE:
{
buff[0] = SYM_GROUND_COURSE;
if (osdIsHeadingValid()) {
tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
} else {
buff[1] = buff[2] = buff[3] = '-';
}
buff[4] = SYM_DEGREES;
buff[5] = '\0';
break;
}
case OSD_COURSE_HOLD_ERROR:
{
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
@ -1825,6 +1838,18 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_CROSS_TRACK_ERROR:
{
if (isWaypointNavTrackingActive()) {
buff[0] = SYM_CROSS_TRACK_ERROR;
osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0);
} else {
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
return true;
}
break;
}
case OSD_GPS_HDOP:
{
buff[0] = SYM_HDP_L;
@ -1930,20 +1955,20 @@ static bool osdDrawSingleElement(uint8_t item)
}
#endif
if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
buff[0] = SYM_FLY_M;
buff[0] = SYM_FLIGHT_MINS_REMAINING;
strcpy(buff + 1, "--:--");
#if defined(USE_ADC) && defined(USE_GPS)
updatedTimestamp = 0;
#endif
} else if (timeSeconds == -2) {
// Wind is too strong to come back with cruise throttle
buff[0] = SYM_FLY_M;
buff[0] = SYM_FLIGHT_MINS_REMAINING;
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);
osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING);
if (timeSeconds == 0)
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
@ -1964,7 +1989,8 @@ static bool osdDrawSingleElement(uint8_t item)
updatedTimestamp = currentTimeUs;
}
#endif
buff[0] = SYM_TRIP_DIST;
//buff[0] = SYM_TRIP_DIST;
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING);
if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
buff[4] = SYM_BLANK;
buff[5] = '\0';
@ -2540,7 +2566,7 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)currentBatteryProfile->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
return true;
case OSD_FW_ALT_PID_OUTPUTS:
@ -3217,7 +3243,7 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
}
static uint8_t osdIncElementIndex(uint8_t elementIndex)
uint8_t osdIncElementIndex(uint8_t elementIndex)
{
++elementIndex;
@ -3373,6 +3399,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
.right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
.sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT,
.ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT,
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
@ -3434,8 +3461,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_GROUND_COURSE] = OSD_POS(12, 3);
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3);
osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
@ -3637,7 +3666,7 @@ static void osdCompleteAsyncInitialization(void)
char string_buffer[30];
tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING);
uint8_t xPos = osdDisplayIsHD() ? 7 : 5;
uint8_t xPos = osdDisplayIsHD() ? 15 : 5;
displayWrite(osdDisplayPort, xPos, y++, string_buffer);
#ifdef USE_CMS
displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1);
@ -3647,8 +3676,8 @@ static void osdCompleteAsyncInitialization(void)
#ifdef USE_STATS
uint8_t statNameX = osdDisplayIsHD() ? 7 : 4;
uint8_t statValueX = osdDisplayIsHD() ? 27 : 24;
uint8_t statNameX = osdDisplayIsHD() ? 14 : 4;
uint8_t statValueX = osdDisplayIsHD() ? 34 : 24;
if (statsConfig()->stats_enabled) {
displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:");
@ -3809,8 +3838,8 @@ static void osdShowStatsPage1(void)
{
const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
uint8_t top = 1; /* first fully visible line */
const uint8_t statNameX = osdDisplayIsHD() ? 7 : 1;
const uint8_t statValuesX = osdDisplayIsHD() ? 33 : 20;
const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20;
char buff[10];
statsPagesCheck = 1;
@ -3884,8 +3913,8 @@ static void osdShowStatsPage1(void)
static void osdShowStatsPage2(void)
{
uint8_t top = 1; /* first fully visible line */
const uint8_t statNameX = osdDisplayIsHD() ? 7 : 1;
const uint8_t statValuesX = osdDisplayIsHD() ? 33 : 20;
const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20;
char buff[10];
statsPagesCheck = 1;

View file

@ -266,6 +266,8 @@ typedef enum {
OSD_GLIDE_RANGE,
OSD_CLIMB_EFFICIENCY,
OSD_NAV_WP_MULTI_MISSION_INDEX,
OSD_GROUND_COURSE, // 140
OSD_CROSS_TRACK_ERROR,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@ -427,6 +429,7 @@ typedef struct osdConfig_s {
uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers.
uint16_t system_msg_display_time; // system message display time for multiple messages (ms)
uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD)
char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0.
char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
@ -457,6 +460,7 @@ void osdOverrideLayout(int layout, timeMs_t duration);
// set by the user configuration (modes, etc..) or by overriding it.
int osdGetActiveLayout(bool *overridden);
bool osdItemIsFixed(osd_items_e item);
uint8_t osdIncElementIndex(uint8_t elementIndex);
displayPort_t *osdGetDisplayPort(void);
displayCanvas_t *osdGetDisplayPortCanvas(void);

View file

@ -438,7 +438,7 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can
void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading)
{
static const uint16_t graph[] = {
static const uint8_t graph[] = {
SYM_HEADING_W,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,

View file

@ -953,7 +953,7 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
break;
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
tfp_sprintf(buff, "MTDPA %4d", currentBatteryProfileMutable->fwMinThrottleDownPitchAngle);
tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
break;
case ADJUSTMENT_TPA:
tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);

View file

@ -140,12 +140,27 @@ void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned
}
}
int8_t ahiPitchAngleDatum; // sets the pitch datum AHI is drawn relative to (degrees)
int8_t ahiLineEndPitchOffset; // AHI end of line offset in degrees when ahiPitchAngleDatum > 0
if (osdConfig()->ahi_pitch_interval) {
ahiPitchAngleDatum = osdConfig()->ahi_pitch_interval * (int8_t)(RADIANS_TO_DEGREES(pitchAngle) / osdConfig()->ahi_pitch_interval);
pitchAngle -= DEGREES_TO_RADIANS(ahiPitchAngleDatum);
} else {
ahiPitchAngleDatum = 0;
}
if (fabsf(ky) < fabsf(kx)) {
previous_orient = 0;
/* ahi line ends drawn with 3 deg offset when ahiPitchAngleDatum > 0
* Line end offset increased by 1 deg with every 20 deg pitch increase */
const int8_t ahiLineEndOffsetFactor = ahiPitchAngleDatum / 20;
for (int8_t dx = -OSD_AHI_WIDTH / 2; dx <= OSD_AHI_WIDTH / 2; dx++) {
float fy = (ratio * dx) * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f;
ahiLineEndPitchOffset = ahiPitchAngleDatum && (dx == -OSD_AHI_WIDTH / 2 || dx == OSD_AHI_WIDTH / 2) ? -(ahiLineEndOffsetFactor + 3 * ABS(ahiPitchAngleDatum) / ahiPitchAngleDatum) : 0;
float fy = (ratio * dx) * (ky / kx) + (pitchAngle + DEGREES_TO_RADIANS(ahiLineEndPitchOffset)) * pitch_rad_to_char + 0.49f;
int8_t dy = floorf(fy);
const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
uint16_t c;

View file

@ -29,7 +29,8 @@
#define BOX_PERMANENT_ID_USER1 47
#define BOX_PERMANENT_ID_USER2 48
#define BOX_PERMANENT_ID_USER3 49
#define BOX_PERMANENT_ID_USER3 57
#define BOX_PERMANENT_ID_USER4 58
#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode

View file

@ -99,7 +99,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -127,7 +127,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
// General navigation parameters
.pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
.waypoint_safe_distance = SETTING_NAV_WP_MAX_SAFE_DISTANCE_DEFAULT, // Metres - first waypoint should be closer than this
#ifdef USE_MULTI_MISSION
.waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
#endif
@ -185,7 +185,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
//Fixed wing landing
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
@ -198,7 +200,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
.launch_wiggle_wake_idle = SETTING_NAV_FW_LAUNCH_WIGGLE_TO_WAKE_IDLE_DEFAULT, // uint8_t
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to greaually increase throttle from idle to launch
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
@ -2118,7 +2120,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
/*-----------------------------------------------------------
* Processes an update to estimated heading
*-----------------------------------------------------------*/
void updateActualHeading(bool headingValid, int32_t newHeading)
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
{
/* Update heading. Check if we're acquiring a valid heading for the
* first time and update home heading accordingly.
@ -2149,7 +2151,9 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
}
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
}
/* Use course over ground for fixed wing nav "heading" when valid - TODO use heading and cog as specifically required for FW and MR */
posControl.actualState.yaw = newHeading;
posControl.actualState.cog = newGroundCourse; // currently only used for OSD display
posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */
@ -2224,7 +2228,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3
static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
{
// Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP.
if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) {
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && !isLastMissionWaypoint()) {
navWaypointActions_e nextWpAction = posControl.waypointList[posControl.activeWaypointIndex + 1].action;
if (!(nextWpAction == NAV_WP_ACTION_SET_POI || nextWpAction == NAV_WP_ACTION_SET_HEAD)) {
@ -2264,6 +2268,11 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
}
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
// If WP turn smoothing CUT option used WP is reached when start of turn is initiated
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
posControl.flags.wpTurnSmoothingActive = false;
return true;
}
// Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
// Same method for turn smoothing option but relative bearing set at 60 degrees
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
@ -2641,10 +2650,17 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
* Reverts to normal RTH heading direct to home when end of track reached.
* Trackpoints logged with precedence for course/altitude changes. Distance based changes
* only logged if no course/altitude changes logged over an extended distance.
* Tracking suspended during fixed wing loiter (PosHold and WP Mode timed hold).
* --------------------------------------------------------------------------------- */
static void updateRthTrackback(bool forceSaveTrackPoint)
{
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED)) {
static bool suspendTracking = false;
bool fwLoiterIsActive = STATE(AIRPLANE) && (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED || FLIGHT_MODE(NAV_POSHOLD_MODE));
if (!fwLoiterIsActive && suspendTracking) {
suspendTracking = false;
}
if (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_OFF || FLIGHT_MODE(NAV_RTH_MODE) || !ARMING_FLAG(ARMED || suspendTracking)) {
return;
}
@ -2693,6 +2709,11 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
saveTrackpoint = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.activeRthTBPointIndex]) > METERS_TO_CENTIMETERS(20);
previousTBTripDist = posControl.totalTripDistance;
}
// Suspend tracking during loiter on fixed wing. Save trackpoint at start of loiter.
if (distanceCounter && fwLoiterIsActive) {
saveTrackpoint = suspendTracking = true;
}
}
// when trackpoint store full, overwrite from start of store using 'rthTBWrapAroundCounter' to track overwrite position
@ -2892,9 +2913,16 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
static bool targetHoldActive = false;
// Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way
targetHoldActive = false;
} else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
targetHoldActive = true;
}
}
}
else {
@ -3853,7 +3881,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
// Don't allow arming if first waypoint is farther than configured safe distance
if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
if (distanceToFirstWP() > navConfig()->general.waypoint_safe_distance && !checkStickPosition(YAW_HI)) {
if (distanceToFirstWP() > METERS_TO_CENTIMETERS(navConfig()->general.waypoint_safe_distance) && !checkStickPosition(YAW_HI)) {
return NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR;
}
}
@ -4309,8 +4337,7 @@ bool isNavLaunchEnabled(void)
bool abortLaunchAllowed(void)
{
// allow NAV_LAUNCH_MODE to be aborted if throttle is low or throttle stick position is < launch idle throttle setting
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
return throttleStatus == THROTTLE_LOW || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
return throttleStickIsLow() || throttleStickMixedValue() < currentBatteryProfile->nav.fw.launch_idle_throttle;
}
int32_t navigationGetHomeHeading(void)

View file

@ -294,7 +294,9 @@ typedef struct navConfig_s {
uint8_t control_smoothness; // The amount of smoothing to apply to controls for navigation
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
uint16_t minThrottleDownPitchAngle; // Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle. [decidegrees]
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
int8_t land_dive_angle;
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s)
@ -608,6 +610,7 @@ void updateLandingStatus(void);
const navigationPIDControllers_t* getNavigationPIDControllers(void);
int32_t navigationGetHeadingError(void);
float navigationGetCrossTrackError(void);
int32_t getCruiseHeadingAdjustment(void);
bool isAdjustingPosition(void);
bool isAdjustingHeading(void);

View file

@ -72,6 +72,7 @@ static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static float navCrossTrackError;
static int8_t loiterDirYaw = 1;
bool needToCalculateCircularLoiter;
@ -240,11 +241,11 @@ void resetFixedWingPositionController(void)
static int8_t loiterDirection(void) {
int8_t dir = 1; //NAV_LOITER_RIGHT
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) {
if (navConfig()->fw.loiter_direction == NAV_LOITER_LEFT) {
dir = -1;
}
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (navConfig()->fw.loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise
@ -285,30 +286,28 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
/* WP turn smoothing option - switches to loiter path around waypoint using navLoiterRadius.
/* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP
* Works for turns > 30 degs and < 160 degs.
* Option 1 switches to loiter path around waypoint using navLoiterRadius.
* Loiter centered on point inside turn at required distance from waypoint and
* on a bearing midway between current and next waypoint course bearings.
* Works for turns > 30 degs and < 120 degs.
* 2 options, 1 = pass through WP, 2 = cut inside turn missing WP */
* Option 2 simply uses a normal turn once the turn initiation point is reached */
int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle);
posControl.flags.wpTurnSmoothingActive = false;
if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// turnFactor adjusts start of loiter based on turn angle
float turnFactor = 0.0f;
if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// turnStartFactor adjusts start of loiter based on turn angle
float turnStartFactor;
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP
turnFactor = waypointTurnAngle / 6000.0f;
} else {
turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); // cut inside turn missing WP
turnStartFactor = waypointTurnAngle / 6000.0f;
} else { // // cut inside turn missing WP
turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f);
}
constrainf(turnFactor, 0.5f, 2.0f);
if (posControl.wpDistance < navLoiterRadius * turnFactor) {
// velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time
if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) {
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) {
int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000);
float distToTurnCentre = navLoiterRadius;
if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f));
}
loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + distToTurnCentre * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing));
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
@ -316,7 +315,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// turn direction to next waypoint
loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right
needToCalculateCircularLoiter = posControl.flags.wpTurnSmoothingActive = true;
needToCalculateCircularLoiter = true;
}
posControl.flags.wpTurnSmoothingActive = true;
}
}
@ -396,26 +397,31 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
/* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// only apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m)
if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
// tracking only active when certain distance and heading conditions are met
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
// captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
// Closing distance threashold based on speed and an assumed 1 second response time.
float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
// initial courseCorrectionFactor based on distance to course line
float courseCorrectionFactor = constrainf(distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// course heading alignment factor
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f);
float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
// final courseCorrectionFactor combining distance and heading factors
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line
// final courseVirtualCorrection value
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
}
@ -680,12 +686,11 @@ bool isFixedWingLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static bool fixAxisCheck = false;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
// Basic condition to start looking for landing
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| FLIGHT_MODE(FAILSAFE_MODE)
|| (!navigationIsControllingThrottle() && throttleIsLow);
|| (!navigationIsControllingThrottle() && throttleStickIsLow());
if (!startCondition || posControl.flags.resetLandingDetector) {
return fixAxisCheck = posControl.flags.resetLandingDetector = false;
@ -820,3 +825,8 @@ int32_t navigationGetHeadingError(void)
{
return navHeadingError;
}
float navigationGetCrossTrackError(void)
{
return navCrossTrackError;
}

View file

@ -261,11 +261,6 @@ static void applyThrottleIdleLogic(bool forceMixerIdle)
}
}
static inline bool isThrottleLow(void)
{
return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
}
static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
static timeMs_t wiggleTime = 0;
static timeMs_t wigglesTime = 0;
@ -326,7 +321,7 @@ static inline bool isProbablyNotFlying(void)
static void resetPidsIfNeeded(void) {
// Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && isThrottleLow())) {
if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) {
pidResetErrorAccumulators();
pidResetTPAFilter();
}
@ -346,7 +341,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
{
UNUSED(currentTimeUs);
if (!isThrottleLow()) {
if (!throttleStickIsLow()) {
if (isThrottleIdleEnabled()) {
return FW_LAUNCH_EVENT_SUCCESS;
}
@ -365,7 +360,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
}
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs) {
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -385,7 +380,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(tim
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -404,7 +399,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(tim
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -423,7 +418,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t
static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
{
if (isThrottleLow()) {
if (throttleStickIsLow()) {
return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
}
@ -501,7 +496,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
if (navConfig()->fw.launch_manual_throttle) {
// reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
if (isThrottleLow()) {
if (throttleStickIsLow()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
fwLaunch.pitchAngle = 0;
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {

View file

@ -176,14 +176,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void)
{
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
const bool throttleIsLow = throttleStickIsLow();
if (navConfig()->general.flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
// If throttle status is THROTTLE_LOW - use Thr Mid anyway
if (throttleStatus == THROTTLE_LOW) {
// If throttle is LOW - use Thr Mid anyway
if (throttleIsLow) {
altHoldThrottleRCZero = rcLookupThrottleMid();
}
else {
@ -198,7 +198,7 @@ void setupMulticopterAltitudeController(void)
// Force AH controller to initialize althold integral for pending takeoff on reset
// Signal for that is low throttle _and_ low actual altitude
if (throttleStatus == THROTTLE_LOW && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
if (throttleIsLow && fabsf(navGetCurrentActualPositionAndVelocity()->pos.z) <= 50.0f) {
prepareForTakeoffOnReset = true;
}
}
@ -724,13 +724,12 @@ bool isMulticopterLandingDetected(void)
{
DEBUG_SET(DEBUG_LANDING, 4, 0);
static timeUs_t landingDetectorStartedAt;
const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
/* Basic condition to start looking for landing
* Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
|| (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
|| (!navigationIsFlyingAutonomousMode() && throttleIsLow);
|| (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
if (!startCondition || posControl.flags.resetLandingDetector) {
landingDetectorStartedAt = 0;

View file

@ -36,6 +36,7 @@
#include "fc/config.h"
#include "fc/settings.h"
#include "fc/rc_modes.h"
#include "flight/imu.h"
@ -686,6 +687,20 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
return false;
}
static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
{
if (STATE(GPS_FIX) && navIsHeadingUsable()) {
static timeUs_t lastUpdateTimeUs = 0;
if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate
const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
lastUpdateTimeUs = currentTimeUs;
}
}
}
/**
* Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
* Function is called at main loop rate
@ -758,6 +773,9 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
}
}
/* Update ground course */
estimationCalculateGroundCourse(currentTimeUs);
/* Update uncertainty */
posEstimator.est.eph = ctx.newEPH;
posEstimator.est.epv = ctx.newEPV;
@ -774,8 +792,10 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
{
static navigationTimer_t posPublishTimer;
/* IMU operates in decidegrees while INAV operates in deg*100 */
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw));
/* IMU operates in decidegrees while INAV operates in deg*100
* Use course over ground for fixed wing navigation yaw/"heading" */
int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? posEstimator.est.cog : attitude.values.yaw;
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue), DECIDEGREES_TO_CENTIDEGREES(posEstimator.est.cog));
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
@ -800,6 +820,23 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
//Update Blackbox states
navEPH = posEstimator.est.eph;
navEPV = posEstimator.est.epv;
DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F); // Position estimate X
DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F); // Position estimate Y
if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){
// SURFACE (following) MODE
DEBUG_SET(DEBUG_POS_EST, 2, (int32_t) posControl.actualState.agl.pos.z*1000.0F); // Position estimate Z
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t) posControl.actualState.agl.vel.z*1000.0F); // Speed estimate VZ
} else {
DEBUG_SET(DEBUG_POS_EST, 2, (int32_t) posEstimator.est.pos.z*1000.0F); // Position estimate Z
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t) posEstimator.est.vel.z*1000.0F); // Speed estimate VZ
}
DEBUG_SET(DEBUG_POS_EST, 3, (int32_t) posEstimator.est.vel.x*1000.0F); // Speed estimate VX
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t) posEstimator.est.vel.y*1000.0F); // Speed estimate VY
DEBUG_SET(DEBUG_POS_EST, 6, (int32_t) attitude.values.yaw); // Yaw estimate (4 bytes still available here)
DEBUG_SET(DEBUG_POS_EST, 7, (int32_t) (posEstimator.flags & 0b1111111)<<20 | // navPositionEstimationFlags fit into 8bits
(MIN(navEPH, 1000) & 0x3FF)<<10 |
(MIN(navEPV, 1000) & 0x3FF)); // Horizontal and vertical uncertainties (max value = 1000, fit into 20bits)
}
}

View file

@ -39,6 +39,7 @@
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
#define INAV_PITOT_UPDATE_RATE 10
#define INAV_COG_UPDATE_RATE_HZ 20 // ground course update rate
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
@ -125,6 +126,9 @@ typedef struct {
// FLOW
float flowCoordinates[2];
// COURSE
int16_t cog; // course over ground (decidegrees)
} navPositionEstimatorESTIMATE_t;
typedef struct {

View file

@ -124,6 +124,7 @@ typedef struct {
navEstimatedPosVel_t abs;
navEstimatedPosVel_t agl;
int32_t yaw;
int32_t cog;
// Service values
float sinYaw;
@ -455,7 +456,7 @@ bool isLastMissionWaypoint(void);
float getActiveWaypointSpeed(void);
bool isWaypointNavTrackingActive(void);
void updateActualHeading(bool headingValid, int32_t newHeading);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);

View file

@ -86,10 +86,11 @@ void pgResetFn_logicConditions(logicCondition_t *instance)
logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
static int logicConditionCompute(
int32_t currentVaue,
int32_t currentValue,
logicOperation_e operation,
int32_t operandA,
int32_t operandB
int32_t operandB,
uint8_t lcIndex
) {
int temporaryValue;
vtxDeviceCapability_t vtxDeviceCapability;
@ -104,6 +105,13 @@ static int logicConditionCompute(
return operandA == operandB;
break;
case LOGIC_CONDITION_APPROX_EQUAL:
{
uint16_t offest = operandA / 100;
return ((operandB >= (operandA - offest)) && (operandB <= (operandA + offest)));
}
break;
case LOGIC_CONDITION_GREATER_THAN:
return operandA > operandB;
break;
@ -159,7 +167,67 @@ static int logicConditionCompute(
}
//When both operands are not met, keep current value
return currentVaue;
return currentValue;
break;
case LOGIC_CONDITION_EDGE:
if (operandA && logicConditionStates[lcIndex].timeout == 0 && !(logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED)) {
if (operandB < 100) {
logicConditionStates[lcIndex].timeout = millis();
} else {
logicConditionStates[lcIndex].timeout = millis() + operandB;
}
logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
return true;
} else if (logicConditionStates[lcIndex].timeout > 0) {
if (logicConditionStates[lcIndex].timeout < millis()) {
logicConditionStates[lcIndex].timeout = 0;
} else {
return true;
}
}
if (!operandA) {
logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
}
return false;
break;
case LOGIC_CONDITION_DELAY:
if (operandA) {
if (logicConditionStates[lcIndex].timeout == 0) {
logicConditionStates[lcIndex].timeout = millis() + operandB;
} else if (millis() > logicConditionStates[lcIndex].timeout ) {
logicConditionStates[lcIndex].flags |= LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
return true;
} else if (logicConditionStates[lcIndex].flags & LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED) {
return true;
}
} else {
logicConditionStates[lcIndex].timeout = 0;
logicConditionStates[lcIndex].flags &= ~LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED;
}
return false;
break;
case LOGIC_CONDITION_TIMER:
if ((logicConditionStates[lcIndex].timeout == 0) || (millis() > logicConditionStates[lcIndex].timeout && !currentValue)) {
logicConditionStates[lcIndex].timeout = millis() + operandA;
return true;
} else if (millis() > logicConditionStates[lcIndex].timeout && currentValue) {
logicConditionStates[lcIndex].timeout = millis() + operandB;
return false;
}
return currentValue;
break;
case LOGIC_CONDITION_DELTA:
{
int difference = logicConditionStates[lcIndex].lastValue - operandA;
logicConditionStates[lcIndex].lastValue = operandA;
return ABS(difference) >= operandB;
}
break;
case LOGIC_CONDITION_GVAR_SET:
@ -425,7 +493,8 @@ void logicConditionProcess(uint8_t i) {
logicConditionStates[i].value,
logicConditions(i)->operation,
operandAValue,
operandBValue
operandBValue,
i
);
logicConditionStates[i].value = newValue;
@ -688,6 +757,10 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
return IS_RC_MODE_ACTIVE(BOXUSER3);
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4:
return IS_RC_MODE_ACTIVE(BOXUSER4);
break;
default:
return 0;
break;
@ -781,7 +854,9 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) {
void logicConditionReset(void) {
for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
logicConditionStates[i].value = 0;
logicConditionStates[i].lastValue = 0;
logicConditionStates[i].flags = 0;
logicConditionStates[i].timeout = 0;
}
}

View file

@ -76,7 +76,12 @@ typedef enum {
LOGIC_CONDITION_MAX = 44,
LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE = 45,
LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE = 46,
LOGIC_CONDITION_LAST = 47,
LOGIC_CONDITION_EDGE = 47,
LOGIC_CONDITION_DELAY = 48,
LOGIC_CONDITION_TIMER = 49,
LOGIC_CONDITION_DELTA = 50,
LOGIC_CONDITION_APPROX_EQUAL = 51,
LOGIC_CONDITION_LAST = 52,
} logicOperation_e;
typedef enum logicOperandType_s {
@ -148,6 +153,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD, // 11
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3, // 12
LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4, // 13
} logicFlightModeOperands_e;
typedef enum {
@ -166,6 +172,7 @@ typedef enum {
typedef enum {
LOGIC_CONDITION_FLAG_LATCH = 1 << 0,
LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED = 1 << 1,
} logicConditionFlags_e;
typedef struct logicOperand_s {
@ -186,7 +193,9 @@ PG_DECLARE_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions);
typedef struct logicConditionState_s {
int value;
int32_t lastValue;
uint8_t flags;
timeMs_t timeout;
} logicConditionState_t;
typedef struct rcChannelOverride_s {

View file

@ -167,7 +167,7 @@ bool mspOverrideCalculateChannels(timeUs_t currentTimeUs)
uint16_t sample = (*rxRuntimeConfigMSP.rcReadRawFn)(&rxRuntimeConfigMSP, rawChannel);
// apply the rx calibration to flight channel
if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) {
if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) {
sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
}

View file

@ -166,7 +166,8 @@ static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
{
UNUSED(rxRuntimeConfig);
UNUSED(channel);
return PPM_RCVR_TIMEOUT;
return 0;
}
static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
@ -474,7 +475,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel);
// apply the rx calibration to flight channel
if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) {
if (channel < NON_AUX_CHANNEL_COUNT && sample != 0) {
sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
}

View file

@ -51,8 +51,6 @@
#define RSSI_MAX_VALUE 1023
#define PPM_RCVR_TIMEOUT 0
typedef enum {
RX_FRAME_PENDING = 0, // No new data available from receiver
RX_FRAME_COMPLETE = (1 << 0), // There is new data available
@ -83,7 +81,6 @@ typedef enum {
SERIALRX_MAVLINK,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
#define NON_AUX_CHANNEL_COUNT 4

View file

@ -198,7 +198,6 @@ void schedulerResetTaskStatistics(cfTaskId_e taskId)
} else if (taskId < TASK_COUNT) {
cfTasks[taskId].movingSumExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
cfTasks[taskId].totalExecutionTime = 0;
}
}

View file

@ -94,7 +94,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw
batteryState_e batteryState;
const batteryProfile_t *currentBatteryProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 1);
PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2);
void pgResetFn_batteryProfiles(batteryProfile_t *instance)
{
@ -130,8 +130,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
.failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
.nav = {
.mc = {
@ -549,7 +547,6 @@ void currentMeterUpdate(timeUs_t timeDelta)
case CURRENT_SENSOR_VIRTUAL:
amperage = batteryMetersConfig()->current.offset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE;
bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP));
@ -558,7 +555,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop
throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
} else {
throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
throttleOffset = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
}
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;

View file

@ -106,8 +106,6 @@ typedef struct batteryProfile_s {
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t fwMinThrottleDownPitchAngle;
struct {
struct {

View file

@ -0,0 +1 @@
target_stm32f405xg(AOCODARCF4V2)

View file

@ -0,0 +1,46 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include "platform.h"
#include "drivers/io.h"
#include "drivers/timer.h"
#include "drivers/pwm_mapping.h"
#include "drivers/bus.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0 ), // PPM IN
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S2
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S3
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S4
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S5
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S6
DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S7
DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S8
DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0 ), // LED_STRIP
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

View file

@ -0,0 +1,165 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define USE_TARGET_CONFIG
#define TARGET_BOARD_IDENTIFIER "AOF4V2"
#define USBD_PRODUCT_STRING "AocodaRCF4V2"
// ******** Board LEDs **********************
#define LED0 PC13
// ******* Beeper ***********
#define BEEPER PB8
#define BEEPER_INVERTED
// ******* GYRO and ACC ********
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW0_DEG
#define MPU6500_SPI_BUS BUS_SPI1
#define MPU6500_CS_PIN PA4
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW270_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN PA4
// *************** Baro **************************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB6 // SCL
#define I2C1_SDA PB7 // SDA
#define DEFAULT_I2C_BUS BUS_I2C1
#define USE_BARO
#define BARO_I2C_BUS DEFAULT_I2C_BUS
#define USE_BARO_BMP280
#define USE_BARO_MS5611
//*********** Magnetometer / Compass *************
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_LIS3MDL
// ******* SERIAL ********
#define USE_VCP
#define VBUS_SENSING_PIN PB12
#define VBUS_SENSING_ENABLED
#define USE_UART1
#define UART1_TX_PIN PA9
#define UART1_RX_PIN PA10
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PC11
#define UART3_RX_PIN PC10
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define SERIAL_PORT_COUNT 6
// ******* SPI ********
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_NSS_PIN PA4
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PA13
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_NSS_PIN PC0
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
// ******* ADC ********
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PC3
#define VBAT_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1
#define VBAT_SCALE_DEFAULT 1100
// ******* OSD ********
#define USE_OSD
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PA13
//******* FLASH ********
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PC0
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
//************ LEDSTRIP *****************
#define USE_LED_STRIP
#define WS2811_PIN PB1
// ******* FEATURES ********
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_UART SERIAL_PORT_USART2
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD 0xffff
#define MAX_PWM_OUTPUT_PORTS 10
// ESC-related features
#define USE_DSHOT
#define USE_SERIALSHOT
#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

View file

@ -27,20 +27,18 @@
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6000_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), //PPM
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 D(2, 2, 7)
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 D(2, 3, 7)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 D(2, 4, 7)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 D(2, 7, 7)
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2, 2, 7)
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2, 3, 7)
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2, 4, 7)
DEF_TIM(TIM3, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 D(2, 7, 7)
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S5 D(1, 7, 5)
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 2, 5)
DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S5 D(1, 7, 5)
DEF_TIM(TIM8, CH3N, PB1, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 2, 5)
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S7 D(1, 0, 2)
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR |TIM_USE_FW_SERVO, 0, 0), // S8 D(1, 3, 2)

View file

@ -27,7 +27,7 @@
#define BEEPER PC13
#define BEEPER_INVERTED
// *************** SPI1 Gyro & ACC *******************
// *************** SPI2 Gyro & ACC *******************
#define USE_SPI
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
@ -43,11 +43,6 @@
#define MPU6000_CS_PIN PB12
#define MPU6000_SPI_BUS BUS_SPI2
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW270_DEG
#define MPU6500_CS_PIN PB12
#define MPU6500_SPI_BUS BUS_SPI2
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_CS_PIN PA13
@ -87,7 +82,7 @@
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS BUS_I2C2
// *************** SPI2 OSD ***********************
// *************** SPI1 OSD ***********************
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6

View file

@ -0,0 +1,2 @@
target_stm32f722xe(FOXEERF722V4)
target_stm32f722xe(FOXEERF722V4_X8)

View file

@ -0,0 +1,44 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <platform.h>
#include "drivers/io.h"
#include "drivers/bus.h"
#include "drivers/timer.h"
#include "drivers/sensor.h"
#include "drivers/pwm_mapping.h"
timerHardware_t timerHardware[] = {
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2, 1, 6)
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - D(2, 1, 6)
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(2, 7, 7)
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(2, 4, 7)
#ifdef FOXEERF722V4_X8
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
#endif
DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP(1,5)
};
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

View file

@ -0,0 +1,160 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "FX74"
#define USBD_PRODUCT_STRING "FOXEER F722 V4"
/*** Indicators ***/
#define LED0 PC15
#define BEEPER PA4
#define BEEPER_INVERTED
/*** IMU sensors ***/
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PB2
#define ICM42605_EXTI_PIN PC4
/*** SPI/I2C bus ***/
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
/*** Onboard flash ***/
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PB12
#define M25P16_SPI_BUS BUS_SPI2
/*** OSD ***/
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI3
#define MAX7456_CS_PIN PC3
/*** Serial ports ***/
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#ifdef FOXEERF722V4_X8
//X8 variant without UART2 and UART6
#define SERIAL_PORT_COUNT 5
#else
//Standard variant
#define SERIAL_PORT_COUNT 7
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#endif
/*** BARO & MAG ***/
#define USE_BARO
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_DPS310
#define USE_BARO_SPL06
#define USE_MAG
#define MAG_I2C_BUS BUS_I2C1
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
#define BNO055_I2C_BUS BUS_I2C1
/*** ADC ***/
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC0
#define ADC_CHANNEL_2_PIN PC2
#define ADC_CHANNEL_3_PIN PA0
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define RSSI_ADC_CHANNEL ADC_CHN_3
/*** LED STRIP ***/
#define USE_LED_STRIP
#define WS2811_PIN PA15
/*** Default settings ***/
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#define SERIALRX_UART SERIAL_PORT_USART1
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
#define SERIALRX_PROVIDER SERIALRX_SBUS
/*** Timer/PWM output ***/
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define MAX_PWM_OUTPUT_PORTS 4
#define USE_DSHOT
#define USE_ESC_SENSOR
/*** Used pins ***/
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))

View file

@ -1 +1,2 @@
target_stm32f745xg(FOXEERF745AIO)
target_stm32f745xg(FOXEERF745AIO_V3)

View file

@ -25,12 +25,29 @@
#define BEEPER PD2
#define BEEPER_INVERTED
/*** IMU sensors ***/
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#ifdef FOXEERF745AIO_V3
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define ICM42605_SPI_BUS BUS_SPI3
#define ICM42605_CS_PIN PA15
#else
// MPU6000
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define MPU6000_CS_PIN PA15
#define MPU6000_SPI_BUS BUS_SPI3
#endif
/*** SPI/I2C bus ***/
#define USE_SPI
#define USE_SPI_DEVICE_1

View file

@ -24,9 +24,9 @@
#include "drivers/pinio.h"
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, BMI270_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
timerHardware_t timerHardware[] = {

View file

@ -54,11 +54,6 @@
#define I2C1_SDA PB7
/*** IMU sensors ***/
#define USE_EXTI
#define GYRO_INT_EXTI PC3
#define USE_MPU_DATA_READY_SIGNAL
#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
// MPU6000
@ -66,20 +61,18 @@
#define IMU_MPU6000_ALIGN CW270_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN SPI1_NSS_PIN
#define MPU6000_EXTI_PIN GYRO_INT_EXTI
// ICM42605/ICM42688P
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW270_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN SPI1_NSS_PIN
#define ICM42605_EXTI_PIN GYRO_INT_EXTI
//BMI270
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW270_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN SPI1_NSS_PIN
#define BMI270_EXTI_PIN GYRO_INT_EXTI
/*** OSD ***/
#define USE_MAX7456

View file

@ -47,30 +47,24 @@
#define I2C1_SDA PB9
/*** IMU sensors ***/
#define USE_EXTI
#define GYRO_INT_EXTI PA1
#define USE_MPU_DATA_READY_SIGNAL
// MPU6000
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN SPI1_NSS_PIN
#define MPU6000_EXTI_PIN GYRO_INT_EXTI
// ICM42605/ICM42688P
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW180_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN SPI1_NSS_PIN
#define ICM42605_EXTI_PIN GYRO_INT_EXTI
//BMI270
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN SPI1_NSS_PIN
#define BMI270_EXTI_PIN GYRO_INT_EXTI
/*** OSD ***/
#define USE_MAX7456

View file

@ -25,9 +25,9 @@
#include "drivers/pinio.h"
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, BMI270_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, ICM42605_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM

View file

@ -73,22 +73,16 @@
#define IMU_ICM42605_ALIGN CW180_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PB2
#define ICM42605_EXTI_PIN PC4
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN PB2
#define BMI270_EXTI_PIN PC4
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN PB2
#define MPU6000_EXTI_PIN PC4
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
/*** I2C (Baro & Mag) ***/
#define USE_I2C

View file

@ -1,2 +1,3 @@
target_stm32h743xi(KAKUTEH7)
target_stm32h743xi(KAKUTEH7V2)
target_stm32h743xi(KAKUTEH7MINI)

View file

@ -30,10 +30,14 @@ void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
#ifdef KAKUTEH7V2
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
#endif
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200;
#ifdef KAKUTEH7
#if defined(KAKUTEH7) || defined(KAKUTEH7V2)
//No BT on H7 Mini, do not reserve port
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].msp_baudrateIndex = BAUD_115200;

View file

@ -46,17 +46,32 @@
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#ifdef KAKUTEH7MINI
#if defined(KAKUTEH7MINI)
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN PA4
#define M25P16_SPI_BUS BUS_SPI1
#define USE_FLASH_W25N01G
#define W25N01G_SPI_BUS BUS_SPI1
#define W25N01G_CS_PIN PA4
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#elif defined(KAKUTEH7V2)
#define USE_FLASHFS
#define USE_FLASH_W25N01G
#define W25N01G_SPI_BUS BUS_SPI1
#define W25N01G_CS_PIN PA4
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#else
//Regular Kakute H7
#define USE_SDCARD
#define USE_SDCARD_SPI
#define SDCARD_SPI_BUS BUS_SPI1
@ -81,11 +96,23 @@
#define SPI4_MISO_PIN PE5
#define SPI4_MOSI_PIN PE6
//MPU6000
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW270_DEG
#define MPU6000_SPI_BUS BUS_SPI4
#define MPU6000_CS_PIN PE4
//BMI270
#define USE_IMU_BMI270
#define BMI270_SPI_BUS BUS_SPI4
#define BMI270_CS_PIN PE4
#ifdef KAKUTEH7MINI
#define IMU_BMI270_ALIGN CW270_DEG
#else
#define IMU_BMI270_ALIGN CW0_DEG
#endif
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
@ -170,6 +197,13 @@
#ifdef KAKUTEH7MINI
#define PINIO1_PIN PB11
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
#elif defined(KAKUTEH7V2)
#define PINIO1_PIN PE13
#define PINIO2_PIN PB11
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
#else
#define PINIO1_PIN PE13
#endif

View file

@ -31,15 +31,10 @@
#define BEEPER_INVERTED
// ******* GYRO and ACC ********
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN SPI1_NSS_PIN
#define ICM42605_EXTI_PIN GYRO_INT_EXTI
#define USE_I2C

View file

@ -67,7 +67,7 @@
// #define SPI4_MOSI_PIN PE14
#define USE_IMU_ICM42605
#define IMU_ICM42605_ALIGN CW90_DEG
#define IMU_ICM42605_ALIGN CW0_DEG
#define ICM42605_SPI_BUS BUS_SPI1
#define ICM42605_CS_PIN PA4

View file

@ -1,2 +1,3 @@
target_stm32f722xe(MATEKF722PX)
target_stm32f722xe(MATEKF722PX_PINIO)
target_stm32f722xe(MATEKF722WPX)

View file

@ -31,6 +31,10 @@ void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
#ifdef MATEKF722PX_PINIO
pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3;
pinioBoxConfigMutable()->permanentId[3] = BOX_PERMANENT_ID_USER4;
#endif
serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_FRSKY_OSD;
}

View file

@ -111,9 +111,11 @@
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#ifndef MATEKF722PX_PINIO
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#endif
#define USE_UART6
#define UART6_TX_PIN PC6
@ -150,6 +152,11 @@
#define PINIO1_PIN PA15 // Power switch
#define PINIO2_PIN PB3 // Camera switch
#ifdef MATEKF722PX_PINIO
#define PINIO3_PIN PC12 // UART 5 TX - USER 3 PINIO
#define PINIO4_PIN PD2 // UART 5 RX - USER 4 PINIO
#endif
// *************** LEDSTRIP ************************
#define USE_LED_STRIP
#define WS2811_PIN PA8

View file

@ -32,7 +32,7 @@
#include "drivers/pinio.h"
#include "drivers/sensor.h"
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, BMI270_EXTI_PIN, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
timerHardware_t timerHardware[] = {
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1

View file

@ -80,10 +80,6 @@
#define IMU_BMI270_ALIGN CW0_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN PA4
#define BMI270_EXTI_PIN PC4
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
// *************** I2C(Baro & I2C) **************************
#define USE_I2C

View file

@ -73,7 +73,6 @@
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW90_DEG
#define GYRO_INT_EXTI PE1
#define MPU6000_CS_PIN PE4
#define MPU6000_SPI_BUS BUS_SPI4
@ -82,7 +81,6 @@
#define IMU_BMI270_ALIGN CW90_DEG
#define BMI270_SPI_BUS BUS_SPI4
#define BMI270_CS_PIN PE4
#define BMI270_EXTI_PIN GYRO_INT_EXTI
// *************** I2C(Baro & I2C) **************************
#define USE_I2C

View file

@ -47,9 +47,6 @@
#define USE_SPI
#ifdef ZEEZF7V3
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define GYRO_INT_EXTI PC13
#define USE_SPI_DEVICE_3
@ -64,7 +61,6 @@
#define IMU_ICM42605_ALIGN CW0_DEG
#define ICM42605_SPI_BUS BUS_SPI3
#define ICM42605_CS_PIN SPI3_NSS_PIN
#define ICM42605_EXTI_PIN GYRO_INT_EXTI
// *************** SPI3 IMU0 BMI270 **************
#define USE_IMU_BMI270
@ -72,7 +68,6 @@
#define IMU_BMI270_ALIGN CW270_DEG
#define BMI270_SPI_BUS BUS_SPI3
#define BMI270_CS_PIN SPI3_NSS_PIN
#define BMI270_EXTI_PIN GYRO_INT_EXTI
#endif
#ifdef ZEEZF7V2
@ -84,11 +79,7 @@
#define MPU6000_CS_PIN SPI1_NSS_PIN
#define MPU6000_SPI_BUS BUS_SPI1
#define GYRO_INT_EXTI PC4
#define IMU_MPU6000_ALIGN CW0_DEG
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define USE_IMU_MPU6000
#endif
@ -102,12 +93,8 @@
#define MPU6000_CS_PIN SPI2_NSS_PIN
#define MPU6000_SPI_BUS BUS_SPI2
#define GYRO_INT_EXTI PC9
#define IMU_MPU6000_ALIGN CW0_DEG_FLIP
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define USE_IMU_MPU6000
#endif
@ -175,10 +162,6 @@
#define USE_SDCARD_SPI
#endif
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USE_FLASH_W25N01G
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#endif
#ifdef ZEEZF7
@ -194,11 +177,12 @@
#define W25N01G_SPI_BUS BUS_SPI1
#define W25N01G_CS_PIN SPI1_NSS_PIN
#endif
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USE_FLASH_W25N01G
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
#endif
// *************** OSD *****************************
#define USE_MAX7456

View file

@ -195,10 +195,10 @@ static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
if (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) {
throttleForRPM = 0;
}
serialize16(throttleForRPM);
} else {
serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));