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:
commit
15cee7c808
76 changed files with 1120 additions and 417 deletions
4
.github/workflows/ci.yml
vendored
4
.github/workflows/ci.yml
vendored
|
@ -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
|
||||
|
|
2
.github/workflows/docs.yml
vendored
2
.github/workflows/docs.yml
vendored
|
@ -11,7 +11,7 @@ on:
|
|||
|
||||
jobs:
|
||||
settings_md:
|
||||
runs-on: ubuntu-18.04
|
||||
runs-on: ubuntu-latest
|
||||
|
||||
steps:
|
||||
- uses: actions/checkout@v3
|
||||
|
|
|
@ -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).
|
||||
|
|
|
@ -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 |
|
||||
|
|
|
@ -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 |
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
|
|
16
docs/boards/Matek F722PX-WPX-HD.md
Normal file
16
docs/boards/Matek F722PX-WPX-HD.md
Normal 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.
|
|
@ -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
|
||||
|
||||
|
|
|
@ -71,5 +71,6 @@ typedef enum {
|
|||
DEBUG_AUTOTUNE,
|
||||
DEBUG_RATE_DYNAMICS,
|
||||
DEBUG_LANDING,
|
||||
DEBUG_POS_EST,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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),
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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, ¤tBatteryProfileMutable->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:
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -75,7 +75,8 @@ typedef enum {
|
|||
BOXPLANWPMISSION = 46,
|
||||
BOXSOARING = 47,
|
||||
BOXUSER3 = 48,
|
||||
BOXCHANGEMISSION = 49,
|
||||
BOXUSER4 = 49,
|
||||
BOXCHANGEMISSION = 50,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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 {
|
||||
|
|
1
src/main/target/AOCODARCF4V2/CMakeLists.txt
Normal file
1
src/main/target/AOCODARCF4V2/CMakeLists.txt
Normal file
|
@ -0,0 +1 @@
|
|||
target_stm32f405xg(AOCODARCF4V2)
|
46
src/main/target/AOCODARCF4V2/target.c
Normal file
46
src/main/target/AOCODARCF4V2/target.c
Normal 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]);
|
165
src/main/target/AOCODARCF4V2/target.h
Normal file
165
src/main/target/AOCODARCF4V2/target.h
Normal 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
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
2
src/main/target/FOXEERF722V4/CMakeLists.txt
Normal file
2
src/main/target/FOXEERF722V4/CMakeLists.txt
Normal file
|
@ -0,0 +1,2 @@
|
|||
target_stm32f722xe(FOXEERF722V4)
|
||||
target_stm32f722xe(FOXEERF722V4_X8)
|
44
src/main/target/FOXEERF722V4/target.c
Normal file
44
src/main/target/FOXEERF722V4/target.c
Normal 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]);
|
160
src/main/target/FOXEERF722V4/target.h
Normal file
160
src/main/target/FOXEERF722V4/target.h
Normal 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))
|
||||
|
|
@ -1 +1,2 @@
|
|||
target_stm32f745xg(FOXEERF745AIO)
|
||||
target_stm32f745xg(FOXEERF745AIO_V3)
|
|
@ -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
|
||||
|
|
|
@ -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[] = {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -1,2 +1,3 @@
|
|||
target_stm32h743xi(KAKUTEH7)
|
||||
target_stm32h743xi(KAKUTEH7V2)
|
||||
target_stm32h743xi(KAKUTEH7MINI)
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -1,2 +1,3 @@
|
|||
target_stm32f722xe(MATEKF722PX)
|
||||
target_stm32f722xe(MATEKF722PX_PINIO)
|
||||
target_stm32f722xe(MATEKF722WPX)
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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));
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue