mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Merge remote-tracking branch 'origin/master' into sh_mixer_profile
This commit is contained in:
shota
commit
e51fd7862c
85 changed files with 2802 additions and 372 deletions
61
docs/Cli.md
61
docs/Cli.md
|
|
@ -114,6 +114,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
|
|||
| `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. |
|
||||
| `timer_output_mode` | Override automatic timer / pwm function allocation. [Additional Information](#timer_outout_mode)|
|
||||
| `version` | Show version |
|
||||
| `wp` | List or configure waypoints. See the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). |
|
||||
|
||||
|
|
@ -170,6 +171,66 @@ serial 0 -4
|
|||
|
||||
`serial` can also be used without any argument to print the current configuration of all the serial ports.
|
||||
|
||||
### `timer_output_mode`
|
||||
|
||||
Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_SERVOS6`).
|
||||
|
||||
#### Syntax
|
||||
|
||||
```
|
||||
timer_output_mode [timer [function]]
|
||||
```
|
||||
where:
|
||||
* Without parameters, lists the current timers and modes
|
||||
* With just a `timer` lists the mode for that timer
|
||||
* With both `timer` and `function`, sets the function for that timers
|
||||
|
||||
Note:
|
||||
|
||||
* `timer` identifies the timer **index** (from 0); thus is one less than the corresponding `TIMn` definition in a target's `target.c`.
|
||||
* The function is one of `AUTO` (the default), `MOTORS` or `SERVOS`.
|
||||
|
||||
Motors are allocated first, hence having a servo before a motor may require use of `timer_output_mode`.
|
||||
|
||||
#### Example
|
||||
|
||||
The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_SERVOS6` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1.
|
||||
|
||||
#### Solution
|
||||
|
||||
There is now a single `MATEKF405` target. The `target.c` sets the relevant outputs as:
|
||||
|
||||
```
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1)
|
||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED, 0, 0), // S5 UP(1,7)
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5)
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6)
|
||||
```
|
||||
|
||||
Using the "motors first" allocation, the servo would end up on S6, which in the legacy "tricopter servo on S1" case is not desired.
|
||||
|
||||
Forcing the S1 output (`TIM3`) to servo is achieved by:
|
||||
|
||||
```
|
||||
timer_output_mode 2 SERVOS
|
||||
```
|
||||
|
||||
with resulting `resource` output:
|
||||
|
||||
```
|
||||
C06: SERVO4 OUT
|
||||
C07: MOTOR1 OUT
|
||||
C08: MOTOR2 OUT
|
||||
C09: MOTOR3 OUT
|
||||
```
|
||||
|
||||
Note that the `timer` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer` of 2 for `TIM3`.
|
||||
|
||||
Note that the usual caveat that one should not share a timer with both a motor and a servo still apply.
|
||||
|
||||
## Flash chip management
|
||||
|
||||
For targets that have a flash data chip, typically used for blackbox logs, the following additional comamnds are provided.
|
||||
|
|
|
|||
|
|
@ -28,8 +28,23 @@ While motors are usually ordered sequentially, here is no standard output layout
|
|||
|
||||
## Modifying output mapping
|
||||
|
||||
INAV 5 allows the limited output type mapping by allowing to change the function of *ALL* outputs at the same time. It can be done with the `output_mode` CLI setting. Allowed values:
|
||||
### Modifying all outputs at the same time
|
||||
|
||||
Since INAV 5, it has been possible to force *ALL* outputs to be `MOTORS` or `SERVOS`.
|
||||
|
||||
Traditional ESCs usually can be controlled via a servo output, but would require calibration.
|
||||
|
||||
This can be done with the `output_mode` CLI setting. Allowed values:
|
||||
|
||||
* `AUTO` assigns outputs according to the default mapping
|
||||
* `SERVOS` assigns all outputs to servos
|
||||
* `MOTORS` assigns all outputs to motors
|
||||
|
||||
### Modifying only some outputs
|
||||
|
||||
INAV 7 introduced extra functionality that let you force only some outputs to be either *MOTORS* or *SERVOS*, with some restrictions dictated by the hardware.
|
||||
|
||||
The mains restrictions is that outputs need to be associated with timers, which are usually shared between multiple outputs. Two outputs on the same timer need to have the same function.
|
||||
|
||||
The easiest way to modify outputs, is to use the Mixer tab in the Configurator, as it will clearly show you which timer is used by all outputs, but you can also use `timer_output_mode` on the cli.
|
||||
This can be used in conjunction to the previous method, in that cass all outputs will follow `output_mode` and `timer_output_mode` overrides are applied after that.
|
||||
|
|
|
|||
|
|
@ -46,16 +46,16 @@ IPF can be edited using INAV Configurator user interface, or via CLI
|
|||
| 10 | NAND | `false` if `Operand A` and `Operand B` are both `true`|
|
||||
| 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` |
|
||||
| 12 | NOT | The boolean opposite to `Operand A` |
|
||||
| 13 | STICKY | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
|
||||
| 14 | ADD | Add `Operand A` to `Operand B` and returns the result |
|
||||
| 15 | SUB | Substract `Operand B` from `Operand A` and returns the result |
|
||||
| 16 | MUL | Multiply `Operand A` by `Operand B` and returns the result |
|
||||
| 17 | DIV | Divide `Operand A` by `Operand B` and returns the result |
|
||||
| 18 | GVAR SET | Store value from `Operand B` into the Global Variable addressed by
|
||||
| 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|
|
||||
| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result |
|
||||
| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result |
|
||||
| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result |
|
||||
| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result |
|
||||
| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by
|
||||
`Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
|
||||
| 19 | GVAR INC | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
|
||||
| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
|
||||
| 21 | IO PORT SET | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
|
||||
| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
|
||||
| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` |
|
||||
| 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
|
||||
| 22 | OVERRIDE_ARMING_SAFETY | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
|
||||
| 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
|
||||
| 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
|
||||
|
|
@ -67,18 +67,18 @@ IPF can be edited using INAV Configurator user interface, or via CLI
|
|||
| 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` |
|
||||
| 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` |
|
||||
| 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` |
|
||||
| 33 | SIN | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 34 | COS | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 35 | TAN | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
|
||||
| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
|
||||
| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
|
||||
| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
|
||||
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
|
||||
| 40 | MOD | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
|
||||
| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
|
||||
| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
|
||||
| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
|
||||
| 43 | MIN | Finds the lowest value of `Operand A` and `Operand B` |
|
||||
| 44 | MAX | Finds the highest value of `Operand A` and `Operand B` |
|
||||
| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` |
|
||||
| 44 | Use Highest Value | 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 | Momentarily true when triggered by `Operand A`. `Operand A` is the 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. |
|
||||
|
|
|
|||
|
|
@ -2702,6 +2702,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
|
|||
|
||||
---
|
||||
|
||||
### nav_cruise_yaw_rate
|
||||
|
||||
Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 20 | 0 | 120 |
|
||||
|
||||
---
|
||||
|
||||
### nav_disarm_on_landing
|
||||
|
||||
If set to ON, INAV disarms the FC after landing
|
||||
|
|
@ -2792,16 +2802,6 @@ Cruise throttle in GPS assisted modes, this includes RTH. Should be set high eno
|
|||
|
||||
---
|
||||
|
||||
### nav_fw_cruise_yaw_rate
|
||||
|
||||
Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 20 | 0 | 60 |
|
||||
|
||||
---
|
||||
|
||||
### nav_fw_dive_angle
|
||||
|
||||
Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ Install Git, Make, gcc and Ruby
|
|||
- `sudo apt-get install git make cmake ruby`
|
||||
|
||||
Install python and python-yaml to allow updates to settings.md
|
||||
- `sudo apt-get install python3 python-yaml`
|
||||
- `sudo apt-get install python3`
|
||||
|
||||
### CMAKE and Ubuntu 18_04
|
||||
|
||||
|
|
|
|||
|
|
@ -76,9 +76,11 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
|
|||
|
||||
A set of macros `LOG_ERROR()` (log error) through `LOG_DEBUG()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
|
||||
|
||||
Note that the `topic` is specified without the `LOG_TOPIC_` prefix:
|
||||
|
||||
```
|
||||
// LOG_DEBUG(topic, fmt, ...)
|
||||
LOG_DEBUG(LOG_TOPIC_SYSTEM, "This is %s topic debug message, value %d", "system", 42);
|
||||
LOG_DEBUG(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
|
||||
```
|
||||
|
||||
It is also possible to dump a hex representation of arbitrary data, using functions named variously `LOG_BUFFER_` (`ERROR`) and `LOG_BUF_` (anything else, alas) e.g.:
|
||||
|
|
@ -89,7 +91,7 @@ It is also possible to dump a hex representation of arbitrary data, using funct
|
|||
|
||||
struct {...} tstruct;
|
||||
...
|
||||
LOG_BUF_DEBUG(LOG_TOPIC_TEMPERATURE, &tstruct, sizeof(tstruct));
|
||||
LOG_BUF_DEBUG(TEMPERATURE, &tstruct, sizeof(tstruct));
|
||||
```
|
||||
|
||||
## Output Support
|
||||
|
|
@ -104,7 +106,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
|
|||
For example, with the final lines of `src/main/fc/fc_init.c` set to:
|
||||
|
||||
```
|
||||
LOG_ERROR(LOG_TOPIC_SYSTEM, "Init is complete");
|
||||
LOG_ERROR(SYSTEM, "Init is complete");
|
||||
systemState |= SYSTEM_STATE_READY;
|
||||
```
|
||||
|
||||
|
|
|
|||
4
src/main/CMakeLists.txt
Normal file → Executable file
4
src/main/CMakeLists.txt
Normal file → Executable file
|
|
@ -208,6 +208,8 @@ main_sources(COMMON_SRC
|
|||
drivers/pitotmeter/pitotmeter_adc.h
|
||||
drivers/pitotmeter/pitotmeter_ms4525.c
|
||||
drivers/pitotmeter/pitotmeter_ms4525.h
|
||||
drivers/pitotmeter/pitotmeter_dlvr_l10d.c
|
||||
drivers/pitotmeter/pitotmeter_dlvr_l10d.h
|
||||
drivers/pitotmeter/pitotmeter_msp.c
|
||||
drivers/pitotmeter/pitotmeter_msp.h
|
||||
drivers/pitotmeter/pitotmeter_virtual.c
|
||||
|
|
@ -285,6 +287,8 @@ main_sources(COMMON_SRC
|
|||
fc/firmware_update.h
|
||||
fc/firmware_update_common.c
|
||||
fc/firmware_update_common.h
|
||||
fc/multifunction.c
|
||||
fc/multifunction.h
|
||||
fc/rc_smoothing.c
|
||||
fc/rc_smoothing.h
|
||||
fc/rc_adjustments.c
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include "drivers/time.h"
|
||||
#include "common/calibration.h"
|
||||
|
||||
|
||||
void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
|
||||
{
|
||||
// Reset parameters and state
|
||||
|
|
@ -75,9 +76,11 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
|
|||
// Check if calibration is complete
|
||||
if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
|
||||
const float stddev = devStandardDeviation(&s->val.stdDev);
|
||||
|
||||
if (stddev > s->params.stdDevThreshold) {
|
||||
if (!s->params.allowFailure) {
|
||||
// If deviation is too big - restart calibration
|
||||
// If deviation is too big && no failure allowed - restart calibration
|
||||
// TODO :: some safeguard should exist which will not allow to keep on calibrating for ever
|
||||
s->params.startTimeMs = millis();
|
||||
s->params.sampleCount = 0;
|
||||
s->val.accumulatedValue = 0;
|
||||
|
|
|
|||
|
|
@ -119,7 +119,8 @@
|
|||
#define PG_UNUSED_1 1029
|
||||
#define PG_POWER_LIMITS_CONFIG 1030
|
||||
#define PG_OSD_COMMON_CONFIG 1031
|
||||
#define PG_INAV_END 1031
|
||||
#define PG_TIMER_OVERRIDE_CONFIG 1032
|
||||
#define PG_INAV_END 1032
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
//#define PG_OSD_FONT_CONFIG 2047
|
||||
|
|
|
|||
|
|
@ -138,6 +138,7 @@ typedef enum {
|
|||
|
||||
/* Other hardware */
|
||||
DEVHW_MS4525, // Pitot meter
|
||||
DEVHW_DLVR, // Pitot meter
|
||||
DEVHW_M25P16, // SPI NOR flash
|
||||
DEVHW_W25N01G, // SPI 128MB flash
|
||||
DEVHW_UG2864, // I2C OLED display
|
||||
|
|
|
|||
|
|
@ -174,6 +174,7 @@
|
|||
#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_ALERT 0xDD // 221 General alert symbol
|
||||
|
||||
#define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust)
|
||||
#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure
|
|||
typedef struct pitotDev_s {
|
||||
busDevice_t * busDev;
|
||||
uint16_t delay;
|
||||
float calibThreshold;
|
||||
pitotOpFuncPtr start;
|
||||
pitotOpFuncPtr get;
|
||||
pitotCalculateFuncPtr calculate;
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera
|
|||
bool adcPitotDetect(pitotDev_t *pitot)
|
||||
{
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00001f; // TODO :: should be tested !!!
|
||||
pitot->start = adcPitotStart;
|
||||
pitot->get = adcPitotRead;
|
||||
pitot->calculate = adcPitotCalculate;
|
||||
|
|
|
|||
165
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
Executable file
165
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
Executable file
|
|
@ -0,0 +1,165 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
#include <build/debug.h>
|
||||
|
||||
#include "common/log.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/maths.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/pitotmeter/pitotmeter.h"
|
||||
|
||||
#define DLVR_L10D_ADDR 0x28 // this var is not used !!!
|
||||
|
||||
// //---------------------------------------------------
|
||||
// // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
|
||||
// #define ISA_GAS_CONSTANT 287.26f
|
||||
// #define ISA_LAPSE_RATE 0.0065f
|
||||
|
||||
// // Standard Sea Level values
|
||||
// // Ref: https://en.wikipedia.org/wiki/Standard_sea_level
|
||||
// #define SSL_AIR_DENSITY 1.225f // kg/m^3
|
||||
// #define SSL_AIR_PRESSURE 101325.01576f // Pascal
|
||||
// #define SSL_AIR_TEMPERATURE 288.15f // K
|
||||
// //---------------------------------------------------
|
||||
|
||||
#define INCH_OF_H2O_TO_PASCAL 248.84f
|
||||
|
||||
#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
|
||||
|
||||
#define RANGE_INCH_H2O 10
|
||||
#define DLVR_OFFSET 8192.0f
|
||||
#define DLVR_SCALE 16384.0f
|
||||
// NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration
|
||||
#define DLVR_OFFSET_CORR 0.0f //-9.0f
|
||||
|
||||
|
||||
typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
|
||||
bool dataValid;
|
||||
uint32_t dlvr_ut;
|
||||
uint32_t dlvr_up;
|
||||
} dlvrCtx_t;
|
||||
|
||||
STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
|
||||
|
||||
static bool dlvr_start(pitotDev_t * pitot)
|
||||
{
|
||||
UNUSED(pitot);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool dlvr_read(pitotDev_t * pitot)
|
||||
{
|
||||
uint8_t rxbuf1[4];
|
||||
|
||||
dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
|
||||
ctx->dataValid = false;
|
||||
|
||||
if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// status = 00 -> ok, new data
|
||||
// status = 01 -> reserved
|
||||
// status = 10 -> ok, data stale
|
||||
// status = 11 -> error
|
||||
const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
|
||||
|
||||
if (status) {
|
||||
// anything other then 00 in the status bits is an error
|
||||
LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) );
|
||||
return false;
|
||||
}
|
||||
|
||||
int16_t dP_raw1, dT_raw1;
|
||||
|
||||
dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
|
||||
dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
|
||||
|
||||
// Data valid, update ut/up values
|
||||
ctx->dataValid = true;
|
||||
ctx->dlvr_up = dP_raw1;
|
||||
ctx->dlvr_ut = dT_raw1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
|
||||
{
|
||||
dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
|
||||
|
||||
|
||||
// pressure in inchH2O
|
||||
float dP_inchH2O = 1.25f * 2.0f * RANGE_INCH_H2O * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE);
|
||||
|
||||
// temperature in deg C
|
||||
float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;
|
||||
|
||||
// result must fit inside the max pressure range
|
||||
if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
|
||||
LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) );
|
||||
return;
|
||||
}
|
||||
|
||||
if (pressure) {
|
||||
*pressure = INCH_H2O_TO_PASCAL( dP_inchH2O); // Pa
|
||||
}
|
||||
|
||||
if (temperature) {
|
||||
*temperature = C_TO_KELVIN( T_C); // K
|
||||
}
|
||||
}
|
||||
|
||||
bool dlvrDetect(pitotDev_t * pitot)
|
||||
{
|
||||
uint8_t rxbuf[4];
|
||||
bool ack = false;
|
||||
|
||||
pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED);
|
||||
if (pitot->busDev == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Read twice to fix:
|
||||
// Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
|
||||
// communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
|
||||
// An additional start condition must be sent, which results in restoration of proper communication.
|
||||
ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
|
||||
ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Initialize busDev data
|
||||
dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
|
||||
ctx->dataValid = false;
|
||||
ctx->dlvr_ut = 0;
|
||||
ctx->dlvr_up = 0;
|
||||
|
||||
// Initialize pitotDev object
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00001f; // low noise sensor
|
||||
pitot->start = dlvr_start;
|
||||
pitot->get = dlvr_read;
|
||||
pitot->calculate = dlvr_calculate;
|
||||
return true;
|
||||
}
|
||||
20
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h
Executable file
20
src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h
Executable file
|
|
@ -0,0 +1,20 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
bool dlvrDetect(pitotDev_t *pitot);
|
||||
|
|
@ -75,6 +75,7 @@ bool fakePitotDetect(pitotDev_t *pitot)
|
|||
fakeTemperature = 273; // 0C
|
||||
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00001f;
|
||||
pitot->start = fakePitotStart;
|
||||
pitot->get = fakePitotRead;
|
||||
pitot->calculate = fakePitotCalculate;
|
||||
|
|
|
|||
|
|
@ -74,6 +74,13 @@ static bool ms4525_read(pitotDev_t * pitot)
|
|||
dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
|
||||
dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
|
||||
|
||||
// reject any values that are the absolute minimum or maximums these
|
||||
// can happen due to gnd lifts or communication errors on the bus
|
||||
if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 ||
|
||||
dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// reject any double reads where the value has shifted in the upper more than 0xFF
|
||||
if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
|
||||
return false;
|
||||
|
|
@ -83,6 +90,7 @@ static bool ms4525_read(pitotDev_t * pitot)
|
|||
ctx->dataValid = true;
|
||||
ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
|
||||
ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -136,6 +144,7 @@ bool ms4525Detect(pitotDev_t * pitot)
|
|||
|
||||
// Initialize pitotDev object
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00005f; // noisy sensor
|
||||
pitot->start = ms4525_start;
|
||||
pitot->get = ms4525_read;
|
||||
pitot->calculate = ms4525_calculate;
|
||||
|
|
|
|||
|
|
@ -87,6 +87,7 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
|
|||
mspPitotTemperature = 27315; // 0 deg/c
|
||||
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00001f;
|
||||
pitot->start = mspPitotStart;
|
||||
pitot->get = mspPitotRead;
|
||||
pitot->calculate = mspPitotCalculate;
|
||||
|
|
@ -95,3 +96,4 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
|
|||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -88,6 +88,7 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem
|
|||
bool virtualPitotDetect(pitotDev_t *pitot)
|
||||
{
|
||||
pitot->delay = 10000;
|
||||
pitot->calibThreshold = 0.00001f;
|
||||
pitot->start = virtualPitotStart;
|
||||
pitot->get = virtualPitotRead;
|
||||
pitot->calculate = virtualPitotCalculate;
|
||||
|
|
|
|||
|
|
@ -214,25 +214,129 @@ static void timerHardwareOverride(timerHardware_t * timer) {
|
|||
if (currentMixerConfig.outputMode == OUTPUT_MODE_SERVOS) {
|
||||
|
||||
//Motors are rewritten as servos
|
||||
if (timer->usageFlags & TIM_USE_MC_MOTOR) {
|
||||
timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_MOTOR;
|
||||
timer->usageFlags = timer->usageFlags | TIM_USE_MC_SERVO;
|
||||
}
|
||||
if (timer->usageFlags & TIM_USE_FW_MOTOR) {
|
||||
timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_MOTOR;
|
||||
timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO;
|
||||
if (timer->usageFlags & (TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR)) {
|
||||
timer->usageFlags &= ~(TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR);
|
||||
timer->usageFlags |= TIM_USE_MC_SERVO | TIM_USE_FW_SERVO;
|
||||
}
|
||||
|
||||
} else if (currentMixerConfig.outputMode == OUTPUT_MODE_MOTORS) {
|
||||
|
||||
// Servos are rewritten as motors
|
||||
if (timer->usageFlags & TIM_USE_MC_SERVO) {
|
||||
timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_SERVO;
|
||||
timer->usageFlags = timer->usageFlags | TIM_USE_MC_MOTOR;
|
||||
if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) {
|
||||
timer->usageFlags &= ~(TIM_USE_MC_SERVO | TIM_USE_FW_SERVO);
|
||||
timer->usageFlags |= TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR;
|
||||
}
|
||||
}
|
||||
|
||||
switch (timerOverrides(timer2id(timer->tim))->outputMode) {
|
||||
case OUTPUT_MODE_MOTORS:
|
||||
if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) {
|
||||
timer->usageFlags &= ~(TIM_USE_MC_SERVO | TIM_USE_FW_SERVO);
|
||||
timer->usageFlags |= TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR;
|
||||
}
|
||||
break;
|
||||
case OUTPUT_MODE_SERVOS:
|
||||
if (timer->usageFlags & (TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR)) {
|
||||
timer->usageFlags &= ~(TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR);
|
||||
timer->usageFlags |= TIM_USE_MC_SERVO | TIM_USE_FW_SERVO;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
|
||||
{
|
||||
for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) {
|
||||
if (timOutputs->timMotors[i]->tim == tim) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
|
||||
{
|
||||
for (int i = 0; i < timOutputs->maxTimServoCount; ++i) {
|
||||
if (timOutputs->timServos[i]->tim == tim) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) {
|
||||
uint8_t changed = 0;
|
||||
for (int idx = 0; idx < timerHardwareCount; idx++) {
|
||||
timerHardware_t *timHw = &timerHardware[idx];
|
||||
if (timHw->tim == tim && timHw->usageFlags != usageFlags) {
|
||||
timHw->usageFlags = usageFlags;
|
||||
changed++;
|
||||
}
|
||||
}
|
||||
|
||||
return changed;
|
||||
}
|
||||
|
||||
void pwmEnsureEnoughtMotors(uint8_t motorCount)
|
||||
{
|
||||
uint8_t motorOnlyOutputs = 0;
|
||||
uint8_t mcMotorOnlyOutputs = 0;
|
||||
|
||||
for (int idx = 0; idx < timerHardwareCount; idx++) {
|
||||
timerHardware_t *timHw = &timerHardware[idx];
|
||||
|
||||
timerHardwareOverride(timHw);
|
||||
|
||||
if (checkPwmTimerConflicts(timHw)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (timHw->usageFlags & (TIM_USE_MC_MOTOR) && !(timHw->usageFlags & (TIM_USE_MC_SERVO))) {
|
||||
mcMotorOnlyOutputs++;
|
||||
mcMotorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
}
|
||||
if (timHw->usageFlags & (TIM_USE_FW_MOTOR) && !(timHw->usageFlags & (TIM_USE_FW_SERVO))) {
|
||||
motorOnlyOutputs++;
|
||||
motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
}
|
||||
}
|
||||
|
||||
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR ||
|
||||
mixerConfig()->platformType == PLATFORM_TRICOPTER) {
|
||||
|
||||
for (int idx = 0; mcMotorOnlyOutputs < motorCount && idx < timerHardwareCount; idx++) {
|
||||
timerHardware_t *timHw = &timerHardware[idx];
|
||||
|
||||
if (checkPwmTimerConflicts(timHw)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint32_t mcFlags = timHw->usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO);
|
||||
|
||||
if (mcFlags && mcFlags != TIM_USE_MC_MOTOR) {
|
||||
timHw->usageFlags &= ~TIM_USE_MC_SERVO;
|
||||
timHw->usageFlags |= TIM_USE_MC_MOTOR;
|
||||
mcMotorOnlyOutputs++;
|
||||
mcMotorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int idx = 0; motorOnlyOutputs < motorCount && idx < timerHardwareCount; idx++) {
|
||||
timerHardware_t *timHw = &timerHardware[idx];
|
||||
|
||||
if (checkPwmTimerConflicts(timHw)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint32_t fwFlags = timHw->usageFlags & (TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO);
|
||||
if (fwFlags && fwFlags != TIM_USE_FW_MOTOR) {
|
||||
timHw->usageFlags &= ~TIM_USE_FW_SERVO;
|
||||
timHw->usageFlags |= TIM_USE_FW_MOTOR;
|
||||
motorOnlyOutputs++;
|
||||
motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
}
|
||||
if (timer->usageFlags & TIM_USE_FW_SERVO) {
|
||||
timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_SERVO;
|
||||
timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -245,8 +349,9 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
|
|||
uint8_t motorCount = getMotorCount();
|
||||
uint8_t motorIdx = 0;
|
||||
|
||||
for (int idx = 0; idx < timerHardwareCount; idx++) {
|
||||
pwmEnsureEnoughtMotors(motorCount);
|
||||
|
||||
for (int idx = 0; idx < timerHardwareCount; idx++) {
|
||||
timerHardware_t *timHw = &timerHardware[idx];
|
||||
|
||||
timerHardwareOverride(timHw);
|
||||
|
|
@ -266,32 +371,42 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
|
|||
// Make sure first motorCount outputs get assigned to motor
|
||||
if ((timHw->usageFlags & TIM_USE_MC_MOTOR) && (motorIdx < motorCount)) {
|
||||
timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO;
|
||||
pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
motorIdx += 1;
|
||||
}
|
||||
|
||||
// We enable mapping to servos if mixer is actually using them
|
||||
if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO) {
|
||||
// We enable mapping to servos if mixer is actually using them and it does not conflict with used motors
|
||||
if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
|
||||
type = MAP_TO_SERVO_OUTPUT;
|
||||
}
|
||||
else if (timHw->usageFlags & TIM_USE_MC_MOTOR) {
|
||||
} else if (timHw->usageFlags & TIM_USE_MC_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
|
||||
type = MAP_TO_MOTOR_OUTPUT;
|
||||
}
|
||||
} else {
|
||||
// Fixed wing or HELI (one/two motors and a lot of servos
|
||||
if (timHw->usageFlags & TIM_USE_FW_SERVO) {
|
||||
type = MAP_TO_SERVO_OUTPUT;
|
||||
// Make sure first motorCount motor outputs get assigned to motor
|
||||
if ((timHw->usageFlags & TIM_USE_FW_MOTOR) && (motorIdx < motorCount)) {
|
||||
timHw->usageFlags = timHw->usageFlags & ~TIM_USE_FW_SERVO;
|
||||
pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
motorIdx += 1;
|
||||
}
|
||||
else if (timHw->usageFlags & TIM_USE_FW_MOTOR) {
|
||||
|
||||
// Fixed wing or HELI (one/two motors and a lot of servos
|
||||
if (timHw->usageFlags & TIM_USE_FW_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
|
||||
type = MAP_TO_SERVO_OUTPUT;
|
||||
} else if (timHw->usageFlags & TIM_USE_FW_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
|
||||
type = MAP_TO_MOTOR_OUTPUT;
|
||||
}
|
||||
}
|
||||
|
||||
switch(type) {
|
||||
case MAP_TO_MOTOR_OUTPUT:
|
||||
timHw->usageFlags &= (TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR);
|
||||
timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
|
||||
pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
break;
|
||||
case MAP_TO_SERVO_OUTPUT:
|
||||
timHw->usageFlags &= (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO);
|
||||
timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
|
||||
pwmClaimTimer(timHw->tim, timHw->usageFlags);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "drivers/io_types.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/mixer_profile.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -42,6 +42,9 @@
|
|||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "drivers/timer_impl.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_HZ * 5 / 1000000.0f)
|
||||
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f)
|
||||
|
||||
|
|
@ -56,7 +59,9 @@
|
|||
#define DSHOT_MOTOR_BITLENGTH 20
|
||||
|
||||
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
|
||||
#define MAX_DMA_TIMERS 8
|
||||
|
||||
#define DSHOT_COMMAND_DELAY_US 1000
|
||||
#define DSHOT_COMMAND_INTERVAL_US 10000
|
||||
#define DSHOT_COMMAND_QUEUE_LENGTH 8
|
||||
#define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
|
||||
|
|
@ -64,6 +69,10 @@
|
|||
|
||||
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
|
||||
#endif
|
||||
|
||||
typedef struct {
|
||||
TCH_t * tch;
|
||||
bool configured;
|
||||
|
|
@ -77,6 +86,9 @@ typedef struct {
|
|||
#ifdef USE_DSHOT
|
||||
// DSHOT parameters
|
||||
timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
timerDMASafeType_t *dmaBurstBuffer;
|
||||
#endif
|
||||
#endif
|
||||
} pwmOutputPort_t;
|
||||
|
||||
|
|
@ -249,6 +261,22 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
|
||||
uint8_t burstDmaTimersCount = 0;
|
||||
|
||||
static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
|
||||
{
|
||||
for (int i = 0; i < burstDmaTimersCount; i++) {
|
||||
if (burstDmaTimers[i].timer == timer) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
burstDmaTimers[burstDmaTimersCount++].timer = timer;
|
||||
return burstDmaTimersCount - 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
|
||||
{
|
||||
// Try allocating new port
|
||||
|
|
@ -259,15 +287,43 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
|
|||
}
|
||||
|
||||
// Configure timer DMA
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
//uint8_t timerIndex = lookupTimerIndex(timerHardware->tim);
|
||||
uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim);
|
||||
if (burstDmaTimerIndex >= MAX_DMA_TIMERS) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0];
|
||||
burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
|
||||
burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer;
|
||||
|
||||
if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
|
||||
port->configured = true;
|
||||
}
|
||||
#else
|
||||
if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
|
||||
// Only mark as DSHOT channel if DMA was set successfully
|
||||
ZERO_FARRAY(port->dmaBuffer);
|
||||
port->configured = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
return port;
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 16; i++) {
|
||||
dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first
|
||||
packet <<= 1;
|
||||
}
|
||||
dmaBuffer[i++ * stride] = 0;
|
||||
dmaBuffer[i++ * stride] = 0;
|
||||
}
|
||||
#else
|
||||
static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
|
||||
{
|
||||
for (int i = 0; i < 16; i++) {
|
||||
|
|
@ -275,6 +331,7 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
|
|||
packet <<= 1;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
|
||||
{
|
||||
|
|
@ -378,6 +435,7 @@ static void executeDShotCommands(void){
|
|||
return;
|
||||
}
|
||||
}
|
||||
delayMicroseconds(DSHOT_COMMAND_DELAY_US);
|
||||
for (uint8_t i = 0; i < getMotorCount(); i++) {
|
||||
motors[i].requestTelemetry = true;
|
||||
motors[i].value = currentExecutingCommand.cmd;
|
||||
|
|
@ -408,6 +466,20 @@ void pwmCompleteMotorUpdate(void) {
|
|||
|
||||
executeDShotCommands();
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
for (int index = 0; index < motorCount; index++) {
|
||||
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
|
||||
uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry);
|
||||
loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet);
|
||||
motors[index].requestTelemetry = false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) {
|
||||
burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
|
||||
pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4);
|
||||
}
|
||||
#else
|
||||
// Generate DMA buffers
|
||||
for (int index = 0; index < motorCount; index++) {
|
||||
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
|
||||
|
|
@ -424,6 +496,7 @@ void pwmCompleteMotorUpdate(void) {
|
|||
timerPWMStartDMA(motors[index].pwmPort->tch);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,8 +38,18 @@
|
|||
|
||||
timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT];
|
||||
|
||||
|
||||
uint8_t timer2id(const HAL_Timer_t *tim)
|
||||
{
|
||||
for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
|
||||
if (timerDefinitions[i].tim == tim) return i;
|
||||
}
|
||||
|
||||
return (uint8_t)-1;
|
||||
}
|
||||
|
||||
#if defined(AT32F43x)
|
||||
uint8_t lookupTimerIndex(const tmr_type *tim)
|
||||
uint8_t lookupTimerIndex(const HAL_Timer_t *tim)
|
||||
{
|
||||
int i;
|
||||
|
||||
|
|
@ -54,7 +64,7 @@ uint8_t lookupTimerIndex(const tmr_type *tim)
|
|||
}
|
||||
#else
|
||||
// return index of timer in timer table. Lowest timer has index 0
|
||||
uint8_t lookupTimerIndex(const TIM_TypeDef *tim)
|
||||
uint8_t lookupTimerIndex(const HAL_Timer_t *tim)
|
||||
{
|
||||
int i;
|
||||
|
||||
|
|
@ -287,3 +297,15 @@ bool timerPWMDMAInProgress(TCH_t * tch)
|
|||
{
|
||||
return tch->dmaState != TCH_DMA_IDLE;
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
|
||||
{
|
||||
return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount);
|
||||
}
|
||||
|
||||
void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
|
||||
{
|
||||
impl_pwmBurstDMAStart(burstDmaTimer, BurstLength);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
#include "drivers/rcc_types.h"
|
||||
#include "drivers/timer_def.h"
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4
|
||||
|
||||
typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t)
|
||||
|
|
@ -63,8 +65,9 @@ typedef uint32_t timCNT_t;
|
|||
#endif
|
||||
// tmr_type instead in AT32
|
||||
#if defined(AT32F43x)
|
||||
typedef tmr_type HAL_Timer_t;
|
||||
typedef struct timerDef_s {
|
||||
tmr_type * tim;
|
||||
HAL_Timer_t * tim;
|
||||
rccPeriphTag_t rcc;
|
||||
uint8_t irq;
|
||||
uint8_t secondIrq;
|
||||
|
|
@ -82,8 +85,9 @@ typedef struct timerHardware_s {
|
|||
uint32_t dmaMuxid; //DMAMUX ID
|
||||
} timerHardware_t;
|
||||
#else
|
||||
typedef TIM_TypeDef HAL_Timer_t;
|
||||
typedef struct timerDef_s {
|
||||
TIM_TypeDef * tim;
|
||||
HAL_Timer_t * tim;
|
||||
rccPeriphTag_t rcc;
|
||||
uint8_t irq;
|
||||
uint8_t secondIrq;
|
||||
|
|
@ -117,6 +121,7 @@ typedef enum {
|
|||
TIM_USE_BEEPER = (1 << 25),
|
||||
} timerUsageFlag_e;
|
||||
|
||||
#define TIM_USE_OUTPUT_AUTO (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO)
|
||||
|
||||
enum {
|
||||
TIMER_OUTPUT_NONE = 0x00,
|
||||
|
|
@ -160,6 +165,10 @@ typedef struct timHardwareContext_s {
|
|||
TIM_HandleTypeDef * timHandle;
|
||||
#endif
|
||||
TCH_t ch[CC_CHANNELS_PER_TIMER];
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
DMA_t dmaBurstRef;
|
||||
uint16_t DMASource;
|
||||
#endif
|
||||
} timHardwareContext_t;
|
||||
|
||||
// Per MCU timer definitions
|
||||
|
|
@ -170,6 +179,20 @@ extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT];
|
|||
extern timerHardware_t timerHardware[];
|
||||
extern const int timerHardwareCount;
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
typedef struct {
|
||||
TIM_TypeDef *timer;
|
||||
#ifdef USE_HAL_DRIVER
|
||||
DMA_TypeDef *dma;
|
||||
uint32_t streamLL;
|
||||
#else
|
||||
DMA_Stream_TypeDef *dmaBurstStream;
|
||||
#endif
|
||||
timerDMASafeType_t *dmaBurstBuffer;
|
||||
uint16_t burstRequestSource;
|
||||
} burstDmaTimer_t;
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
TYPE_FREE,
|
||||
TYPE_PWMINPUT,
|
||||
|
|
@ -231,3 +254,10 @@ void timerPWMStopDMA(TCH_t * tch);
|
|||
bool timerPWMDMAInProgress(TCH_t * tch);
|
||||
|
||||
volatile timCCR_t *timerCCR(TCH_t * tch);
|
||||
|
||||
uint8_t timer2id(const HAL_Timer_t *tim);
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
|
||||
void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -84,3 +84,8 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
|
|||
void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
|
||||
void impl_timerPWMStartDMA(TCH_t * tch);
|
||||
void impl_timerPWMStopDMA(TCH_t * tch);
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
|
||||
void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
|
||||
#endif
|
||||
|
|
@ -408,6 +408,109 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
|
|||
return true;
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
|
||||
{
|
||||
tch->dma = dmaGetByTag(tch->timHw->dmaTag);
|
||||
tch->dmaBuffer = dmaBuffer;
|
||||
if (tch->dma == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If DMA is already in use - abort
|
||||
if (dmaGetOwner(tch->dma) != OWNER_FREE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!tch->timCtx->dmaBurstRef) {
|
||||
// We assume that timer channels are already initialized by calls to:
|
||||
// timerConfigBase
|
||||
// timerPWMConfigChannel
|
||||
const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
|
||||
|
||||
LL_DMA_DeInit(tch->dma->dma, streamLL);
|
||||
|
||||
LL_DMA_InitTypeDef init;
|
||||
LL_DMA_StructInit(&init);
|
||||
|
||||
#if defined(STM32H7) || defined(STM32G4)
|
||||
// For H7 the DMA periphRequest is encoded in the DMA tag
|
||||
init.PeriphRequest = DMATAG_GET_CHANNEL(tch->timHw->dmaTag);
|
||||
#else
|
||||
init.Channel = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)];
|
||||
#endif
|
||||
|
||||
init.PeriphOrM2MSrcAddress = (uint32_t)&tch->timHw->tim->DMAR;
|
||||
init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
|
||||
|
||||
switch (dmaBufferElementSize) {
|
||||
case 1:
|
||||
init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
|
||||
init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE;
|
||||
break;
|
||||
case 2:
|
||||
init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
|
||||
init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD;
|
||||
break;
|
||||
case 4:
|
||||
init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
|
||||
init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
|
||||
break;
|
||||
default:
|
||||
// Programmer error
|
||||
while(1) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer;
|
||||
init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
|
||||
init.NbData = dmaBufferElementCount;
|
||||
init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
|
||||
init.Mode = LL_DMA_MODE_NORMAL;
|
||||
init.Priority = LL_DMA_PRIORITY_HIGH;
|
||||
init.FIFOMode = LL_DMA_FIFOMODE_ENABLE;
|
||||
init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
|
||||
init.MemBurst = LL_DMA_MBURST_SINGLE;
|
||||
init.PeriphBurst = LL_DMA_PBURST_SINGLE;
|
||||
|
||||
dmaInit(tch->dma, OWNER_TIMER, 0);
|
||||
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
|
||||
|
||||
LL_DMA_Init(tch->dma->dma, streamLL, &init);
|
||||
|
||||
tch->timCtx->dmaBurstRef = tch->dma;
|
||||
burstDmaTimer->burstRequestSource = lookupDMASourceTable[tch->timHw->channelIndex];
|
||||
burstDmaTimer->streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
|
||||
burstDmaTimer->dma = tch->dma->dma;
|
||||
|
||||
tch->dmaState = TCH_DMA_READY;
|
||||
}
|
||||
|
||||
// Start PWM generation
|
||||
if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
|
||||
HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
|
||||
}
|
||||
else {
|
||||
HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
|
||||
{
|
||||
LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, BurstLength);
|
||||
LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL);
|
||||
LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL);
|
||||
/* configure the DMA Burst Mode */
|
||||
LL_TIM_ConfigDMABurst(burstDmaTimer->timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS);
|
||||
/* Enable the TIM DMA Request */
|
||||
//LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer);
|
||||
LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
|
||||
}
|
||||
#endif
|
||||
|
||||
void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
|
||||
{
|
||||
const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
|
||||
|
|
|
|||
|
|
@ -357,6 +357,104 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
|
|||
return true;
|
||||
}
|
||||
|
||||
#ifdef USE_DSHOT_DMAR
|
||||
bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
|
||||
{
|
||||
DMA_InitTypeDef DMA_InitStructure;
|
||||
TIM_TypeDef * timer = tch->timHw->tim;
|
||||
|
||||
if (!tch->timCtx->dmaBurstRef) {
|
||||
tch->dma = dmaGetByTag(tch->timHw->dmaTag);
|
||||
if (tch->dma == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If DMA is already in use - abort
|
||||
if (tch->dma->owner != OWNER_FREE) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// We assume that timer channels are already initialized by calls to:
|
||||
// timerConfigBase
|
||||
// timerPWMConfigChannel
|
||||
|
||||
TIM_CtrlPWMOutputs(timer, ENABLE);
|
||||
TIM_ARRPreloadConfig(timer, ENABLE);
|
||||
|
||||
TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
|
||||
TIM_Cmd(timer, ENABLE);
|
||||
|
||||
if (!tch->timCtx->dmaBurstRef) {
|
||||
dmaInit(tch->dma, OWNER_TIMER, 0);
|
||||
dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
|
||||
|
||||
DMA_DeInit(tch->dma->ref);
|
||||
DMA_Cmd(tch->dma->ref, DISABLE);
|
||||
|
||||
DMA_StructInit(&DMA_InitStructure);
|
||||
|
||||
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&tch->timHw->tim->DMAR;
|
||||
DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount;
|
||||
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
||||
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
|
||||
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
|
||||
|
||||
switch (dmaBufferElementSize) {
|
||||
case 1:
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
|
||||
break;
|
||||
case 2:
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
|
||||
break;
|
||||
case 4:
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
|
||||
break;
|
||||
default:
|
||||
// Programmer error
|
||||
while(1) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef STM32F4
|
||||
DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag);
|
||||
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
|
||||
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
|
||||
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
||||
#else // F3
|
||||
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer;
|
||||
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
|
||||
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
||||
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
|
||||
#endif
|
||||
|
||||
DMA_Init(tch->dma->ref, &DMA_InitStructure);
|
||||
DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE);
|
||||
|
||||
tch->timCtx->dmaBurstRef = tch->dma;
|
||||
tch->timCtx->DMASource = lookupDMASourceTable[tch->timHw->channelIndex];
|
||||
burstDmaTimer->dmaBurstStream = tch->timCtx->dmaBurstRef->ref;
|
||||
burstDmaTimer->burstRequestSource = tch->timCtx->DMASource;
|
||||
|
||||
tch->dmaState = TCH_DMA_READY;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
|
||||
{
|
||||
DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength);
|
||||
DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE);
|
||||
TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
|
||||
TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
|
||||
}
|
||||
#endif
|
||||
|
||||
void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
|
||||
{
|
||||
// Make sure we terminate any DMA transaction currently in progress
|
||||
|
|
|
|||
|
|
@ -156,6 +156,13 @@ static const char * const featureNames[] = {
|
|||
"OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
|
||||
};
|
||||
|
||||
static const char * outputModeNames[] = {
|
||||
"AUTO",
|
||||
"MOTORS",
|
||||
"SERVOS",
|
||||
NULL
|
||||
};
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
static const char * const blackboxIncludeFlagNames[] = {
|
||||
"NAV_ACC",
|
||||
|
|
@ -2514,6 +2521,82 @@ static void cliOsdLayout(char *cmdline)
|
|||
|
||||
#endif
|
||||
|
||||
static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, const timerOverride_t* defaultTimerOverride, int timer)
|
||||
{
|
||||
const char *format = "timer_output_mode %d %s";
|
||||
|
||||
for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
|
||||
if (timer < 0 || timer == i) {
|
||||
outputMode_e mode = to[i].outputMode;
|
||||
bool equalsDefault = false;
|
||||
if(defaultTimerOverride) {
|
||||
outputMode_e defaultMode = defaultTimerOverride[i].outputMode;
|
||||
equalsDefault = mode == defaultMode;
|
||||
cliDefaultPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[defaultMode]);
|
||||
}
|
||||
cliDumpPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[mode]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void cliTimerOutputMode(char *cmdline)
|
||||
{
|
||||
char * saveptr;
|
||||
|
||||
int timer = -1;
|
||||
uint8_t mode;
|
||||
char *tok = strtok_r(cmdline, " ", &saveptr);
|
||||
|
||||
int ii;
|
||||
|
||||
for (ii = 0; tok != NULL; ii++, tok = strtok_r(NULL, " ", &saveptr)) {
|
||||
switch (ii) {
|
||||
case 0:
|
||||
timer = fastA2I(tok);
|
||||
if (timer < 0 || timer >= HARDWARE_TIMER_DEFINITION_COUNT) {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if(!sl_strcasecmp("AUTO", tok)) {
|
||||
mode = OUTPUT_MODE_AUTO;
|
||||
} else if(!sl_strcasecmp("MOTORS", tok)) {
|
||||
mode = OUTPUT_MODE_MOTORS;
|
||||
} else if(!sl_strcasecmp("SERVOS", tok)) {
|
||||
mode = OUTPUT_MODE_SERVOS;
|
||||
} else {
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
switch (ii) {
|
||||
case 0:
|
||||
FALLTHROUGH;
|
||||
case 1:
|
||||
// No args, or just timer. If any of them not provided,
|
||||
// it will be the -1 that we used during initialization, so printOsdLayout()
|
||||
// won't use them for filtering.
|
||||
printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer);
|
||||
break;
|
||||
case 2:
|
||||
timerOverridesMutable(timer)->outputMode = mode;
|
||||
printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer);
|
||||
break;
|
||||
default:
|
||||
// Unhandled
|
||||
cliShowParseError();
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault)
|
||||
{
|
||||
uint32_t mask = featureConfig->enabledFeatures;
|
||||
|
|
@ -3687,6 +3770,10 @@ static void printConfig(const char *cmdline, bool doDiff)
|
|||
}
|
||||
|
||||
cliPrintHashLine("resources");
|
||||
//printResource(dumpMask, &defaultConfig);
|
||||
|
||||
cliPrintHashLine("Timer overrides");
|
||||
printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1);
|
||||
|
||||
// print servo parameters
|
||||
cliPrintHashLine("Outputs [servo]");
|
||||
|
|
@ -4006,6 +4093,7 @@ const clicmd_t cmdTable[] = {
|
|||
#ifdef USE_OSD
|
||||
CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[<layout> [<item> [<col> <row> [<visible>]]]]", cliOsdLayout),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[<timer> [<AUTO|MOTORS|SERVOS>]]", cliTimerOutputMode),
|
||||
};
|
||||
|
||||
static void cliHelp(char *cmdline)
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@
|
|||
#include "fc/cli.h"
|
||||
#include "fc/config.h"
|
||||
#include "fc/controlrate_profile.h"
|
||||
#include "fc/multifunction.h"
|
||||
#include "fc/rc_adjustments.h"
|
||||
#include "fc/rc_smoothing.h"
|
||||
#include "fc/rc_controls.h"
|
||||
|
|
@ -376,7 +377,7 @@ static bool emergencyArmingCanOverrideArmingDisabled(void)
|
|||
|
||||
static bool emergencyArmingIsEnabled(void)
|
||||
{
|
||||
return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM)) && emergencyArmingCanOverrideArmingDisabled();
|
||||
return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled();
|
||||
}
|
||||
|
||||
static void processPilotAndFailSafeActions(float dT)
|
||||
|
|
@ -468,7 +469,7 @@ disarmReason_t getDisarmReason(void)
|
|||
return lastDisarmReason;
|
||||
}
|
||||
|
||||
bool emergencyArmingUpdate(bool armingSwitchIsOn)
|
||||
bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
|
||||
{
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
return false;
|
||||
|
|
@ -497,6 +498,10 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn)
|
|||
toggle = true;
|
||||
}
|
||||
|
||||
if (forceArm) {
|
||||
counter = EMERGENCY_ARMING_MIN_ARM_COUNT;
|
||||
}
|
||||
|
||||
return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
|
||||
}
|
||||
|
||||
|
|
@ -519,9 +524,12 @@ void tryArm(void)
|
|||
}
|
||||
|
||||
#ifdef USE_DSHOT
|
||||
if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXTURTLE) && !FLIGHT_MODE(TURTLE_MODE) &&
|
||||
emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()
|
||||
) {
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE);
|
||||
#else
|
||||
const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
|
||||
#endif
|
||||
if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
|
||||
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||
ENABLE_ARMING_FLAG(ARMED);
|
||||
enableFlightMode(TURTLE_MODE);
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ timeUs_t getLastDisarmTimeUs(void);
|
|||
void tryArm(void);
|
||||
disarmReason_t getDisarmReason(void);
|
||||
|
||||
bool emergencyArmingUpdate(bool armingSwitchIsOn);
|
||||
bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm);
|
||||
|
||||
bool areSensorsCalibrating(void);
|
||||
float getFlightTime(void);
|
||||
|
|
|
|||
|
|
@ -1517,6 +1517,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
}
|
||||
break;
|
||||
|
||||
case MSP2_INAV_OUTPUT_MAPPING_EXT:
|
||||
for (uint8_t i = 0; i < timerHardwareCount; ++i)
|
||||
if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
|
||||
#if defined(SITL_BUILD)
|
||||
sbufWriteU8(dst, i);
|
||||
#else
|
||||
sbufWriteU8(dst, timer2id(timerHardware[i].tim));
|
||||
#endif
|
||||
sbufWriteU8(dst, timerHardware[i].usageFlags);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP2_INAV_MC_BRAKING:
|
||||
#ifdef USE_MR_BRAKING_MODE
|
||||
sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
|
||||
|
|
@ -3666,6 +3678,42 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
|
|||
*ret = MSP_RESULT_ACK;
|
||||
break;
|
||||
#endif
|
||||
#ifndef SITL_BUILD
|
||||
case MSP2_INAV_TIMER_OUTPUT_MODE:
|
||||
if (dataSize == 0) {
|
||||
for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
|
||||
sbufWriteU8(dst, i);
|
||||
sbufWriteU8(dst, timerOverrides(i)->outputMode);
|
||||
}
|
||||
*ret = MSP_RESULT_ACK;
|
||||
} else if(dataSize == 1) {
|
||||
uint8_t timer = sbufReadU8(src);
|
||||
if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
|
||||
sbufWriteU8(dst, timer);
|
||||
sbufWriteU8(dst, timerOverrides(timer)->outputMode);
|
||||
*ret = MSP_RESULT_ACK;
|
||||
} else {
|
||||
*ret = MSP_RESULT_ERROR;
|
||||
}
|
||||
} else {
|
||||
*ret = MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
case MSP2_INAV_SET_TIMER_OUTPUT_MODE:
|
||||
if(dataSize == 2) {
|
||||
uint8_t timer = sbufReadU8(src);
|
||||
uint8_t outputMode = sbufReadU8(src);
|
||||
if(timer < HARDWARE_TIMER_DEFINITION_COUNT) {
|
||||
timerOverridesMutable(timer)->outputMode = outputMode;
|
||||
*ret = MSP_RESULT_ACK;
|
||||
} else {
|
||||
*ret = MSP_RESULT_ERROR;
|
||||
}
|
||||
} else {
|
||||
*ret = MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
// Not handled
|
||||
|
|
|
|||
|
|
@ -99,8 +99,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 },
|
||||
{ .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 },
|
||||
{ .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 60 },
|
||||
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 61 },
|
||||
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 62 },
|
||||
{ .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 },
|
||||
{ .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 },
|
||||
{ .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 },
|
||||
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -182,6 +183,9 @@ void initActiveBoxIds(void)
|
|||
RESET_BOX_ID_COUNT;
|
||||
ADD_ACTIVE_BOX(BOXARM);
|
||||
ADD_ACTIVE_BOX(BOXPREARM);
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
ADD_ACTIVE_BOX(BOXMULTIFUNCTION);
|
||||
#endif
|
||||
|
||||
if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
|
||||
ADD_ACTIVE_BOX(BOXANGLE);
|
||||
|
|
@ -230,6 +234,8 @@ void initActiveBoxIds(void)
|
|||
if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) {
|
||||
ADD_ACTIVE_BOX(BOXNAVRTH);
|
||||
ADD_ACTIVE_BOX(BOXNAVWP);
|
||||
ADD_ACTIVE_BOX(BOXNAVCRUISE);
|
||||
ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
|
||||
ADD_ACTIVE_BOX(BOXHOMERESET);
|
||||
ADD_ACTIVE_BOX(BOXGCSNAV);
|
||||
ADD_ACTIVE_BOX(BOXPLANWPMISSION);
|
||||
|
|
@ -239,8 +245,6 @@ void initActiveBoxIds(void)
|
|||
}
|
||||
|
||||
if (STATE(AIRPLANE)) {
|
||||
ADD_ACTIVE_BOX(BOXNAVCRUISE);
|
||||
ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD);
|
||||
ADD_ACTIVE_BOX(BOXSOARING);
|
||||
}
|
||||
}
|
||||
|
|
@ -420,6 +424,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
|
|||
#ifdef USE_MULTI_MISSION
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION);
|
||||
#endif
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION);
|
||||
#endif
|
||||
#if (MAX_MIXER_PROFILE_COUNT > 1)
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE);
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
|
||||
|
|
|
|||
|
|
@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_PITOT] = {
|
||||
.taskName = "PITOT",
|
||||
.taskFunc = taskUpdatePitot,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(100),
|
||||
.desiredPeriod = TASK_PERIOD_MS(20),
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
#endif
|
||||
|
|
|
|||
132
src/main/fc/multifunction.c
Normal file
132
src/main/fc/multifunction.c
Normal file
|
|
@ -0,0 +1,132 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute 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.
|
||||
*
|
||||
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
#include "build/debug.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/multifunction.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/osd.h"
|
||||
#include "navigation/navigation.h"
|
||||
|
||||
multi_function_e selectedItem = MULTI_FUNC_NONE;
|
||||
uint8_t multiFunctionFlags;
|
||||
bool nextItemIsAvailable = false;
|
||||
|
||||
static void multiFunctionApply(multi_function_e selectedItem)
|
||||
{
|
||||
switch (selectedItem) {
|
||||
case MULTI_FUNC_NONE:
|
||||
break;
|
||||
case MULTI_FUNC_1: // redisplay current warnings
|
||||
osdResetWarningFlags();
|
||||
break;
|
||||
case MULTI_FUNC_2: // control manual emergency landing
|
||||
checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
|
||||
break;
|
||||
case MULTI_FUNC_3: // toggle Safehome suspend
|
||||
#if defined(USE_SAFE_HOME)
|
||||
if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
|
||||
MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case MULTI_FUNC_4: // toggle RTH Trackback suspend
|
||||
if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
|
||||
MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK);
|
||||
}
|
||||
break;
|
||||
case MULTI_FUNC_5:
|
||||
#ifdef USE_DSHOT
|
||||
if (STATE(MULTIROTOR)) { // toggle Turtle mode
|
||||
MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case MULTI_FUNC_6: // emergency ARM
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
emergencyArmingUpdate(true, true);
|
||||
}
|
||||
case MULTI_FUNC_END:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool isNextMultifunctionItemAvailable(void)
|
||||
{
|
||||
return nextItemIsAvailable;
|
||||
}
|
||||
|
||||
void setMultifunctionSelection(multi_function_e item)
|
||||
{
|
||||
selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item;
|
||||
nextItemIsAvailable = false;
|
||||
}
|
||||
|
||||
multi_function_e multiFunctionSelection(void)
|
||||
{
|
||||
static timeMs_t startTimer;
|
||||
static timeMs_t selectTimer;
|
||||
static bool toggle = true;
|
||||
const timeMs_t currentTime = millis();
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
|
||||
if (selectTimer) {
|
||||
if (currentTime - selectTimer > 3000) { // 3s selection duration to activate selected function
|
||||
multiFunctionApply(selectedItem);
|
||||
selectTimer = 0;
|
||||
selectedItem = MULTI_FUNC_NONE;
|
||||
nextItemIsAvailable = false;
|
||||
}
|
||||
} else if (toggle) {
|
||||
if (selectedItem == MULTI_FUNC_NONE) {
|
||||
selectedItem++;
|
||||
} else {
|
||||
selectTimer = currentTime;
|
||||
nextItemIsAvailable = true;
|
||||
}
|
||||
}
|
||||
startTimer = currentTime;
|
||||
toggle = false;
|
||||
} else if (startTimer) {
|
||||
if (!toggle && selectTimer) {
|
||||
setMultifunctionSelection(++selectedItem);
|
||||
}
|
||||
if (currentTime - startTimer > 4000) { // 4s reset delay after mode deselected
|
||||
startTimer = 0;
|
||||
selectedItem = MULTI_FUNC_NONE;
|
||||
}
|
||||
selectTimer = 0;
|
||||
toggle = true;
|
||||
}
|
||||
|
||||
return selectedItem;
|
||||
}
|
||||
#endif
|
||||
55
src/main/fc/multifunction.h
Normal file
55
src/main/fc/multifunction.h
Normal file
|
|
@ -0,0 +1,55 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute 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.
|
||||
*
|
||||
* This file 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 this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
|
||||
extern uint8_t multiFunctionFlags;
|
||||
|
||||
#define MULTI_FUNC_FLAG_DISABLE(mask) (multiFunctionFlags &= ~(mask))
|
||||
#define MULTI_FUNC_FLAG_ENABLE(mask) (multiFunctionFlags |= (mask))
|
||||
#define MULTI_FUNC_FLAG(mask) (multiFunctionFlags & (mask))
|
||||
|
||||
typedef enum {
|
||||
MF_SUSPEND_SAFEHOMES = (1 << 0),
|
||||
MF_SUSPEND_TRACKBACK = (1 << 1),
|
||||
MF_TURTLE_MODE = (1 << 2),
|
||||
} multiFunctionFlags_e;
|
||||
|
||||
typedef enum {
|
||||
MULTI_FUNC_NONE,
|
||||
MULTI_FUNC_1,
|
||||
MULTI_FUNC_2,
|
||||
MULTI_FUNC_3,
|
||||
MULTI_FUNC_4,
|
||||
MULTI_FUNC_5,
|
||||
MULTI_FUNC_6,
|
||||
MULTI_FUNC_END,
|
||||
} multi_function_e;
|
||||
|
||||
multi_function_e multiFunctionSelection(void);
|
||||
bool isNextMultifunctionItemAvailable(void);
|
||||
void setMultifunctionSelection(multi_function_e item);
|
||||
#endif
|
||||
|
|
@ -226,7 +226,7 @@ void processRcStickPositions(bool isThrottleLow)
|
|||
rcDisarmTimeMs = currentTimeMs;
|
||||
tryArm();
|
||||
} else {
|
||||
emergencyArmingUpdate(armingSwitchIsActive);
|
||||
emergencyArmingUpdate(armingSwitchIsActive, false);
|
||||
// Disarming via ARM BOX
|
||||
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
|
||||
// and can't afford to risk disarming in the air
|
||||
|
|
|
|||
|
|
@ -78,8 +78,9 @@ typedef enum {
|
|||
BOXUSER4 = 49,
|
||||
BOXCHANGEMISSION = 50,
|
||||
BOXBEEPERMUTE = 51,
|
||||
BOXMIXERPROFILE = 52,
|
||||
BOXMIXERTRANSITION = 53,
|
||||
BOXMULTIFUNCTION = 52,
|
||||
BOXMIXERPROFILE = 53,
|
||||
BOXMIXERTRANSITION = 54,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
14
src/main/fc/settings.yaml
Normal file → Executable file
14
src/main/fc/settings.yaml
Normal file → Executable file
|
|
@ -19,7 +19,7 @@ tables:
|
|||
values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
|
||||
enum: baroSensor_e
|
||||
- name: pitot_hardware
|
||||
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
|
||||
values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
|
||||
enum: pitotSensor_e
|
||||
- name: receiver_type
|
||||
values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
|
||||
|
|
@ -2614,6 +2614,12 @@ groups:
|
|||
default_value: ON
|
||||
field: general.flags.mission_planner_reset
|
||||
type: bool
|
||||
- name: nav_cruise_yaw_rate
|
||||
description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]"
|
||||
default_value: 20
|
||||
field: general.cruise_yaw_rate
|
||||
min: 0
|
||||
max: 120
|
||||
- name: nav_mc_bank_angle
|
||||
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
|
||||
default_value: 30
|
||||
|
|
@ -2849,12 +2855,6 @@ groups:
|
|||
field: fw.launch_abort_deadband
|
||||
min: 2
|
||||
max: 250
|
||||
- name: nav_fw_cruise_yaw_rate
|
||||
description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]"
|
||||
default_value: 20
|
||||
field: fw.cruise_yaw_rate
|
||||
min: 0
|
||||
max: 60
|
||||
- name: nav_fw_allow_manual_thr_increase
|
||||
description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing"
|
||||
default_value: OFF
|
||||
|
|
|
|||
|
|
@ -353,11 +353,9 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
|
|||
|
||||
// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
|
||||
// GPS must also be working, and home position set
|
||||
if (failsafeConfig()->failsafe_min_distance > 0 &&
|
||||
sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
|
||||
|
||||
if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
|
||||
// get the distance to the original arming point
|
||||
uint32_t distance = calculateDistanceToDestination(&original_rth_home);
|
||||
uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
|
||||
if (distance < failsafeConfig()->failsafe_min_distance) {
|
||||
// Use the alternate, minimum distance failsafe procedure instead
|
||||
return failsafeConfig()->failsafe_min_distance_procedure;
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config_reset.h"
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
|
@ -91,9 +92,17 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
|||
.mincommand = SETTING_MIN_COMMAND_DEFAULT,
|
||||
.motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles
|
||||
);
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0);
|
||||
|
||||
#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
|
||||
|
||||
void pgResetFn_timerOverrides(timerOverride_t *instance)
|
||||
{
|
||||
for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) {
|
||||
RESET_CONFIG(timerOverride_t, &instance[i], .outputMode = OUTPUT_MODE_AUTO);
|
||||
}
|
||||
}
|
||||
|
||||
int getThrottleIdleValue(void)
|
||||
{
|
||||
if (!throttleIdleValue) {
|
||||
|
|
|
|||
|
|
@ -19,6 +19,8 @@
|
|||
|
||||
#include "config/parameter_group.h"
|
||||
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#if defined(TARGET_MOTOR_COUNT)
|
||||
#define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT
|
||||
#else
|
||||
|
|
@ -62,6 +64,13 @@ typedef struct motorMixer_s {
|
|||
float yaw;
|
||||
} motorMixer_t;
|
||||
|
||||
|
||||
typedef struct timerOverride_s {
|
||||
uint8_t outputMode;
|
||||
} timerOverride_t;
|
||||
|
||||
PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides);
|
||||
|
||||
typedef struct reversibleMotorsConfig_s {
|
||||
uint16_t deadband_low; // min 3d value
|
||||
uint16_t deadband_high; // max 3d value
|
||||
|
|
|
|||
|
|
@ -900,17 +900,14 @@ float pidHeadingHold(float dT)
|
|||
{
|
||||
float headingHoldRate;
|
||||
|
||||
/* Convert absolute error into relative to current heading */
|
||||
int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
|
||||
|
||||
/*
|
||||
* Convert absolute error into relative to current heading
|
||||
*/
|
||||
if (error <= -180) {
|
||||
error += 360;
|
||||
}
|
||||
|
||||
if (error >= +180) {
|
||||
/* Convert absolute error into relative to current heading */
|
||||
if (error > 180) {
|
||||
error -= 360;
|
||||
} else if (error < -180) {
|
||||
error += 360;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -159,12 +159,12 @@ typedef struct ledStripConfig_s {
|
|||
PG_DECLARE(ledStripConfig_t, ledStripConfig);
|
||||
|
||||
#define DEFINE_LED(ledConfigPtr, x, y, col, dir, func, ol, params) { \
|
||||
ledConfigPtr->led_position = CALCULATE_LED_XY(x, y); \
|
||||
ledConfigPtr->led_color = (col); \
|
||||
ledConfigPtr->led_direction = (dir); \
|
||||
ledConfigPtr->led_function = (func); \
|
||||
ledConfigPtr->led_overlay = (ol); \
|
||||
ledConfigPtr->led_params = (params); }
|
||||
(ledConfigPtr)->led_position = CALCULATE_LED_XY(x, y); \
|
||||
(ledConfigPtr)->led_color = (col); \
|
||||
(ledConfigPtr)->led_direction = (dir); \
|
||||
(ledConfigPtr)->led_function = (func); \
|
||||
(ledConfigPtr)->led_overlay = (ol); \
|
||||
(ledConfigPtr)->led_params = (params); }
|
||||
|
||||
static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return (lcfg->led_position); }
|
||||
static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((lcfg->led_position >> (LED_X_BIT_OFFSET)) & LED_XY_MASK); }
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@
|
|||
#include "fc/controlrate_profile.h"
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
#include "fc/multifunction.h"
|
||||
#include "fc/rc_adjustments.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
|
|
@ -99,6 +100,7 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/diagnostics.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
|
|
@ -186,6 +188,9 @@ static bool fullRedraw = false;
|
|||
|
||||
static uint8_t armState;
|
||||
|
||||
static textAttributes_t osdGetMultiFunctionMessage(char *buff);
|
||||
static uint8_t osdWarningsFlags = 0;
|
||||
|
||||
typedef struct osdMapData_s {
|
||||
uint32_t scale;
|
||||
char referenceSymbol;
|
||||
|
|
@ -986,7 +991,7 @@ static const char * osdFailsafeInfoMessage(void)
|
|||
#if defined(USE_SAFE_HOME)
|
||||
static const char * divertingToSafehomeMessage(void)
|
||||
{
|
||||
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
|
||||
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
|
||||
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
|
||||
}
|
||||
return NULL;
|
||||
|
|
@ -1032,7 +1037,7 @@ static const char * navigationStateMessage(void)
|
|||
case MW_NAV_STATE_HOVER_ABOVE_HOME:
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
#if defined(USE_SAFE_HOME)
|
||||
if (safehome_applied) {
|
||||
if (posControl.safehomeState.isApplied) {
|
||||
return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1787,8 +1792,10 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
buff[1] = SYM_SAT_R;
|
||||
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
|
||||
if (!STATE(GPS_FIX)) {
|
||||
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
|
||||
strcpy(buff + 2, "X!");
|
||||
hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
|
||||
if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) {
|
||||
buff[2] = SYM_ALERT;
|
||||
buff[3] = '\0';
|
||||
}
|
||||
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
||||
}
|
||||
|
|
@ -2041,7 +2048,6 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
case OSD_ALTITUDE_MSL:
|
||||
{
|
||||
int32_t alt = osdGetAltitudeMsl();
|
||||
|
|
@ -3475,6 +3481,21 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
}
|
||||
#endif // USE_ADC
|
||||
#endif // USE_POWER_LIMITS
|
||||
case OSD_MULTI_FUNCTION:
|
||||
{
|
||||
// message shown infrequently so only write when needed
|
||||
static bool clearMultiFunction = true;
|
||||
elemAttr = osdGetMultiFunctionMessage(buff);
|
||||
if (buff[0] == 0) {
|
||||
if (clearMultiFunction) {
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
|
||||
clearMultiFunction = false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
clearMultiFunction = true;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
|
|
@ -3852,6 +3873,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
|
|||
osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3);
|
||||
osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4);
|
||||
|
||||
osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4);
|
||||
|
||||
osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7);
|
||||
osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8);
|
||||
osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
|
||||
|
|
@ -4419,13 +4442,13 @@ static void osdShowArmed(void)
|
|||
}
|
||||
y += 4;
|
||||
#if defined (USE_SAFE_HOME)
|
||||
if (safehome_distance) { // safehome found during arming
|
||||
if (posControl.safehomeState.distance) { // safehome found during arming
|
||||
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
|
||||
strcpy(buf, "SAFEHOME FOUND; MODE OFF");
|
||||
} else {
|
||||
char buf2[12]; // format the distance first
|
||||
osdFormatDistanceStr(buf2, safehome_distance);
|
||||
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
|
||||
osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
|
||||
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
|
||||
}
|
||||
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
|
||||
// write this message above the ARMED message to make it obvious
|
||||
|
|
@ -4553,7 +4576,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
|
|||
osdShowArmed();
|
||||
uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
|
||||
#if defined(USE_SAFE_HOME)
|
||||
if (safehome_distance)
|
||||
if (posControl.safehomeState.distance)
|
||||
delay *= 3;
|
||||
#endif
|
||||
osdSetNextRefreshIn(delay);
|
||||
|
|
@ -4882,6 +4905,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
// by OSD_FLYMODE.
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
|
||||
}
|
||||
if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
if (posControl.cruise.multicopterSpeed >= 50.0f) {
|
||||
char buf[6];
|
||||
osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
|
||||
tfp_sprintf(messageBuf, "(SPD %s)", buf);
|
||||
} else {
|
||||
strcpy(messageBuf, "(HOLD)");
|
||||
}
|
||||
messages[messageCount++] = messageBuf;
|
||||
}
|
||||
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
|
||||
}
|
||||
|
|
@ -4952,12 +4985,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef USE_DEV_TOOLS
|
||||
if (systemConfig()->groundTestMode) {
|
||||
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE);
|
||||
}
|
||||
#endif
|
||||
|
||||
if (messageCount > 0) {
|
||||
message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)];
|
||||
if (message == failsafeInfoMessage) {
|
||||
|
|
@ -4980,4 +5007,191 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
|
|||
return elemAttr;
|
||||
}
|
||||
|
||||
void osdResetWarningFlags(void)
|
||||
{
|
||||
osdWarningsFlags = 0;
|
||||
}
|
||||
|
||||
static bool osdCheckWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount)
|
||||
{
|
||||
#define WARNING_REDISPLAY_DURATION 5000; // milliseconds
|
||||
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
static timeMs_t warningDisplayStartTime = 0;
|
||||
static timeMs_t redisplayStartTimeMs = 0;
|
||||
static uint16_t osdWarningTimerDuration;
|
||||
static uint8_t newWarningFlags;
|
||||
|
||||
if (condition) { // condition required to trigger warning
|
||||
if (!(osdWarningsFlags & warningFlag)) {
|
||||
osdWarningsFlags |= warningFlag;
|
||||
newWarningFlags |= warningFlag;
|
||||
redisplayStartTimeMs = 0;
|
||||
}
|
||||
#ifdef USE_DEV_TOOLS
|
||||
if (systemConfig()->groundTestMode) {
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
/* Warnings displayed in full for set time before shrinking down to alert symbol with warning count only.
|
||||
* All current warnings then redisplayed for 5s on 30s rolling cycle.
|
||||
* New warnings dislayed individually for 10s */
|
||||
if (currentTimeMs > redisplayStartTimeMs) {
|
||||
warningDisplayStartTime = currentTimeMs;
|
||||
osdWarningTimerDuration = newWarningFlags ? 10000 : WARNING_REDISPLAY_DURATION;
|
||||
redisplayStartTimeMs = currentTimeMs + osdWarningTimerDuration + 30000;
|
||||
}
|
||||
|
||||
if (currentTimeMs - warningDisplayStartTime < osdWarningTimerDuration) {
|
||||
return (newWarningFlags & warningFlag) || osdWarningTimerDuration == WARNING_REDISPLAY_DURATION;
|
||||
} else {
|
||||
newWarningFlags = 0;
|
||||
}
|
||||
*warningsCount += 1;
|
||||
} else if (osdWarningsFlags & warningFlag) {
|
||||
osdWarningsFlags &= ~warningFlag;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static textAttributes_t osdGetMultiFunctionMessage(char *buff)
|
||||
{
|
||||
/* Message length limit 10 char max */
|
||||
|
||||
textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
|
||||
static uint8_t warningsCount;
|
||||
const char *message = NULL;
|
||||
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
/* --- FUNCTIONS --- */
|
||||
multi_function_e selectedFunction = multiFunctionSelection();
|
||||
|
||||
if (selectedFunction) {
|
||||
multi_function_e activeFunction = selectedFunction;
|
||||
|
||||
switch (selectedFunction) {
|
||||
case MULTI_FUNC_NONE:
|
||||
case MULTI_FUNC_1:
|
||||
message = warningsCount ? "WARNINGS !" : "0 WARNINGS";
|
||||
break;
|
||||
case MULTI_FUNC_2:
|
||||
message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
|
||||
break;
|
||||
case MULTI_FUNC_3:
|
||||
#if defined(USE_SAFE_HOME)
|
||||
if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
|
||||
message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
activeFunction++;
|
||||
FALLTHROUGH;
|
||||
case MULTI_FUNC_4:
|
||||
if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
|
||||
message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
|
||||
break;
|
||||
}
|
||||
activeFunction++;
|
||||
FALLTHROUGH;
|
||||
case MULTI_FUNC_5:
|
||||
#ifdef USE_DSHOT
|
||||
if (STATE(MULTIROTOR)) {
|
||||
message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
activeFunction++;
|
||||
FALLTHROUGH;
|
||||
case MULTI_FUNC_6:
|
||||
message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
|
||||
break;
|
||||
case MULTI_FUNC_END:
|
||||
break;
|
||||
}
|
||||
|
||||
if (activeFunction != selectedFunction) {
|
||||
setMultifunctionSelection(activeFunction);
|
||||
}
|
||||
|
||||
strcpy(buff, message);
|
||||
|
||||
if (isNextMultifunctionItemAvailable()) {
|
||||
// provides feedback indicating when a new selection command has been received by flight controller
|
||||
buff[9] = '>';
|
||||
}
|
||||
|
||||
return elemAttr;
|
||||
}
|
||||
#endif // MULTIFUNCTION - functions only, warnings always defined
|
||||
|
||||
/* --- WARNINGS --- */
|
||||
const char *messages[7];
|
||||
uint8_t messageCount = 0;
|
||||
bool warningCondition = false;
|
||||
warningsCount = 0;
|
||||
uint8_t warningFlagID = 1;
|
||||
|
||||
// Low Battery
|
||||
const batteryState_e batteryState = getBatteryState();
|
||||
warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
|
||||
if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) {
|
||||
messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
|
||||
}
|
||||
|
||||
#if defined(USE_GPS)
|
||||
// GPS Fix and Failure
|
||||
if (feature(FEATURE_GPS)) {
|
||||
if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) {
|
||||
bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
|
||||
messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
|
||||
}
|
||||
}
|
||||
|
||||
// RTH sanity (warning if RTH heads 200m further away from home than closest point)
|
||||
warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
|
||||
(posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
|
||||
if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
|
||||
messages[messageCount++] = "RTH SANITY";
|
||||
}
|
||||
|
||||
// Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
|
||||
if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) {
|
||||
messages[messageCount++] = "ALT SANITY";
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_MAG)
|
||||
// Magnetometer failure
|
||||
if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
|
||||
hardwareSensorStatus_e magStatus = getHwCompassStatus();
|
||||
if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) {
|
||||
messages[messageCount++] = "MAG FAILED";
|
||||
}
|
||||
}
|
||||
#endif
|
||||
// Vibration levels TODO - needs better vibration measurement to be useful
|
||||
// const float vibrationLevel = accGetVibrationLevel();
|
||||
// warningCondition = vibrationLevel > 1.5f;
|
||||
// if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
|
||||
// messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
|
||||
// }
|
||||
|
||||
#ifdef USE_DEV_TOOLS
|
||||
if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {
|
||||
messages[messageCount++] = "GRD TEST !";
|
||||
}
|
||||
#endif
|
||||
|
||||
if (messageCount) {
|
||||
message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; // display each warning on 1s cycle
|
||||
strcpy(buff, message);
|
||||
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
|
||||
} else if (warningsCount) {
|
||||
buff[0] = SYM_ALERT;
|
||||
tfp_sprintf(buff + 1, "%u ", warningsCount);
|
||||
}
|
||||
|
||||
return elemAttr;
|
||||
}
|
||||
#endif // OSD
|
||||
|
|
|
|||
|
|
@ -273,6 +273,7 @@ typedef enum {
|
|||
OSD_CROSS_TRACK_ERROR,
|
||||
OSD_PILOT_NAME,
|
||||
OSD_PAN_SERVO_CENTRED,
|
||||
OSD_MULTI_FUNCTION,
|
||||
OSD_ITEM_COUNT // MUST BE LAST
|
||||
} osd_items_e;
|
||||
|
||||
|
|
@ -488,6 +489,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max);
|
|||
// Returns a heading angle in degrees normalized to [0, 360).
|
||||
int osdGetHeadingAngle(int angle);
|
||||
|
||||
void osdResetWarningFlags(void);
|
||||
|
||||
int16_t osdGetPanServoOffset(void);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -31,6 +31,9 @@
|
|||
#define MSP2_INAV_OUTPUT_MAPPING 0x200A
|
||||
#define MSP2_INAV_MC_BRAKING 0x200B
|
||||
#define MSP2_INAV_SET_MC_BRAKING 0x200C
|
||||
#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D
|
||||
#define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E
|
||||
#define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F
|
||||
|
||||
#define MSP2_INAV_MIXER 0x2010
|
||||
#define MSP2_INAV_SET_MIXER 0x2011
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
|
||||
#include "fc/fc_core.h"
|
||||
#include "fc/config.h"
|
||||
#include "fc/multifunction.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
|
@ -82,17 +83,10 @@ gpsLocation_t GPS_home;
|
|||
uint32_t GPS_distanceToHome; // distance to home point in meters
|
||||
int16_t GPS_directionToHome; // direction to home point in degrees
|
||||
|
||||
fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
|
||||
|
||||
radar_pois_t radar_pois[RADAR_MAX_POIS];
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
|
||||
uint32_t safehome_distance = 0; // distance to the nearest safehome
|
||||
fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming
|
||||
bool safehome_applied = false; // whether the safehome has been applied to home.
|
||||
|
||||
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
|
||||
|
||||
#endif
|
||||
|
||||
// waypoint 254, 255 are special waypoints
|
||||
|
|
@ -102,7 +96,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, 4);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
|
||||
|
||||
PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
||||
.general = {
|
||||
|
|
@ -159,6 +153,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
|
@ -214,7 +209,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
|
||||
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
|
||||
|
||||
.cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
|
||||
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
|
|
@ -433,7 +427,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
|
||||
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -492,7 +486,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS,
|
||||
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS,
|
||||
.timeoutMs = 10,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
|
||||
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE,
|
||||
.mwState = MW_NAV_STATE_NONE,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
|
|
@ -1008,7 +1002,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
|
|||
return navFSM[state].stateFlags;
|
||||
}
|
||||
|
||||
static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
|
||||
flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
|
||||
{
|
||||
return navFSM[state].mapToFlightModes;
|
||||
}
|
||||
|
|
@ -1115,7 +1109,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now
|
||||
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -1127,7 +1121,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
|
|||
|
||||
resetPositionController();
|
||||
|
||||
if (STATE(AIRPLANE)) {
|
||||
posControl.cruise.course = posControl.actualState.cog; // Store the course to follow
|
||||
} else { // Multicopter
|
||||
posControl.cruise.course = posControl.actualState.yaw;
|
||||
posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed);
|
||||
posControl.desiredState.pos = posControl.actualState.abs.pos;
|
||||
}
|
||||
posControl.cruise.previousCourse = posControl.cruise.course;
|
||||
posControl.cruise.lastCourseAdjustmentTime = 0;
|
||||
|
||||
|
|
@ -1148,15 +1148,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
|
|||
DEBUG_SET(DEBUG_CRUISE, 0, 2);
|
||||
DEBUG_SET(DEBUG_CRUISE, 2, 0);
|
||||
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch
|
||||
return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
|
||||
}
|
||||
|
||||
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
|
||||
|
||||
// User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
|
||||
// We record the desired course and change the desired target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) {
|
||||
int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
|
||||
int16_t headingAdjustCommand = rcCommand[YAW];
|
||||
if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) {
|
||||
headingAdjustCommand = -rcCommand[ROLL];
|
||||
}
|
||||
|
||||
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
|
||||
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
|
||||
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
|
||||
float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
|
||||
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
|
||||
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
|
||||
|
|
@ -1189,7 +1198,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
|
|||
|
||||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
|
||||
{
|
||||
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
|
||||
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
|
||||
return NAV_FSM_EVENT_ERROR;
|
||||
}
|
||||
|
||||
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
|
||||
|
||||
|
|
@ -1365,8 +1376,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
|
|||
|
||||
if (posControl.flags.estPosStatus >= EST_USABLE) {
|
||||
const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
|
||||
#else
|
||||
const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
|
||||
#endif
|
||||
const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
|
||||
(rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated);
|
||||
(overrideTrackback && !posControl.flags.forcedRTHActivated);
|
||||
|
||||
if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
|
||||
posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
|
||||
|
|
@ -2178,7 +2194,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to Z-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
|
||||
{
|
||||
posControl.actualState.abs.pos.z = newAltitude;
|
||||
posControl.actualState.abs.vel.z = newVelocity;
|
||||
|
|
@ -2201,11 +2217,14 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = true;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
/* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
|
||||
posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estAglStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = false;
|
||||
posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
|
|
@ -2570,12 +2589,13 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
|
|||
}
|
||||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
|
||||
void checkSafeHomeState(bool shouldBeEnabled)
|
||||
{
|
||||
const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
|
||||
posControl.flags.rthTrackbackActive ||
|
||||
(!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance);
|
||||
bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
|
||||
(!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
|
||||
#ifdef USE_MULTI_FUNCTIONS
|
||||
safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
|
||||
#endif
|
||||
|
||||
if (safehomeNotApplicable) {
|
||||
shouldBeEnabled = false;
|
||||
|
|
@ -2585,17 +2605,17 @@ void checkSafeHomeState(bool shouldBeEnabled)
|
|||
shouldBeEnabled = posControl.flags.forcedRTHActivated;
|
||||
}
|
||||
// no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
|
||||
if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
|
||||
if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
|
||||
return;
|
||||
}
|
||||
if (shouldBeEnabled) {
|
||||
// set home to safehome
|
||||
setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = true;
|
||||
setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
posControl.safehomeState.isApplied = true;
|
||||
} else {
|
||||
// set home to original arming point
|
||||
setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
safehome_applied = false;
|
||||
setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
posControl.safehomeState.isApplied = false;
|
||||
}
|
||||
// if we've changed the home position, update the distance and direction
|
||||
updateHomePosition();
|
||||
|
|
@ -2607,7 +2627,7 @@ void checkSafeHomeState(bool shouldBeEnabled)
|
|||
**********************************************************/
|
||||
bool findNearestSafeHome(void)
|
||||
{
|
||||
safehome_index = -1;
|
||||
posControl.safehomeState.index = -1;
|
||||
uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
|
||||
uint32_t distance_to_current;
|
||||
fpVector3_t currentSafeHome;
|
||||
|
|
@ -2623,19 +2643,17 @@ bool findNearestSafeHome(void)
|
|||
distance_to_current = calculateDistanceToDestination(¤tSafeHome);
|
||||
if (distance_to_current < nearest_safehome_distance) {
|
||||
// this safehome is the nearest so far - keep track of it.
|
||||
safehome_index = i;
|
||||
posControl.safehomeState.index = i;
|
||||
nearest_safehome_distance = distance_to_current;
|
||||
nearestSafeHome.x = currentSafeHome.x;
|
||||
nearestSafeHome.y = currentSafeHome.y;
|
||||
nearestSafeHome.z = currentSafeHome.z;
|
||||
posControl.safehomeState.nearestSafeHome = currentSafeHome;
|
||||
}
|
||||
}
|
||||
if (safehome_index >= 0) {
|
||||
safehome_distance = nearest_safehome_distance;
|
||||
if (posControl.safehomeState.index >= 0) {
|
||||
posControl.safehomeState.distance = nearest_safehome_distance;
|
||||
} else {
|
||||
safehome_distance = 0;
|
||||
posControl.safehomeState.distance = 0;
|
||||
}
|
||||
return safehome_distance > 0;
|
||||
return posControl.safehomeState.distance > 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -2665,9 +2683,7 @@ void updateHomePosition(void)
|
|||
#endif
|
||||
setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
// save the current location in case it is replaced by a safehome or HOME_RESET
|
||||
original_rth_home.x = posControl.rthState.homePosition.pos.x;
|
||||
original_rth_home.y = posControl.rthState.homePosition.pos.y;
|
||||
original_rth_home.z = posControl.rthState.homePosition.pos.z;
|
||||
posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3559,13 +3575,15 @@ bool isNavHoldPositionActive(void)
|
|||
navigationIsExecutingAnEmergencyLanding();
|
||||
}
|
||||
|
||||
float getActiveWaypointSpeed(void)
|
||||
float getActiveSpeed(void)
|
||||
{
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
// In manual control mode use different cap for speed
|
||||
/* Currently only applicable for multicopter */
|
||||
|
||||
// Speed limit for modes where speed manually controlled
|
||||
if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return navConfig()->general.max_manual_speed;
|
||||
}
|
||||
else {
|
||||
|
||||
uint16_t waypointSpeed = navConfig()->general.auto_speed;
|
||||
|
||||
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
|
||||
|
|
@ -3585,7 +3603,6 @@ float getActiveWaypointSpeed(void)
|
|||
}
|
||||
|
||||
return waypointSpeed;
|
||||
}
|
||||
}
|
||||
|
||||
bool isWaypointNavTrackingActive(void)
|
||||
|
|
@ -3604,29 +3621,21 @@ static void processNavigationRCAdjustments(void)
|
|||
{
|
||||
/* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
}
|
||||
|
||||
if (navStateFlags & NAV_RC_POS) {
|
||||
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE);
|
||||
if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) {
|
||||
resetMulticopterBrakingMode();
|
||||
}
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingAltitude = false;
|
||||
posControl.flags.isAdjustingPosition = false;
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) {
|
||||
posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput();
|
||||
}
|
||||
else {
|
||||
posControl.flags.isAdjustingHeading = false;
|
||||
}
|
||||
posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput();
|
||||
posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput();
|
||||
posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput();
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -3734,7 +3743,7 @@ static bool isWaypointMissionValid(void)
|
|||
return posControl.waypointListValid && (posControl.waypointCount > 0);
|
||||
}
|
||||
|
||||
static void checkManualEmergencyLandingControl(void)
|
||||
void checkManualEmergencyLandingControl(bool forcedActivation)
|
||||
{
|
||||
static timeMs_t timeout = 0;
|
||||
static int8_t counter = 0;
|
||||
|
|
@ -3759,7 +3768,7 @@ static void checkManualEmergencyLandingControl(void)
|
|||
}
|
||||
|
||||
// Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
|
||||
if (counter >= 5) {
|
||||
if (counter >= 5 || forcedActivation) {
|
||||
counter = 0;
|
||||
posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
|
||||
|
||||
|
|
@ -3796,7 +3805,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
/* Emergency landing controlled manually by rapid switching of Poshold mode.
|
||||
* Landing toggled ON or OFF for each Poshold activation sequence */
|
||||
checkManualEmergencyLandingControl();
|
||||
checkManualEmergencyLandingControl(false);
|
||||
|
||||
/* Emergency landing triggered by failsafe Landing or manually initiated */
|
||||
if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
|
||||
|
|
@ -3983,17 +3992,16 @@ int8_t navigationGetHeadingControlState(void)
|
|||
}
|
||||
|
||||
// For multirotors it depends on navigation system mode
|
||||
// Course hold requires Auto Control to update heading hold target whilst RC adjustment active
|
||||
if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
else {
|
||||
|
||||
return NAV_HEADING_CONTROL_AUTO;
|
||||
}
|
||||
}
|
||||
else {
|
||||
|
||||
return NAV_HEADING_CONTROL_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
bool navigationTerrainFollowingEnabled(void)
|
||||
|
|
@ -4318,6 +4326,7 @@ void navigationInit(void)
|
|||
posControl.wpPlannerActiveWPIndex = 0;
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
posControl.startWpIndex = 0;
|
||||
posControl.safehomeState.isApplied = false;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
#endif
|
||||
|
|
@ -4521,3 +4530,8 @@ bool isAdjustingHeading(void) {
|
|||
int32_t getCruiseHeadingAdjustment(void) {
|
||||
return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse);
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@
|
|||
extern gpsLocation_t GPS_home;
|
||||
extern uint32_t GPS_distanceToHome; // distance to home point in meters
|
||||
extern int16_t GPS_directionToHome; // direction to home point in degrees
|
||||
extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
|
||||
|
||||
extern bool autoThrottleManuallyIncreased;
|
||||
|
||||
|
|
@ -59,13 +58,8 @@ typedef enum {
|
|||
|
||||
PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
|
||||
|
||||
extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
|
||||
extern uint32_t safehome_distance; // distance to the nearest safehome
|
||||
extern bool safehome_applied; // whether the safehome has been applied to home.
|
||||
|
||||
void resetSafeHomes(void); // remove all safehomes
|
||||
bool findNearestSafeHome(void); // Find nearest safehome
|
||||
|
||||
#endif // defined(USE_SAFE_HOME)
|
||||
|
||||
#ifndef NAV_MAX_WAYPOINTS
|
||||
|
|
@ -273,6 +267,7 @@ typedef struct navConfig_s {
|
|||
uint8_t land_detect_sensitivity; // Sensitivity of landing detector
|
||||
uint16_t auto_disarm_delay; // safety time delay for landing detector
|
||||
uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
} general;
|
||||
|
||||
struct {
|
||||
|
|
@ -321,7 +316,6 @@ typedef struct navConfig_s {
|
|||
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
|
||||
bool launch_manual_throttle; // Allows launch with manual throttle control
|
||||
uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort
|
||||
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
|
||||
bool allow_manual_thr_increase;
|
||||
bool useFwNavYawControl;
|
||||
uint8_t yawControlDeadband;
|
||||
|
|
@ -628,6 +622,7 @@ bool isAdjustingHeading(void);
|
|||
float getEstimatedAglPosition(void);
|
||||
bool isEstimatedAglTrusted(void);
|
||||
|
||||
void checkManualEmergencyLandingControl(bool forcedActivation);
|
||||
float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
|
||||
|
||||
/* Returns the heading recorded when home position was acquired.
|
||||
|
|
|
|||
|
|
@ -72,7 +72,6 @@ static bool isRollAdjustmentValid = false;
|
|||
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;
|
||||
|
|
@ -450,7 +449,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
* Calculate NAV heading error
|
||||
* Units are centidegrees
|
||||
*/
|
||||
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
|
||||
int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog);
|
||||
|
||||
// Forced turn direction
|
||||
// If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
|
||||
|
|
@ -856,11 +855,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
}
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return navHeadingError;
|
||||
}
|
||||
|
||||
float navigationGetCrossTrackError(void)
|
||||
{
|
||||
return navCrossTrackError;
|
||||
|
|
|
|||
|
|
@ -218,7 +218,7 @@ void resetMulticopterAltitudeController(void)
|
|||
pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
|
||||
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
|
||||
const float maxSpeed = getActiveWaypointSpeed();
|
||||
const float maxSpeed = getActiveSpeed();
|
||||
nav_speed_up = maxSpeed;
|
||||
nav_accel_z = maxSpeed;
|
||||
nav_speed_down = navConfig()->general.max_auto_climb_rate;
|
||||
|
|
@ -285,14 +285,15 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
bool adjustMulticopterHeadingFromRCInput(void)
|
||||
{
|
||||
if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) {
|
||||
// Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home
|
||||
// Heading during Cruise Hold mode set by Nav function so no adjustment required here
|
||||
if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
posControl.desiredState.yaw = posControl.actualState.yaw;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -402,8 +403,44 @@ void resetMulticopterPositionController(void)
|
|||
}
|
||||
}
|
||||
|
||||
static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment)
|
||||
{
|
||||
static timeMs_t lastUpdateTimeMs;
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
const timeMs_t updateDeltaTimeMs = currentTimeMs - lastUpdateTimeMs;
|
||||
lastUpdateTimeMs = currentTimeMs;
|
||||
|
||||
const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband);
|
||||
|
||||
if (rcVelX > posControl.cruise.multicopterSpeed) {
|
||||
posControl.cruise.multicopterSpeed = rcVelX;
|
||||
} else if (rcVelX < 0 && updateDeltaTimeMs < 100) {
|
||||
posControl.cruise.multicopterSpeed += MS2S(updateDeltaTimeMs) * rcVelX / 2.0f;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
posControl.cruise.multicopterSpeed = constrainf(posControl.cruise.multicopterSpeed, 10.0f, navConfig()->general.max_manual_speed);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void setMulticopterStopPosition(void)
|
||||
{
|
||||
fpVector3_t stopPosition;
|
||||
calculateMulticopterInitialHoldPosition(&stopPosition);
|
||||
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
|
||||
}
|
||||
|
||||
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment)
|
||||
{
|
||||
if (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) {
|
||||
if (rcPitchAdjustment) {
|
||||
return adjustMulticopterCruiseSpeed(rcPitchAdjustment);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Process braking mode
|
||||
processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment);
|
||||
|
||||
|
|
@ -425,16 +462,12 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
|
|||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
else if (posControl.flags.isAdjustingPosition) {
|
||||
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
fpVector3_t stopPosition;
|
||||
calculateMulticopterInitialHoldPosition(&stopPosition);
|
||||
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
|
||||
setMulticopterStopPosition();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static float getVelocityHeadingAttenuationFactor(void)
|
||||
|
|
@ -463,15 +496,28 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
|
|||
|
||||
static void updatePositionVelocityController_MC(const float maxSpeed)
|
||||
{
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
|
||||
// Position held at cruise speeds below 0.5 m/s, otherwise desired neu velocities set directly from cruise speed
|
||||
if (posControl.cruise.multicopterSpeed >= 50) {
|
||||
// Rotate multicopter x velocity from body frame to earth frame
|
||||
posControl.desiredState.vel.x = posControl.cruise.multicopterSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
|
||||
posControl.desiredState.vel.y = posControl.cruise.multicopterSpeed * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course));
|
||||
|
||||
return;
|
||||
} else if (posControl.flags.isAdjustingPosition) {
|
||||
setMulticopterStopPosition();
|
||||
}
|
||||
}
|
||||
|
||||
const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
|
||||
const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
|
||||
|
||||
// Calculate target velocity
|
||||
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
|
||||
float neuVelX = posErrorX * posControl.pids.pos[X].param.kP;
|
||||
float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP;
|
||||
|
||||
// Scale velocity to respect max_speed
|
||||
float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY);
|
||||
float neuVelTotal = calc_length_pythagorean_2D(neuVelX, neuVelY);
|
||||
|
||||
/*
|
||||
* We override computed speed with max speed in following cases:
|
||||
|
|
@ -481,26 +527,23 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
|
|||
if (
|
||||
((navGetCurrentStateFlags() & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) &&
|
||||
!isNavHoldPositionActive() &&
|
||||
newVelTotal < maxSpeed &&
|
||||
neuVelTotal < maxSpeed &&
|
||||
!navConfig()->mc.slowDownForTurning
|
||||
) || newVelTotal > maxSpeed
|
||||
) || neuVelTotal > maxSpeed
|
||||
) {
|
||||
newVelX = maxSpeed * (newVelX / newVelTotal);
|
||||
newVelY = maxSpeed * (newVelY / newVelTotal);
|
||||
newVelTotal = maxSpeed;
|
||||
neuVelX = maxSpeed * (neuVelX / neuVelTotal);
|
||||
neuVelY = maxSpeed * (neuVelY / neuVelTotal);
|
||||
neuVelTotal = maxSpeed;
|
||||
}
|
||||
|
||||
posControl.pids.pos[X].output_constrained = newVelX;
|
||||
posControl.pids.pos[Y].output_constrained = newVelY;
|
||||
posControl.pids.pos[X].output_constrained = neuVelX;
|
||||
posControl.pids.pos[Y].output_constrained = neuVelY;
|
||||
|
||||
// Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
|
||||
const float velHeadFactor = getVelocityHeadingAttenuationFactor();
|
||||
const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
|
||||
posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor;
|
||||
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed);
|
||||
posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor;
|
||||
posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor;
|
||||
}
|
||||
|
||||
static float computeNormalizedVelocity(const float value, const float maxValue)
|
||||
|
|
@ -660,49 +703,53 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
|
||||
static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
bool bypassPositionController;
|
||||
|
||||
// We should passthrough rcCommand is adjusting position in GPS_ATTI mode
|
||||
bypassPositionController = (navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition;
|
||||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
// and pilots input would be passed thru to PID controller
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
||||
if (!bypassPositionController) {
|
||||
// Update position controller
|
||||
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
|
||||
// Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
|
||||
const float maxSpeed = getActiveWaypointSpeed();
|
||||
updatePositionVelocityController_MC(maxSpeed);
|
||||
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first start or glitch), reset altitude controller
|
||||
resetMulticopterPositionController();
|
||||
}
|
||||
}
|
||||
|
||||
// Indicate that information is no longer usable
|
||||
posControl.flags.horizontalPositionDataConsumed = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (posControl.flags.estPosStatus < EST_USABLE) {
|
||||
/* No position data, disable automatic adjustment, rcCommand passthrough */
|
||||
posControl.rcAdjustment[PITCH] = 0;
|
||||
posControl.rcAdjustment[ROLL] = 0;
|
||||
bypassPositionController = true;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Passthrough rcCommand if adjusting position in GPS_ATTI mode except when Course Hold active
|
||||
bool bypassPositionController = !FLIGHT_MODE(NAV_COURSE_HOLD_MODE) &&
|
||||
navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI &&
|
||||
posControl.flags.isAdjustingPosition;
|
||||
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
// Indicate that information is no longer usable
|
||||
posControl.flags.horizontalPositionDataConsumed = true;
|
||||
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
||||
if (bypassPositionController) {
|
||||
return;
|
||||
}
|
||||
|
||||
// If we have new position data - update velocity and acceleration controllers
|
||||
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
|
||||
// Get max speed for current NAV mode
|
||||
float maxSpeed = getActiveSpeed();
|
||||
updatePositionVelocityController_MC(maxSpeed);
|
||||
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
|
||||
|
||||
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
|
||||
navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first start or glitch), reset position controller
|
||||
resetMulticopterPositionController();
|
||||
}
|
||||
} else if (bypassPositionController) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!bypassPositionController) {
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
}
|
||||
}
|
||||
|
||||
bool isMulticopterFlying(void)
|
||||
|
|
@ -926,6 +973,10 @@ void resetMulticopterHeadingController(void)
|
|||
|
||||
static void applyMulticopterHeadingController(void)
|
||||
{
|
||||
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // heading set by Nav during Course Hold so disable yaw stick input
|
||||
rcCommand[YAW] = 0;
|
||||
}
|
||||
|
||||
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -816,11 +816,12 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Publish altitude update and set altitude validity */
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
|
||||
navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
|
||||
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
|
||||
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
|
||||
}
|
||||
else {
|
||||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
|
||||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0);
|
||||
}
|
||||
|
||||
//Update Blackbox states
|
||||
|
|
|
|||
|
|
@ -91,6 +91,7 @@ typedef struct navigationFlags_s {
|
|||
navigationEstimateStatus_e estVelStatus; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estAglStatus;
|
||||
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
|
||||
bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude
|
||||
|
||||
bool isAdjustingPosition;
|
||||
bool isAdjustingAltitude;
|
||||
|
|
@ -337,6 +338,7 @@ typedef struct {
|
|||
int32_t course;
|
||||
int32_t previousCourse;
|
||||
timeMs_t lastCourseAdjustmentTime;
|
||||
float multicopterSpeed;
|
||||
} navCruise_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -347,6 +349,7 @@ typedef struct {
|
|||
float rthFinalAltitude; // Altitude at end of RTH approach
|
||||
float rthInitialDistance; // Distance when starting flight home
|
||||
fpVector3_t homeTmpWaypoint; // Temporary storage for home target
|
||||
fpVector3_t originalHomePosition; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
|
||||
} rthState_t;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -357,6 +360,13 @@ typedef enum {
|
|||
RTH_HOME_FINAL_LAND, // Home position and altitude
|
||||
} rthTargetMode_e;
|
||||
|
||||
typedef struct {
|
||||
fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming
|
||||
uint32_t distance; // distance to the nearest safehome
|
||||
int8_t index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
|
||||
bool isApplied; // whether the safehome has been applied to home
|
||||
} safehomeState_t;
|
||||
|
||||
typedef struct {
|
||||
/* Flags and navigation system state */
|
||||
navigationFSMState_t navState;
|
||||
|
|
@ -379,14 +389,15 @@ typedef struct {
|
|||
/* INAV GPS origin (position where GPS fix was first acquired) */
|
||||
gpsOrigin_t gpsOrigin;
|
||||
|
||||
/* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */
|
||||
/* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */
|
||||
rthSanityChecker_t rthSanityChecker;
|
||||
rthState_t rthState;
|
||||
|
||||
/* Home parameters */
|
||||
uint32_t homeDistance; // cm
|
||||
int32_t homeDirection; // deg*100
|
||||
|
||||
/* Safehome parameters */
|
||||
safehomeState_t safehomeState;
|
||||
|
||||
/* Cruise */
|
||||
navCruise_t cruise;
|
||||
|
||||
|
|
@ -454,6 +465,7 @@ bool isFixedWingFlying(void);
|
|||
bool isMulticopterFlying(void);
|
||||
|
||||
navigationFSMStateFlags_t navGetCurrentStateFlags(void);
|
||||
flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state);
|
||||
|
||||
void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags);
|
||||
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
|
|
@ -463,12 +475,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
|
|||
|
||||
bool isNavHoldPositionActive(void);
|
||||
bool isLastMissionWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
float getActiveSpeed(void);
|
||||
bool isWaypointNavTrackingActive(void);
|
||||
|
||||
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);
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
|
||||
|
||||
bool checkForPositionSensorTimeout(void);
|
||||
|
||||
|
|
|
|||
56
src/main/sensors/pitotmeter.c
Normal file → Executable file
56
src/main/sensors/pitotmeter.c
Normal file → Executable file
|
|
@ -31,6 +31,7 @@
|
|||
|
||||
#include "drivers/pitotmeter/pitotmeter.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_ms4525.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_adc.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_msp.h"
|
||||
#include "drivers/pitotmeter/pitotmeter_virtual.h"
|
||||
|
|
@ -44,11 +45,18 @@
|
|||
#include "scheduler/protothreads.h"
|
||||
|
||||
#include "sensors/pitotmeter.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
|
||||
//#include "build/debug.h"
|
||||
|
||||
|
||||
#ifdef USE_PITOT
|
||||
|
||||
pitot_t pitot;
|
||||
extern baro_t baro;
|
||||
|
||||
pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
|
||||
|
||||
|
|
@ -86,6 +94,15 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
|
|||
}
|
||||
FALLTHROUGH;
|
||||
|
||||
case PITOT_DLVR:
|
||||
|
||||
// Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config
|
||||
if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) {
|
||||
pitotHardware = PITOT_DLVR;
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
||||
case PITOT_ADC:
|
||||
#if defined(USE_ADC) && defined(USE_PITOT_ADC)
|
||||
if (adcPitotDetect(dev)) {
|
||||
|
|
@ -169,7 +186,7 @@ bool pitotIsCalibrationComplete(void)
|
|||
|
||||
void pitotStartCalibration(void)
|
||||
{
|
||||
zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false);
|
||||
zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * pitot.dev.calibThreshold, false);
|
||||
}
|
||||
|
||||
static void performPitotCalibrationCycle(void)
|
||||
|
|
@ -187,10 +204,12 @@ STATIC_PROTOTHREAD(pitotThread)
|
|||
ptBegin(pitotThread);
|
||||
|
||||
static float pitotPressureTmp;
|
||||
static float pitotTemperatureTmp;
|
||||
timeUs_t currentTimeUs;
|
||||
|
||||
// Init filter
|
||||
pitot.lastMeasurementUs = micros();
|
||||
|
||||
pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
|
||||
|
||||
while(1) {
|
||||
|
|
@ -201,19 +220,23 @@ STATIC_PROTOTHREAD(pitotThread)
|
|||
}
|
||||
#endif
|
||||
|
||||
// Start measurement
|
||||
if ( pitot.lastSeenHealthyMs == 0 ) {
|
||||
if (pitot.dev.start(&pitot.dev)) {
|
||||
pitot.lastSeenHealthyMs = millis();
|
||||
}
|
||||
}
|
||||
|
||||
ptDelayUs(pitot.dev.delay);
|
||||
if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
|
||||
if (pitot.dev.get(&pitot.dev)) // read current data
|
||||
pitot.lastSeenHealthyMs = millis();
|
||||
|
||||
// Read and calculate data
|
||||
if (pitot.dev.get(&pitot.dev)) {
|
||||
if (pitot.dev.start(&pitot.dev)) // init for next read
|
||||
pitot.lastSeenHealthyMs = millis();
|
||||
}
|
||||
|
||||
pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
|
||||
|
||||
pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
|
||||
|
||||
#ifdef USE_SIMULATOR
|
||||
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
|
||||
pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
|
||||
|
|
@ -226,14 +249,9 @@ STATIC_PROTOTHREAD(pitotThread)
|
|||
#endif
|
||||
ptYield();
|
||||
|
||||
// Filter pressure
|
||||
currentTimeUs = micros();
|
||||
pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
|
||||
pitot.lastMeasurementUs = currentTimeUs;
|
||||
ptDelayUs(pitot.dev.delay);
|
||||
|
||||
// Calculate IAS
|
||||
if (pitotIsCalibrationComplete()) {
|
||||
// NOTE ::
|
||||
// https://en.wikipedia.org/wiki/Indicated_airspeed
|
||||
// Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system.
|
||||
// The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for
|
||||
|
|
@ -242,11 +260,21 @@ STATIC_PROTOTHREAD(pitotThread)
|
|||
//
|
||||
// Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
|
||||
// It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
|
||||
pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
|
||||
|
||||
// NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
|
||||
currentTimeUs = micros();
|
||||
pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
|
||||
pitot.lastMeasurementUs = currentTimeUs;
|
||||
|
||||
pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; // cm/s
|
||||
pitot.temperature = pitotTemperatureTmp; // Kelvin
|
||||
|
||||
} else {
|
||||
pitot.pressure = pitotPressureTmp;
|
||||
performPitotCalibrationCycle();
|
||||
pitot.airSpeed = 0.0f;
|
||||
}
|
||||
|
||||
#ifdef USE_SIMULATOR
|
||||
if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
|
||||
pitot.airSpeed = simulatorData.airSpeed;
|
||||
|
|
|
|||
2
src/main/sensors/pitotmeter.h
Normal file → Executable file
2
src/main/sensors/pitotmeter.h
Normal file → Executable file
|
|
@ -31,6 +31,7 @@ typedef enum {
|
|||
PITOT_VIRTUAL = 4,
|
||||
PITOT_FAKE = 5,
|
||||
PITOT_MSP = 6,
|
||||
PITOT_DLVR = 7,
|
||||
} pitotSensor_e;
|
||||
|
||||
#define PITOT_MAX PITOT_FAKE
|
||||
|
|
@ -55,6 +56,7 @@ typedef struct pito_s {
|
|||
|
||||
float pressureZero;
|
||||
float pressure;
|
||||
float temperature;
|
||||
} pitot_t;
|
||||
|
||||
#ifdef USE_PITOT
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
|
|||
} flightDynamicsTrims_t;
|
||||
|
||||
#define CALIBRATING_BARO_TIME_MS 2000
|
||||
#define CALIBRATING_PITOT_TIME_MS 2000
|
||||
#define CALIBRATING_PITOT_TIME_MS 4000
|
||||
#define CALIBRATING_GYRO_TIME_MS 2000
|
||||
#define CALIBRATING_ACC_TIME_MS 500
|
||||
#define CALIBRATING_GYRO_MORON_THRESHOLD 32
|
||||
|
|
|
|||
1
src/main/target/BETAFPVF435/CMakeLists.txt
Normal file
1
src/main/target/BETAFPVF435/CMakeLists.txt
Normal file
|
|
@ -0,0 +1 @@
|
|||
target_at32f43x_xGT7(BETAFPVF435 SKIP_RELEASES)
|
||||
36
src/main/target/BETAFPVF435/config.c
Normal file
36
src/main/target/BETAFPVF435/config.c
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*
|
||||
* This target has been autgenerated by bf2inav.py
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "fc/fc_msp_box.h"
|
||||
#include "fc/config.h"
|
||||
|
||||
#include "io/ledstrip.h"
|
||||
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
ledStripConfig_t *config = ledStripConfigMutable();
|
||||
ledConfig_t *lc = config->ledConfigs;
|
||||
DEFINE_LED(lc, 0, 0, COLOR_RED, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0);
|
||||
DEFINE_LED(lc+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0);
|
||||
}
|
||||
|
||||
44
src/main/target/BETAFPVF435/target.c
Normal file
44
src/main/target/BETAFPVF435/target.c
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* INAV are free software. You can redistribute
|
||||
* this software and/or modify this software 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 are distributed in the hope that they
|
||||
* 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 this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pinio.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TMR3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // TMR3_CH3 motor 1
|
||||
DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // TMR3_CH4 motor 2
|
||||
DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 7), // TMR2_CH4 motor 3
|
||||
DEF_TIM(TMR2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 6), // TMR2_CH3 motor 4
|
||||
|
||||
DEF_TIM(TMR8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 4), // TMR8_CH3 motor 5
|
||||
DEF_TIM(TMR1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 5), // TMR1_CH3 motor 6
|
||||
|
||||
DEF_TIM(TMR4, CH1, PB6, TIM_USE_LED, 0, 0), // TMR4_CH1 LED_STRIP
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
201
src/main/target/BETAFPVF435/target.h
Normal file
201
src/main/target/BETAFPVF435/target.h
Normal file
|
|
@ -0,0 +1,201 @@
|
|||
/*
|
||||
* This file is part of INAV.
|
||||
*
|
||||
* INAV are free software. You can redistribute
|
||||
* this software and/or modify this software 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 are distributed in the hope that they
|
||||
* 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 this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
//https://www.arterychip.com/download/DS/DS_AT32F435_437_V2.02-EN.pdf
|
||||
#pragma once
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "BHER"
|
||||
#define USBD_PRODUCT_STRING "BETAFPVF435"
|
||||
|
||||
#define LED0 PB5
|
||||
|
||||
#define USE_BEEPER
|
||||
#define BEEPER PB4
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
//#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
|
||||
//#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO
|
||||
//#define ENABLE_DSHOT
|
||||
|
||||
// *************** Gyro & ACC **********************
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1
|
||||
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define SPI1_NSS_PIN PA4
|
||||
//#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
//#define USE_EXTI
|
||||
//#define USE_GYRO_EXTI
|
||||
//#define GYRO_1_EXTI_PIN PC4
|
||||
//#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
// MPU6000
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW270_DEG
|
||||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
|
||||
// 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 USE_ACC
|
||||
#define USE_IMU_BMI270
|
||||
#define IMU_BMI270_ALIGN CW270_DEG
|
||||
#define BMI270_SPI_BUS BUS_SPI1
|
||||
#define BMI270_CS_PIN SPI1_NSS_PIN
|
||||
|
||||
// *************** Baro **************************
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP280
|
||||
#define SPI3_NSS_PIN PB3
|
||||
#define BMP280_SPI_BUS BUS_SPI3
|
||||
#define BMP280_CS_PIN SPI3_NSS_PIN
|
||||
#define USE_BARO_DPS310
|
||||
#define DPS310_SPI_BUS BUS_SPI3
|
||||
#define DPS310_CS_PIN SPI3_NSS_PIN
|
||||
|
||||
// *************** BLACKBOX **************************
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define SPI2_SCK_PIN PB13
|
||||
#define SPI2_MISO_PIN PB14
|
||||
#define SPI2_MOSI_PIN PB15
|
||||
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
|
||||
#define USE_FLASHFS
|
||||
#define FLASH_CS_PIN PB12
|
||||
|
||||
#define USE_FLASH_M25P16
|
||||
#define M25P16_SPI_BUS BUS_SPI2
|
||||
#define M25P16_CS_PIN FLASH_CS_PIN
|
||||
#define USE_FLASH_W25N01G // 1Gb NAND flash support
|
||||
#define W25N01G_SPI_BUS BUS_SPI2
|
||||
#define W25N01G_CS_PIN FLASH_CS_PIN
|
||||
#define USE_FLASH_W25M // Stacked die support
|
||||
#define W25M_SPI_BUS BUS_SPI2
|
||||
#define W25M_CS_PIN FLASH_CS_PIN
|
||||
#define USE_FLASH_W25M512 // 512Kb (256Kb x 2 stacked) NOR flash support
|
||||
#define W25M512_SPI_BUS BUS_SPI2
|
||||
#define W25M512_CS_PIN FLASH_CS_PIN
|
||||
#define USE_FLASH_W25M02G // 2Gb (1Gb x 2 stacked) NAND flash support
|
||||
#define W25M02G_SPI_BUS BUS_SPI2
|
||||
#define W25M02G_CS_PIN FLASH_CS_PIN
|
||||
|
||||
// *************** OSD *****************************
|
||||
#define USE_SPI_DEVICE_3
|
||||
#define SPI3_SCK_PIN PC10
|
||||
#define SPI3_MISO_PIN PC11
|
||||
#define SPI3_MOSI_PIN PC12
|
||||
|
||||
#if 0
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_BUS BUS_SPI3
|
||||
#define MAX7456_CS_PIN PA15
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// I2C
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_2
|
||||
#define I2C2_SCL PB10 // SCL pad
|
||||
#define I2C2_SDA PB11 // SDA pad
|
||||
#define USE_I2C_PULLUP
|
||||
#endif
|
||||
|
||||
#define USE_BARO
|
||||
#define BARO_I2C_BUS BUS_I2C2
|
||||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_DPS310
|
||||
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
#define USE_USB_DETECT
|
||||
//#define USB_DETECT_PIN PC5
|
||||
|
||||
|
||||
#define USE_UART1
|
||||
#define UART1_RX_PIN PA10
|
||||
#define UART1_TX_PIN PA9
|
||||
|
||||
#define USE_UART3
|
||||
#define UART3_RX_PIN PB11
|
||||
#define UART3_TX_PIN PB10
|
||||
#define USE_UART_INVERTER
|
||||
#define INVERTER_PIN_UART3_RX PC9
|
||||
#define INVERTER_PIN_USART3_RX PC9
|
||||
|
||||
#define USE_UART4
|
||||
#define UART4_RX_PIN PA1
|
||||
#define UART4_TX_PIN PA0
|
||||
|
||||
#define USE_UART5
|
||||
#define UART5_RX_PIN PD2
|
||||
#define UART5_TX_PIN PD2
|
||||
|
||||
#define USE_UART6
|
||||
#define UART6_RX_PIN PC7
|
||||
#define UART6_TX_PIN PC6
|
||||
|
||||
#define SERIAL_PORT_COUNT 6
|
||||
|
||||
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_CRSF
|
||||
#define SERIALRX_UART SERIAL_PORT_USART3
|
||||
|
||||
// *************** ADC *****************************
|
||||
#define USE_ADC
|
||||
#define ADC_INSTANCE ADC1
|
||||
#define ADC1_DMA_STREAM DMA2_CHANNEL5
|
||||
#define ADC_CHANNEL1_PIN PC2
|
||||
#define ADC_CHANNEL2_PIN PC1
|
||||
#define ADC_CHANNEL3_PIN PC0
|
||||
#define VBAT_ADC_CHANNEL ADC_CHN_1
|
||||
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
|
||||
#define RSSI_ADC_CHANNEL ADC_CHN_3
|
||||
|
||||
#define USE_LED_STRIP
|
||||
#define WS2811_PIN PB6
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP )
|
||||
#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
|
||||
#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC
|
||||
|
||||
#define VBAT_SCALE_DEFAULT 110
|
||||
#define CURRENT_METER_SCALE_DEFAULT 800
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD 0xffff
|
||||
#define TARGET_IO_PORTE BIT(2)
|
||||
|
||||
|
||||
#define MAX_PWM_OUTPUT_PORTS 8
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
//#define USE_DSHOT
|
||||
//#define USE_ESC_SENSOR
|
||||
#define USE_ESCSERIAL
|
||||
1
src/main/target/GEPRCF405/CMakeLists.txt
Normal file
1
src/main/target/GEPRCF405/CMakeLists.txt
Normal file
|
|
@ -0,0 +1 @@
|
|||
target_stm32f405xg(GEPRCF405)
|
||||
46
src/main/target/GEPRCF405/target.c
Normal file
46
src/main/target/GEPRCF405/target.c
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "drivers/io.h"
|
||||
|
||||
#include "drivers/dma.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/timer_def.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0),
|
||||
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
|
||||
DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
|
||||
DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0),
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
|
||||
|
||||
177
src/main/target/GEPRCF405/target.h
Normal file
177
src/main/target/GEPRCF405/target.h
Normal file
|
|
@ -0,0 +1,177 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "GEPR"
|
||||
|
||||
#define USBD_PRODUCT_STRING "GEPRCF405"
|
||||
|
||||
#define LED0 PC14
|
||||
#define LED1 PC15
|
||||
|
||||
|
||||
|
||||
#define BEEPER PC13
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
// *************** Gyro & ACC **********************
|
||||
|
||||
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_3
|
||||
|
||||
#define SPI3_SCK_PIN PB3
|
||||
#define SPI3_MISO_PIN PB4
|
||||
#define SPI3_MOSI_PIN PB5
|
||||
|
||||
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW180_DEG
|
||||
#define MPU6000_CS_PIN PA15
|
||||
#define MPU6000_SPI_BUS BUS_SPI3
|
||||
|
||||
#define USE_IMU_BMI270
|
||||
#define IMU_BMI270_ALIGN CW180_DEG
|
||||
#define BMI270_CS_PIN PA15
|
||||
#define BMI270_SPI_BUS BUS_SPI3
|
||||
|
||||
#define USE_IMU_ICM42605
|
||||
#define IMU_ICM42605_ALIGN CW180_DEG
|
||||
#define ICM42605_CS_PIN PA15
|
||||
#define ICM42605_SPI_BUS BUS_SPI3
|
||||
|
||||
// *************** I2C/Baro/Mag *********************
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_1
|
||||
#define I2C1_SCL PB8
|
||||
#define I2C1_SDA PB9
|
||||
|
||||
#define USE_BARO
|
||||
#define BARO_I2C_BUS BUS_I2C1
|
||||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_DPS310
|
||||
#define USE_BARO_MS5611
|
||||
|
||||
#define USE_MAG
|
||||
#define MAG_I2C_BUS BUS_I2C1
|
||||
#define USE_MAG_HMC5883
|
||||
#define USE_MAG_QMC5883
|
||||
#define USE_MAG_IST8310
|
||||
#define USE_MAG_IST8308
|
||||
#define USE_MAG_MAG3110
|
||||
#define USE_MAG_LIS3MDL
|
||||
|
||||
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
||||
|
||||
#define PITOT_I2C_BUS BUS_I2C1
|
||||
|
||||
#define USE_RANGEFINDER
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C1
|
||||
#define BNO055_I2C_BUS BUS_I2C1
|
||||
|
||||
// *************** FLASH **************************
|
||||
#define M25P16_CS_PIN PB12
|
||||
#define M25P16_SPI_BUS BUS_SPI2
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
|
||||
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define SPI2_SCK_PIN PB13
|
||||
#define SPI2_MISO_PIN PB14
|
||||
#define SPI2_MOSI_PIN PB15
|
||||
|
||||
// *************** OSD *****************************
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_BUS BUS_SPI1
|
||||
#define MAX7456_CS_PIN PA4
|
||||
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
|
||||
#define USE_UART1
|
||||
#define UART1_RX_PIN PB7
|
||||
#define UART1_TX_PIN PB6
|
||||
|
||||
#define USE_UART2
|
||||
#define UART2_RX_PIN PA3
|
||||
#define UART2_TX_PIN PA2
|
||||
|
||||
#define USE_UART3
|
||||
#define UART3_RX_PIN PC11
|
||||
#define UART3_TX_PIN PC10
|
||||
|
||||
#define USE_UART4
|
||||
#define UART4_RX_PIN PA1
|
||||
#define UART4_TX_PIN PA0
|
||||
|
||||
#define USE_UART5
|
||||
#define UART5_RX_PIN PD2
|
||||
#define UART5_TX_PIN PC12
|
||||
|
||||
#define USE_UART6
|
||||
#define UART6_RX_PIN PC7
|
||||
#define UART6_TX_PIN PC6
|
||||
|
||||
#define SERIAL_PORT_COUNT 7
|
||||
|
||||
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||
|
||||
// *************** ADC *****************************
|
||||
#define USE_ADC
|
||||
#define ADC_INSTANCE ADC1
|
||||
#define ADC1_DMA_STREAM DMA2_Stream0
|
||||
#define ADC_CHANNEL_1_PIN PC1
|
||||
#define ADC_CHANNEL_2_PIN PC2
|
||||
#define ADC_CHANNEL_3_PIN PC0
|
||||
|
||||
#define VBAT_ADC_CHANNEL ADC_CHN_1
|
||||
#define RSSI_ADC_CHANNEL ADC_CHN_2
|
||||
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
|
||||
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
|
||||
|
||||
|
||||
|
||||
// *************** LEDSTRIP ************************
|
||||
#define USE_LED_STRIP
|
||||
#define WS2811_PIN PB1
|
||||
|
||||
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD (BIT(2))
|
||||
|
||||
|
||||
#define MAX_PWM_OUTPUT_PORTS 6
|
||||
|
||||
#define USE_DSHOT
|
||||
#define USE_ESC_SENSOR
|
||||
1
src/main/target/GEPRCF722/CMakeLists.txt
Normal file
1
src/main/target/GEPRCF722/CMakeLists.txt
Normal file
|
|
@ -0,0 +1 @@
|
|||
target_stm32f722xe(GEPRCF722)
|
||||
44
src/main/target/GEPRCF722/target.c
Normal file
44
src/main/target/GEPRCF722/target.c
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0),
|
||||
|
||||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
|
||||
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0),
|
||||
|
||||
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
173
src/main/target/GEPRCF722/target.h
Normal file
173
src/main/target/GEPRCF722/target.h
Normal file
|
|
@ -0,0 +1,173 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "GEPR"
|
||||
|
||||
#define USBD_PRODUCT_STRING "GEPRCF722"
|
||||
|
||||
#define LED0 PA13
|
||||
#define LED1 PA14
|
||||
|
||||
#define BEEPER PC13
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
// *************** Gyro & ACC **********************
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_3
|
||||
|
||||
#define SPI3_SCK_PIN PC10
|
||||
#define SPI3_MISO_PIN PC11
|
||||
#define SPI3_MOSI_PIN PC12
|
||||
|
||||
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW270_DEG
|
||||
|
||||
#define MPU6000_CS_PIN PA15
|
||||
#define MPU6000_SPI_BUS BUS_SPI3
|
||||
|
||||
#define USE_IMU_BMI270
|
||||
#define IMU_BMI270_ALIGN CW180_DEG
|
||||
#define BMI270_CS_PIN PA15
|
||||
#define BMI270_SPI_BUS BUS_SPI3
|
||||
|
||||
#define USE_IMU_ICM42605
|
||||
#define IMU_ICM42605_ALIGN CW180_DEG
|
||||
#define ICM42605_CS_PIN PA15
|
||||
#define ICM42605_SPI_BUS BUS_SPI3
|
||||
|
||||
// *************** I2C/Baro/Mag *********************
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_1
|
||||
#define I2C1_SCL PB8
|
||||
#define I2C1_SDA PB9
|
||||
|
||||
#define USE_BARO
|
||||
#define BARO_I2C_BUS BUS_I2C1
|
||||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_DPS310
|
||||
#define USE_BARO_MS5611
|
||||
|
||||
#define USE_MAG
|
||||
#define MAG_I2C_BUS BUS_I2C1
|
||||
#define USE_MAG_HMC5883
|
||||
#define USE_MAG_QMC5883
|
||||
#define USE_MAG_IST8310
|
||||
#define USE_MAG_IST8308
|
||||
#define USE_MAG_MAG3110
|
||||
#define USE_MAG_LIS3MDL
|
||||
|
||||
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
||||
|
||||
#define PITOT_I2C_BUS BUS_I2C1
|
||||
|
||||
#define USE_RANGEFINDER
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C1
|
||||
#define BNO055_I2C_BUS BUS_I2C1
|
||||
|
||||
// *************** FLASH **************************
|
||||
#define M25P16_CS_PIN PB12
|
||||
#define M25P16_SPI_BUS BUS_SPI2
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
|
||||
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define SPI2_SCK_PIN PB13
|
||||
#define SPI2_MISO_PIN PB14
|
||||
#define SPI2_MOSI_PIN PB15
|
||||
|
||||
// *************** OSD *****************************
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_BUS BUS_SPI1
|
||||
#define MAX7456_CS_PIN PB2
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
|
||||
#define USE_UART1
|
||||
#define UART1_RX_PIN PA10
|
||||
#define UART1_TX_PIN PA9
|
||||
|
||||
#define USE_UART2
|
||||
#define UART2_RX_PIN PA3
|
||||
#define UART2_TX_PIN PA2
|
||||
|
||||
#define USE_UART3
|
||||
#define UART3_RX_PIN PB11
|
||||
#define UART3_TX_PIN PB10
|
||||
|
||||
#define USE_UART4
|
||||
#define UART4_RX_PIN PA1
|
||||
#define UART4_TX_PIN PA0
|
||||
|
||||
#define USE_UART6
|
||||
#define UART6_RX_PIN PC7
|
||||
#define UART6_TX_PIN PC6
|
||||
|
||||
#define SERIAL_PORT_COUNT 6
|
||||
|
||||
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||
|
||||
// *************** ADC *****************************
|
||||
#define USE_ADC
|
||||
#define ADC_INSTANCE ADC1
|
||||
#define ADC1_DMA_STREAM DMA2_Stream0
|
||||
#define ADC_CHANNEL_1_PIN PC0
|
||||
#define ADC_CHANNEL_2_PIN PC2
|
||||
#define ADC_CHANNEL_3_PIN PC1
|
||||
|
||||
#define VBAT_ADC_CHANNEL ADC_CHN_1
|
||||
#define RSSI_ADC_CHANNEL ADC_CHN_2
|
||||
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3
|
||||
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
|
||||
|
||||
|
||||
// ************PINIO to other
|
||||
#define PINIO1_PIN PC8 //Enable vsw
|
||||
#define PINIO2_PIN PC9
|
||||
|
||||
|
||||
|
||||
// *************** LEDSTRIP ************************
|
||||
#define USE_LED_STRIP
|
||||
#define WS2811_PIN PA8
|
||||
|
||||
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD (BIT(2))
|
||||
|
||||
#define MAX_PWM_OUTPUT_PORTS 6
|
||||
|
||||
#define USE_DSHOT
|
||||
#define USE_ESC_SENSOR
|
||||
|
|
@ -40,6 +40,10 @@
|
|||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
#define MPU6000_CS_PIN PA4
|
||||
|
||||
#define USE_IMU_MPU6500
|
||||
#define IMU_MPU6500_ALIGN CW180_DEG
|
||||
#define MPU6500_SPI_BUS BUS_SPI1
|
||||
#define MPU6500_CS_PIN PA4
|
||||
|
||||
// *************** Baro **************************
|
||||
#define USE_I2C
|
||||
|
|
|
|||
|
|
@ -1,3 +1,2 @@
|
|||
target_stm32f405xg(MATEKF405)
|
||||
target_stm32f405xg(MATEKF405_SERVOS6)
|
||||
target_stm32f405xg(MATEKF405OSD)
|
||||
|
|
|
|||
|
|
@ -24,23 +24,15 @@
|
|||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
|
||||
|
||||
#ifdef MATEKF405_SERVOS6
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
|
||||
#else
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1
|
||||
#endif
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1)
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1)
|
||||
|
||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED , 0, 0), // S5 UP(1,7)
|
||||
|
||||
#ifdef MATEKF405_SERVOS6
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5)
|
||||
#else
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5)
|
||||
#endif
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6)
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5)
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6)
|
||||
|
||||
DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2
|
||||
|
||||
|
|
|
|||
|
|
@ -183,5 +183,6 @@
|
|||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
#define USE_DSHOT
|
||||
#define USE_DSHOT_DMAR
|
||||
#define USE_ESC_SENSOR
|
||||
|
||||
|
|
|
|||
|
|
@ -1,2 +1 @@
|
|||
target_stm32f722xe(MATEKF722)
|
||||
target_stm32f722xe(MATEKF722_HEXSERVO)
|
||||
|
|
|
|||
18
src/main/target/MATEKF722/target.c
Executable file → Normal file
18
src/main/target/MATEKF722/target.c
Executable file → Normal file
|
|
@ -27,18 +27,14 @@
|
|||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM
|
||||
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1, 4, 5)
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7)
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5)
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7)
|
||||
DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2, 4, 7)
|
||||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(2, 7, 7)
|
||||
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 DMA1_ST2
|
||||
#ifdef MATEKF722_HEXSERVO
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6
|
||||
#else
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6
|
||||
#endif
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_ST2
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA2_ST6
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DMA1_ST7
|
||||
|
||||
// DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2/S8 DMA1_ST0
|
||||
|
||||
|
|
|
|||
0
src/main/target/MATEKF722/target.h
Executable file → Normal file
0
src/main/target/MATEKF722/target.h
Executable file → Normal file
|
|
@ -32,21 +32,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6
|
|||
BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2
|
||||
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4
|
||||
DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4
|
||||
DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5
|
||||
DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6
|
||||
|
||||
DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7
|
||||
DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8
|
||||
DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9
|
||||
DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE
|
||||
DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7
|
||||
DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8
|
||||
DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9
|
||||
DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE
|
||||
|
||||
DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11
|
||||
DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE
|
||||
DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
|
||||
DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE
|
||||
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812
|
||||
DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM
|
||||
|
|
|
|||
|
|
@ -27,8 +27,8 @@ timerHardware_t timerHardware[] = {
|
|||
DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S5
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S6
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5
|
||||
DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7
|
||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8
|
||||
|
||||
|
|
|
|||
|
|
@ -51,6 +51,7 @@
|
|||
#include "drivers/timer.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "config/config_streamer.h"
|
||||
#include "build/version.h"
|
||||
|
||||
#include "target/SITL/sim/realFlight.h"
|
||||
#include "target/SITL/sim/xplane.h"
|
||||
|
|
@ -73,9 +74,12 @@ static int simPort = 0;
|
|||
|
||||
static char **c_argv;
|
||||
|
||||
void systemInit(void) {
|
||||
static void printVersion(void) {
|
||||
fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision);
|
||||
}
|
||||
|
||||
fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
|
||||
void systemInit(void) {
|
||||
printVersion();
|
||||
clock_gettime(CLOCK_MONOTONIC, &start_time);
|
||||
fprintf(stderr, "[SYSTEM] Init...\n");
|
||||
|
||||
|
|
@ -168,6 +172,7 @@ bool parseMapping(char* mapStr)
|
|||
|
||||
void printCmdLineOptions(void)
|
||||
{
|
||||
printVersion();
|
||||
fprintf(stderr, "Avaiable options:\n");
|
||||
fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
|
||||
fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
|
||||
|
|
@ -197,6 +202,7 @@ void parseArguments(int argc, char *argv[])
|
|||
{"simport", required_argument, 0, 'p'},
|
||||
{"help", no_argument, 0, 'h'},
|
||||
{"path", required_argument, 0, 'e'},
|
||||
{"version", no_argument, 0, 'v'},
|
||||
{NULL, 0, NULL, 0}
|
||||
};
|
||||
|
||||
|
|
@ -236,10 +242,12 @@ void parseArguments(int argc, char *argv[])
|
|||
fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
|
||||
}
|
||||
break;
|
||||
case 'h':
|
||||
case 'v':
|
||||
printVersion();
|
||||
exit(0);
|
||||
default:
|
||||
printCmdLineOptions();
|
||||
exit(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
2
src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
Normal file
2
src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,2 @@
|
|||
target_stm32f405xg(SPEEDYBEEF405MINI)
|
||||
target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS)
|
||||
37
src/main/target/SPEEDYBEEF405MINI/config.c
Normal file
37
src/main/target/SPEEDYBEEF405MINI/config.c
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "fc/fc_msp_box.h"
|
||||
|
||||
#include "io/piniobox.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL;
|
||||
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL;
|
||||
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
|
||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
|
||||
|
||||
pinioBoxConfigMutable()->permanentId[0] = BOXARM;
|
||||
}
|
||||
46
src/main/target/SPEEDYBEEF405MINI/target.c
Normal file
46
src/main/target/SPEEDYBEEF405MINI/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/bus.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/pwm_mapping.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/pinio.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S1
|
||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S2
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S3
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4
|
||||
|
||||
#ifdef SPEEDYBEEF405MINI_6OUTPUTS
|
||||
DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 1, 0), // CAM_CTRL
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // LED
|
||||
#else
|
||||
DEF_TIM(TIM12, CH1, PB14, TIM_USE_ANY, 1, 0), // CAM_CTRL
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED
|
||||
#endif
|
||||
|
||||
DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx
|
||||
};
|
||||
|
||||
const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
|
||||
186
src/main/target/SPEEDYBEEF405MINI/target.h
Normal file
186
src/main/target/SPEEDYBEEF405MINI/target.h
Normal file
|
|
@ -0,0 +1,186 @@
|
|||
/*
|
||||
* 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 "SF4M"
|
||||
#define USBD_PRODUCT_STRING "SPEEDYBEEF405MINI"
|
||||
|
||||
#define LED0 PC13
|
||||
|
||||
#define BEEPER PC15
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
/*
|
||||
* SPI Buses
|
||||
*/
|
||||
#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 PC2
|
||||
#define SPI2_MOSI_PIN PC3
|
||||
|
||||
#define USE_SPI_DEVICE_3
|
||||
#define SPI3_SCK_PIN PB3
|
||||
#define SPI3_MISO_PIN PB4
|
||||
#define SPI3_MOSI_PIN PB5
|
||||
|
||||
/*
|
||||
* I2C
|
||||
*/
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_1
|
||||
#define I2C1_SCL PB8
|
||||
#define I2C1_SDA PB9
|
||||
|
||||
/*
|
||||
* Serial
|
||||
*/
|
||||
#define USE_VCP
|
||||
|
||||
#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 PC10
|
||||
#define UART3_RX_PIN PC11
|
||||
|
||||
#define USE_UART4 //Internally routed to BLE
|
||||
#define UART4_TX_PIN PA0
|
||||
#define UART4_RX_PIN PA1
|
||||
|
||||
#define USE_UART5
|
||||
#define UART5_TX_PIN PC12
|
||||
#define UART5_RX_PIN PD2
|
||||
|
||||
#define USE_UART6
|
||||
#define UART6_TX_PIN PC6
|
||||
#define UART6_RX_PIN PC7
|
||||
|
||||
#define USE_SOFTSERIAL1
|
||||
#define SOFTSERIAL_1_TX_PIN PA2
|
||||
#define SOFTSERIAL_1_RX_PIN PA2
|
||||
|
||||
#define SERIAL_PORT_COUNT 8
|
||||
|
||||
/*
|
||||
* Gyro
|
||||
*/
|
||||
#define USE_IMU_ICM42605
|
||||
#define IMU_ICM42605_ALIGN CW270_DEG
|
||||
#define ICM42605_CS_PIN PA4
|
||||
#define ICM42605_SPI_BUS BUS_SPI1
|
||||
|
||||
/*
|
||||
* Other
|
||||
*/
|
||||
#define USE_BARO
|
||||
#define BARO_I2C_BUS BUS_I2C1
|
||||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_MS5611
|
||||
#define USE_BARO_DPS310
|
||||
|
||||
#define USE_MAG
|
||||
#define MAG_I2C_BUS BUS_I2C1
|
||||
#define USE_MAG_AK8975
|
||||
#define USE_MAG_HMC5883
|
||||
#define USE_MAG_QMC5883
|
||||
#define USE_MAG_IST8310
|
||||
#define USE_MAG_IST8308
|
||||
#define USE_MAG_MAG3110
|
||||
#define USE_MAG_LIS3MDL
|
||||
|
||||
#define TEMPERATURE_I2C_BUS BUS_I2C1
|
||||
#define PITOT_I2C_BUS BUS_I2C1
|
||||
#define RANGEFINDER_I2C_BUS BUS_I2C1
|
||||
|
||||
/*
|
||||
* OSD
|
||||
*/
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_CS_PIN PB12
|
||||
#define MAX7456_SPI_BUS BUS_SPI2
|
||||
|
||||
/*
|
||||
* Blackbox
|
||||
*/
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
#define M25P16_SPI_BUS BUS_SPI3
|
||||
#define M25P16_CS_PIN PC14
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
|
||||
|
||||
#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||
|
||||
// *************** ADC *****************************
|
||||
#define USE_ADC
|
||||
#define ADC_INSTANCE ADC1
|
||||
#define ADC1_DMA_STREAM DMA2_Stream0
|
||||
|
||||
#define ADC_CHANNEL_1_PIN PC0
|
||||
#define ADC_CHANNEL_2_PIN PC1
|
||||
|
||||
#define VBAT_ADC_CHANNEL ADC_CHN_1
|
||||
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
|
||||
|
||||
// *************** PINIO ***************************
|
||||
#define USE_PINIO
|
||||
#define USE_PINIOBOX
|
||||
#define PINIO1_PIN PB11 // RF Switch
|
||||
#define PINIO1_FLAGS PINIO_FLAGS_INVERTED
|
||||
|
||||
// *************** LEDSTRIP ************************
|
||||
#define USE_LED_STRIP
|
||||
#define WS2811_PIN PA8
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
|
||||
#define CURRENT_METER_SCALE 250
|
||||
#define CURRENT_METER_OFFSET 0
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD 0xffff
|
||||
|
||||
#ifdef SPEEDYBEEF405MINI_6OUTPUTS
|
||||
|
||||
#define MAX_PWM_OUTPUT_PORTS 6
|
||||
|
||||
#else
|
||||
|
||||
#define MAX_PWM_OUTPUT_PORTS 4
|
||||
|
||||
#endif
|
||||
|
||||
#define USE_DSHOT
|
||||
#define USE_SERIALSHOT
|
||||
#define USE_ESC_SENSOR
|
||||
|
|
@ -152,7 +152,8 @@
|
|||
#define RANGEFINDER_I2C_BUS BUS_I2C2
|
||||
|
||||
#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
|
||||
// #define USE_DSHOT
|
||||
#define USE_DSHOT
|
||||
#define USE_DSHOT_DMAR
|
||||
// #define USE_ESC_SENSOR
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1,2 @@
|
|||
target_stm32f722xe(SPEEDYBEEF7MINI)
|
||||
target_stm32f722xe(SPEEDYBEEF7MINIV2)
|
||||
|
|
|
|||
|
|
@ -26,7 +26,11 @@
|
|||
#include "drivers/pinio.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#ifdef SPEEDYBEEF7MINIV2
|
||||
BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN);
|
||||
#else
|
||||
BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN);
|
||||
#endif
|
||||
|
||||
timerHardware_t timerHardware[] = {
|
||||
DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP1-2 D(1, 4, 5)
|
||||
|
|
|
|||
|
|
@ -19,7 +19,12 @@
|
|||
#pragma once
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "SBMN"
|
||||
|
||||
#ifdef SPEEDYBEEF7MINIV2
|
||||
#define USBD_PRODUCT_STRING "SPEEDYBEEF7MINIV2"
|
||||
#else
|
||||
#define USBD_PRODUCT_STRING "SPEEDYBEEF7MINI"
|
||||
#endif
|
||||
|
||||
#define LED0 PA14 //Blue SWCLK
|
||||
|
||||
|
|
@ -35,11 +40,22 @@
|
|||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#ifdef SPEEDYBEEF7MINIV2
|
||||
|
||||
#define USE_IMU_BMI270
|
||||
#define IMU_BMI270_ALIGN CW180_DEG
|
||||
#define BMI270_CS_PIN PB2
|
||||
#define BMI270_SPI_BUS BUS_SPI1
|
||||
|
||||
#else
|
||||
|
||||
#define USE_IMU_MPU6000
|
||||
#define IMU_MPU6000_ALIGN CW0_DEG
|
||||
#define MPU6000_CS_PIN PB2
|
||||
#define MPU6000_SPI_BUS BUS_SPI1
|
||||
|
||||
#endif
|
||||
|
||||
// *************** I2C /Baro/Mag *********************
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_1
|
||||
|
|
|
|||
2
src/main/target/common.h
Normal file → Executable file
2
src/main/target/common.h
Normal file → Executable file
|
|
@ -92,6 +92,7 @@
|
|||
#define USE_PITOT
|
||||
#define USE_PITOT_MS4525
|
||||
#define USE_PITOT_MSP
|
||||
#define USE_PITOT_DLVR
|
||||
|
||||
#define USE_1WIRE
|
||||
#define USE_1WIRE_DS2482
|
||||
|
|
@ -161,6 +162,7 @@
|
|||
#define NAV_MAX_WAYPOINTS 120
|
||||
#define USE_RCDEVICE
|
||||
#define USE_MULTI_MISSION
|
||||
#define USE_MULTI_FUNCTIONS // defines functions only, warnings always defined
|
||||
|
||||
//Enable VTX control
|
||||
#define USE_VTX_CONTROL
|
||||
|
|
|
|||
|
|
@ -357,10 +357,18 @@
|
|||
#endif
|
||||
|
||||
#if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS)
|
||||
BUSDEV_REGISTER_I2C(busdev_ms5425, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough
|
||||
BUSDEV_REGISTER_I2C(busdev_ms4525, DEVHW_MS4525, MS4525_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS)
|
||||
#define DLVR_I2C_BUS PITOT_I2C_BUS
|
||||
#endif
|
||||
|
||||
#if defined(USE_PITOT_DLVR) && defined(DLVR_I2C_BUS)
|
||||
BUSDEV_REGISTER_I2C(busdev_dlvr, DEVHW_DLVR, DLVR_I2C_BUS, 0x28, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires 0xFF to passthrough
|
||||
#endif
|
||||
|
||||
/** OTHER HARDWARE **/
|
||||
|
||||
#if defined(USE_MAX7456)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue