diff --git a/docs/Cli.md b/docs/Cli.md index c805fe9250b..b0cf7d20bfc 100644 --- a/docs/Cli.md +++ b/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. diff --git a/docs/ESC and servo outputs.md b/docs/ESC and servo outputs.md index 78a5056590a..a9821603c06 100644 --- a/docs/ESC and servo outputs.md +++ b/docs/ESC and servo outputs.md @@ -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 \ No newline at end of file +* `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. diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c2ef089afca..a5b9a3fdbcd 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -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. | @@ -158,7 +158,7 @@ The flight mode operands return `true` when the mode is active. These are modes | 7 | HORIZON | `true` when you are in the **Horizon** flight mode. | | 8 | AIR | `true` when you the **Airmode** flight mode modifier is active. | | 9 | USER1 | `true` when the **USER 1** mode is active. | -| 10 | USER2 | `true` when the **USER 21** mode is active. | +| 10 | USER2 | `true` when the **USER 2** mode is active. | | 11 | COURSE_HOLD | `true` when you are in the **Course Hold** flight mode. | | 12 | USER3 | `true` when the **USER 3** mode is active. | | 13 | USER4 | `true` when the **USER 4** mode is active. | diff --git a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index cf5cc08c061..a2c5894e72f 100644 --- a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -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 diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b14..ac7c04e5dbe 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -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 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 4fc5a00b83e..a3594af2de2 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -212,27 +212,128 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) static void timerHardwareOverride(timerHardware_t * timer) { if (mixerConfig()->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_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; } - if (timer->usageFlags & TIM_USE_FW_MOTOR) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_MOTOR; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO; - } - } else if (mixerConfig()->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); + } } - if (timer->usageFlags & TIM_USE_FW_SERVO) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_SERVO; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR; + } 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); + } } } } @@ -245,8 +346,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 +368,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 { + // 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; + } + // Fixed wing or HELI (one/two motors and a lot of servos - if (timHw->usageFlags & TIM_USE_FW_SERVO) { + if (timHw->usageFlags & TIM_USE_FW_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_SERVO_OUTPUT; - } - else if (timHw->usageFlags & TIM_USE_FW_MOTOR) { + } 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; diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 664bae3ca59..c03db7e57b9 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -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; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index b4b3ed8da03..b822114147f 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -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; @@ -115,6 +119,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, @@ -248,7 +253,9 @@ 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 \ No newline at end of file +#endif diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e402ecf8875..3ac8766ebdc 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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; @@ -3652,6 +3735,9 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("resources"); //printResource(dumpMask, &defaultConfig); + cliPrintHashLine("Timer overrides"); + printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1); + cliPrintHashLine("Mixer: motor mixer"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); @@ -3968,6 +4054,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_OSD CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif + CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), }; static void cliHelp(char *cmdline) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ca1e617ebad..a46e5b39fce 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1516,6 +1516,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); @@ -3662,7 +3674,43 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; break; -#endif // simulator +#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 diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 93638ec1ecb..b8eefadd15a 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -232,8 +232,8 @@ void initActiveBoxIds(void) if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) { ADD_ACTIVE_BOX(BOXNAVRTH); ADD_ACTIVE_BOX(BOXNAVWP); - ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); // CR101 - ADD_ACTIVE_BOX(BOXNAVCRUISE); // CR101 + ADD_ACTIVE_BOX(BOXNAVCRUISE); + ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXHOMERESET); ADD_ACTIVE_BOX(BOXGCSNAV); ADD_ACTIVE_BOX(BOXPLANWPMISSION); @@ -243,8 +243,6 @@ void initActiveBoxIds(void) } if (STATE(AIRPLANE)) { - // ADD_ACTIVE_BOX(BOXNAVCRUISE); // CR101 - // ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXSOARING); } } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 105dcedb5cb..79cee0245e7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2592,14 +2592,12 @@ groups: default_value: ON field: general.flags.mission_planner_reset type: bool - # CR101 - 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 - # CR101 - 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 @@ -2842,12 +2840,6 @@ groups: field: fw.launch_abort_deadband min: 2 max: 250 - # - name: nav_fw_cruise_yaw_rate CR101 - # 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 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 712db3f74c9..fb6da259808 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -146,7 +146,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { */ void failsafeReset(void) { - if (failsafeState.active) { // can't reset when still active CR109 + if (failsafeState.active) { // can't reset if still active return; } @@ -161,7 +161,7 @@ void failsafeReset(void) failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure; - failsafeState.controlling = false; // CR98 + failsafeState.controlling = false; failsafeState.lastGoodRcCommand[ROLL] = 0; failsafeState.lastGoodRcCommand[PITCH] = 0; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 03796856207..5338ce47924 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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" @@ -103,8 +104,17 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +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) { @@ -208,7 +218,7 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { - getThrottleIdleValue(); // CR106 + getThrottleIdleValue(); if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -216,7 +226,7 @@ void mixerResetDisarmedMotors(void) throttleRangeMax = motorConfig()->maxthrottle; } else { motorZeroCommand = motorConfig()->mincommand; - throttleRangeMin = throttleIdleValue; // CR106 + throttleRangeMin = throttleIdleValue; throttleRangeMax = motorConfig()->maxthrottle; } @@ -225,7 +235,7 @@ void mixerResetDisarmedMotors(void) if (feature(FEATURE_MOTOR_STOP)) { motorValueWhenStopped = motorZeroCommand; } else { - motorValueWhenStopped = throttleIdleValue; // CR106 + motorValueWhenStopped = throttleIdleValue; } // set disarmed motor values @@ -471,7 +481,6 @@ void FAST_CODE mixTable(void) return; } #endif - // CR106 - set motors to off and return if disarmed ... no point going any further DEBUG_SET(DEBUG_ALWAYS, 4, rcCommand[THROTTLE]); #ifdef USE_DEV_TOOLS bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode; @@ -486,7 +495,7 @@ void FAST_CODE mixTable(void) mixerThrottleCommand = motor[0]; return; } - // CR106 + int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) { @@ -568,11 +577,11 @@ void FAST_CODE mixTable(void) throttleRangeMax = motorConfig()->maxthrottle; // Throttle scaling to limit max throttle when battery is full - #ifdef USE_PROGRAMMING_FRAMEWORK +#ifdef USE_PROGRAMMING_FRAMEWORK mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin; - #else +#else mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin; - #endif +#endif // Throttle compensation based on battery voltage if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax); @@ -614,12 +623,10 @@ void FAST_CODE mixTable(void) int16_t getThrottlePercent(bool useScaled) // CR106 should use actual throttle sent to motors not throttle stick position { - int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); // CR106 rcCommand[THROTTLE] - - // const int idleThrottle = getThrottleIdleValue(); // CR106 + int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); if (useScaled) { - thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); // CR106 + thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); } else { thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1380ebec983..f7f36f7706a 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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 @@ -64,6 +66,12 @@ typedef struct motorMixer_s { PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); +typedef struct timerOverride_s { + uint8_t outputMode; +} timerOverride_t; + +PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides); + typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; @@ -131,4 +139,4 @@ void stopMotors(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); +bool areMotorsRunning(void); \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a188abdd99d..9369685fa89 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -898,16 +898,17 @@ static uint8_t getHeadingHoldState(void) float pidHeadingHold(float dT) { float headingHoldRate; - // CR101 + + /* Convert absolute error into relative to current heading */ int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; - DEBUG_SET(DEBUG_ALWAYS, 4, error); + /* Convert absolute error into relative to current heading */ if (error > 180) { error -= 360; } else if (error < -180) { error += 360; } -// CR101 + /* New MAG_HOLD controller work slightly different that previous one. Old one mapped error to rotation speed in following way: diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index bf057e830cd..ab5b72f3885 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -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); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 58f91450885..5d5e040573d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -993,10 +993,10 @@ 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 && posControl.safehomeState.isApplied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); - } - return NULL; + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; } #endif @@ -1118,7 +1118,7 @@ bool osdUsingScaledThrottle(void) **/ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) { - buff[0] = SYM_BLANK; // CR106 + buff[0] = SYM_BLANK; buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; @@ -1133,7 +1133,6 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } #endif - // CR106 int8_t throttlePercent = getThrottlePercent(useScaled); if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) { const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM"; @@ -1142,7 +1141,6 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes return; } tfp_sprintf(buff + 2, "%3d", throttlePercent); - // CR106 } /** @@ -4481,15 +4479,14 @@ static void osdShowArmed(void) 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, 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 - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + } else { + char buf2[12]; // format the distance first + 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 + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } #endif } else { @@ -4944,7 +4941,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } - // CR101 if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { char buf[6]; @@ -4955,7 +4951,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } messages[messageCount++] = messageBuf; } - // CR101 if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 344b84f22f5..f746f5c8ff3 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fec9a514ce2..b4431f38e35 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -96,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 = { @@ -153,7 +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 CR101 + .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps }, // MC-specific @@ -210,7 +210,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 // CR101 .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, @@ -428,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_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW, // CR101 + .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, @@ -486,7 +485,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_STATE_CRUISE_IN_PROGRESS] = { .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, - .timeoutMs = 10, // CR101 // CR101 + .timeoutMs = 10, .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, @@ -962,7 +961,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) return navFSM[state].stateFlags; } -flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) // CR101 +flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) { return navFSM[state].mapToFlightModes; } @@ -1067,11 +1066,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { UNUSED(previousState); - // CR101 - if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hole not possible on MR without yaw control + + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control return NAV_FSM_EVENT_ERROR; } - // if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now // CR101 DEBUG_SET(DEBUG_CRUISE, 0, 1); // Switch to IDLE if we do not have an healthy position. Try the next iteration. @@ -1080,7 +1078,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( } resetPositionController(); - // CR101 + if (STATE(AIRPLANE)) { posControl.cruise.course = posControl.actualState.cog; // Store the course to follow } else { // Multicopter @@ -1088,7 +1086,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); posControl.desiredState.pos = posControl.actualState.abs.pos; } - // CR101 posControl.cruise.previousCourse = posControl.cruise.course; posControl.cruise.lastCourseAdjustmentTime = 0; @@ -1106,13 +1103,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS return NAV_FSM_EVENT_SWITCH_TO_IDLE; } - if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // CR101 inhibit for MR, pitch adjusts only speed + DEBUG_SET(DEBUG_CRUISE, 0, 2); + DEBUG_SET(DEBUG_CRUISE, 2, 0); + + if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } - // CR101 + const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband; - // User demanding yaw -> yaw stick on FW, yaw and roll sticks on MR + // 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); @@ -1123,7 +1123,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS 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)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate); // CR101 + 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); posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; @@ -1154,10 +1154,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { - if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hole not possible on MR without yaw control + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control return NAV_FSM_EVENT_ERROR; } - // if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now // CR101 navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); @@ -1815,8 +1814,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS { UNUSED(previousState); - // disarm(DISARM_NAVIGATION); CR109 - return NAV_FSM_EVENT_NONE; } @@ -2875,7 +2872,7 @@ void updateLandingStatus(timeMs_t currentTimeMs) } } else if (STATE(LANDING_DETECTED)) { pidResetErrorAccumulators(); - if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) { // CR109 + if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); disarm(DISARM_LANDING); } else if (!navigationInAutomaticThrottleMode()) { @@ -3492,19 +3489,18 @@ bool isNavHoldPositionActive(void) FLIGHT_MODE(NAV_POSHOLD_MODE) || navigationIsExecutingAnEmergencyLanding(); } -// CR101 + float getActiveSpeed(void) { - /* Only applicable for multicopter */ + /* Currently only applicable for multicopter */ - // In manual control mode and during multicoper cruise use different cap for speed + // Speed limit for modes where speed manually controlled if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return navConfig()->general.max_manual_speed; } uint16_t waypointSpeed = navConfig()->general.auto_speed; - // get required waypoint specific speed for each leg during waypoint mode, don't allow to move slower than 0.5 m/s if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { float wpSpecificSpeed = 0.0f; @@ -3523,7 +3519,7 @@ float getActiveSpeed(void) return waypointSpeed; } -// CR101 + bool isWaypointNavTrackingActive(void) { // NAV_WP_MODE flag used rather than state flag NAV_AUTO_WP to ensure heading to initial waypoint @@ -3540,7 +3536,7 @@ static void processNavigationRCAdjustments(void) { /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); - // CR101 + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); @@ -3552,10 +3548,9 @@ static void processNavigationRCAdjustments(void) return; } - posControl.flags.isAdjustingAltitude = navStateFlags & NAV_RC_ALT && adjustAltitudeFromRCInput(); - posControl.flags.isAdjustingPosition = navStateFlags & NAV_RC_POS && adjustPositionFromRCInput(); - posControl.flags.isAdjustingHeading = navStateFlags & NAV_RC_YAW && adjustHeadingFromRCInput(); - // CR101 + posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput(); + posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput(); + posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput(); } /*----------------------------------------------------------- @@ -3925,8 +3920,9 @@ 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 && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // CR101 + if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return NAV_HEADING_CONTROL_MANUAL; } @@ -4479,9 +4475,8 @@ int32_t getCruiseHeadingAdjustment(void) { return posControl.cruise.course; // return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse); } -// CR101 + int32_t navigationGetHeadingError(void) { return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); } -// CR101 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index c99e793101e..ff68976970a 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -267,7 +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 CR101 + uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled } general; struct { @@ -319,7 +319,6 @@ typedef struct navConfig_s { // xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 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 CR101 bool allow_manual_thr_increase; bool useFwNavYawControl; uint8_t yawControlDeadband; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 876ede1f71e..57db24d83c4 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -71,7 +71,6 @@ static bool isRollAdjustmentValid = false; static bool isYawAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; -// static int32_t navHeadingError; // CR101 static float navCrossTrackError; static int8_t loiterDirYaw = 1; bool needToCalculateCircularLoiter; @@ -492,7 +491,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta * Calculate NAV heading error * Units are centidegrees */ - int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog); // CR101 + 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 @@ -901,12 +900,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } } -// int32_t navigationGetHeadingError(void) -// { - // return navHeadingError; - // // wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); // CR101 move to navigation.c -// } - float navigationGetCrossTrackError(void) { return navCrossTrackError; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 9ce3bf3c678..c0d1e5241d7 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -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 = getActiveSpeed(); // CR101 + const float maxSpeed = getActiveSpeed(); nav_speed_up = maxSpeed; nav_accel_z = maxSpeed; nav_speed_down = navConfig()->general.max_auto_climb_rate; @@ -285,7 +285,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) bool adjustMulticopterHeadingFromRCInput(void) { if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) { - // CR101 // 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; @@ -295,7 +294,6 @@ bool adjustMulticopterHeadingFromRCInput(void) } return false; - // CR101 } /*----------------------------------------------------------- @@ -404,7 +402,7 @@ void resetMulticopterPositionController(void) lastAccelTargetY = 0.0f; } } -// CR101 + static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment) { static timeMs_t lastUpdateTimeMs; @@ -414,8 +412,6 @@ static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment) const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband); - DEBUG_SET(DEBUG_ALWAYS, 0, rcVelX); - DEBUG_SET(DEBUG_ALWAYS, 1, updateDeltaTimeMs); if (rcVelX > posControl.cruise.multicopterSpeed) { posControl.cruise.multicopterSpeed = rcVelX; } else if (rcVelX < 0 && updateDeltaTimeMs < 100) { @@ -434,10 +430,9 @@ static void setMulticopterStopPosition(void) calculateMulticopterInitialHoldPosition(&stopPosition); setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); } -// CR101 + bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { - // CR101 if (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) { if (rcPitchAdjustment) { return adjustMulticopterCruiseSpeed(rcPitchAdjustment); @@ -445,7 +440,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR return false; } - // CR101 + // Process braking mode processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment); @@ -466,14 +461,13 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR } return true; - }// CR101 + } else if (posControl.flags.isAdjustingPosition) { // Adjusting finished - reset desired position to stay exactly where pilot released the stick setMulticopterStopPosition(); } return false; - // CR101 } static float getVelocityHeadingAttenuationFactor(void) @@ -502,8 +496,8 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) static void updatePositionVelocityController_MC(const float maxSpeed) { - // CR101 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)); @@ -514,12 +508,12 @@ static void updatePositionVelocityController_MC(const float maxSpeed) setMulticopterStopPosition(); } } - // CR101 + const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; // Calculate target velocity - float neuVelX = posErrorX * posControl.pids.pos[X].param.kP; // CR101 new to neu + float neuVelX = posErrorX * posControl.pids.pos[X].param.kP; float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP; // Scale velocity to respect max_speed @@ -550,9 +544,6 @@ static void updatePositionVelocityController_MC(const float maxSpeed) const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed); posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor; posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor; - - // navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); // CR101 move - // navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); } static float computeNormalizedVelocity(const float value, const float maxValue) @@ -724,9 +715,11 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // DEBUG_SET(DEBUG_ALWAYS, 4, accelForward * 1000); // DEBUG_SET(DEBUG_ALWAYS, 5, posControl.rcAdjustment[PITCH]); } -// CR101 + static void applyMulticopterPositionController(timeUs_t currentTimeUs) { + // 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) { /* No position data, disable automatic adjustment, rcCommand passthrough */ posControl.rcAdjustment[PITCH] = 0; @@ -734,12 +727,12 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) return; } -DEBUG_SET(DEBUG_ALWAYS, 2, posControl.cruise.multicopterSpeed); + + // 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 we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { // Indicate that information is no longer usable posControl.flags.horizontalPositionDataConsumed = true; @@ -748,12 +741,11 @@ DEBUG_SET(DEBUG_ALWAYS, 2, posControl.cruise.multicopterSpeed); const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // We should passthrough rcCommand if adjusting position in GPS_ATTI mode if (bypassPositionController) { return; } - // Update position controller + // 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(); @@ -774,7 +766,7 @@ DEBUG_SET(DEBUG_ALWAYS, 2, posControl.cruise.multicopterSpeed); rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); } -// CR101 + bool isMulticopterFlying(void) { bool throttleCondition = rcCommand[THROTTLE] > currentBatteryProfile->nav.mc.hover_throttle; @@ -1000,11 +992,10 @@ void resetMulticopterHeadingController(void) static void applyMulticopterHeadingController(void) { - // CR101 - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // heading set by Nav during Course Hold so disable yaw stick input rcCommand[YAW] = 0; } - // CR101 + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw)); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f1b7770f379..05c7430c6d6 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -329,7 +329,7 @@ typedef struct { int32_t course; int32_t previousCourse; timeMs_t lastCourseAdjustmentTime; - float multicopterSpeed; // CR101 + float multicopterSpeed; } navCruise_t; typedef struct { @@ -456,7 +456,7 @@ bool isFixedWingFlying(void); bool isMulticopterFlying(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void); -flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state); // CR101 +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); @@ -466,7 +466,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); -float getActiveSpeed(void); // CR101 +float getActiveSpeed(void); bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); diff --git a/src/main/target/BETAFPVF435/CMakeLists.txt b/src/main/target/BETAFPVF435/CMakeLists.txt new file mode 100644 index 00000000000..cfac04ed933 --- /dev/null +++ b/src/main/target/BETAFPVF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(BETAFPVF435 SKIP_RELEASES) diff --git a/src/main/target/BETAFPVF435/config.c b/src/main/target/BETAFPVF435/config.c new file mode 100644 index 00000000000..895714f46bb --- /dev/null +++ b/src/main/target/BETAFPVF435/config.c @@ -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 . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#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); +} + diff --git a/src/main/target/BETAFPVF435/target.c b/src/main/target/BETAFPVF435/target.c new file mode 100644 index 00000000000..815781e0c62 --- /dev/null +++ b/src/main/target/BETAFPVF435/target.c @@ -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 . + */ + +#include + +#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]); \ No newline at end of file diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h new file mode 100644 index 00000000000..c8e17bd7c48 --- /dev/null +++ b/src/main/target/BETAFPVF435/target.h @@ -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 . + */ +//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 \ No newline at end of file diff --git a/src/main/target/MATEKF405/CMakeLists.txt b/src/main/target/MATEKF405/CMakeLists.txt index c4f839c1c1d..ebf457cda25 100644 --- a/src/main/target/MATEKF405/CMakeLists.txt +++ b/src/main/target/MATEKF405/CMakeLists.txt @@ -1,3 +1,2 @@ target_stm32f405xg(MATEKF405) -target_stm32f405xg(MATEKF405_SERVOS6) target_stm32f405xg(MATEKF405OSD) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 40ebf626e5f..047e0f37751 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -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 diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index 93ab3345def..acaed9daf23 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -178,5 +178,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_DSHOT_DMAR #define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF722/CMakeLists.txt b/src/main/target/MATEKF722/CMakeLists.txt index eb384e12815..600a4401a20 100644 --- a/src/main/target/MATEKF722/CMakeLists.txt +++ b/src/main/target/MATEKF722/CMakeLists.txt @@ -1,2 +1 @@ target_stm32f722xe(MATEKF722) -target_stm32f722xe(MATEKF722_HEXSERVO) diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c old mode 100755 new mode 100644 index dc06c7f508d..d2575f42f2d --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -25,24 +25,20 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + 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 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h old mode 100755 new mode 100644 diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 706d5787865..1923e6623a1 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -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 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 4bf53a791a5..0a686f60619 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -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; } } diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt new file mode 100644 index 00000000000..e18c0cd5ada --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f405xg(SPEEDYBEEF405MINI) +target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS) diff --git a/src/main/target/SPEEDYBEEF405MINI/config.c b/src/main/target/SPEEDYBEEF405MINI/config.c new file mode 100644 index 00000000000..46aa7f00dcb --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/config.c @@ -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 . + */ + +#include + +#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; +} diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c new file mode 100644 index 00000000000..1118940e6f6 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.c @@ -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 . + */ + +#include + +#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]); diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h new file mode 100644 index 00000000000..dd5b9f4cfb3 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -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 . + */ + + +#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 \ No newline at end of file diff --git a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt index c7a92612742..1ae9febd177 100644 --- a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt +++ b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(SPEEDYBEEF7MINI) +target_stm32f722xe(SPEEDYBEEF7MINIV2) diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c index 468ac3c2f62..7cc2506b66c 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.c +++ b/src/main/target/SPEEDYBEEF7MINI/target.c @@ -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) diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h index f6db8e86fa4..0dd3a6e7a83 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.h +++ b/src/main/target/SPEEDYBEEF7MINI/target.h @@ -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