Browse Source

Merge remote-tracking branch 'origin/master' into dzkuvx-pid-target-overrides

master
Pawel Spychalski (DzikuVx) 3 years ago
parent
commit
9a192cab7b
  1. 219
      docs/Programming Framework.md
  2. 2
      src/main/io/osd_hud.c
  3. 1
      src/main/target/MAMBAF405_2022A/CMakeLists.txt
  4. 59
      src/main/target/MAMBAF405_2022A/config.c
  5. 39
      src/main/target/MAMBAF405_2022A/target.c
  6. 197
      src/main/target/MAMBAF405_2022A/target.h
  7. 6
      src/main/target/MAMBAF722/target.h
  8. 1
      src/main/target/MAMBAF722_2022A/CMakeLists.txt
  9. 65
      src/main/target/MAMBAF722_2022A/config.c
  10. 43
      src/main/target/MAMBAF722_2022A/target.c
  11. 190
      src/main/target/MAMBAF722_2022A/target.h

219
docs/Programming Framework.md

@ -31,30 +31,30 @@ IPF can be edited using INAV Configurator user interface, of via CLI
### Operations
| Operation ID | Name | Notes |
|---- |---- |---- |
| 0 | TRUE | Always evaluates as true |
| 1 | EQUAL | Evaluates `false` if `false` or `0` |
| 2 | GREATER_THAN | |
| 3 | LOWER_THAN | |
| 4 | LOW | `true` if `<1333` |
| 5 | MID | `true` if `>=1333 and <=1666` |
| 6 | HIGH | `true` if `>1666` |
| 7 | AND | |
| 8 | OR | |
| 9 | XOR | |
| 10 | NAND | |
| 11 | NOR | |
| 12 | NOT | |
| 13 | STICKY | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After activation, 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 `Operand B`. 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` with value from `Operand B` |
| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` 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` |
| Operation ID | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | TRUE | Always evaluates as true |
| 1 | EQUAL | Evaluates `false` if `false` or `0` |
| 2 | GREATER_THAN | `true` if `Operand A` is a higher value than `Operand B` |
| 3 | LOWER_THAN | `true` if `Operand A` is a lower value than `Operand B` |
| 4 | LOW | `true` if `<1333` |
| 5 | MID | `true` if `>=1333 and <=1666` |
| 6 | HIGH | `true` if `>1666` |
| 7 | AND | `true` if `Operand A` and `Operand B` are the same value or both `true` |
| 8 | OR | `true` if `Operand A` and/or `OperandB` is `true` |
| 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both |
| 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 activation operator, `Operand B` is deactivation operator. After activation, 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 `Operand B`. 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` with value from `Operand B` |
| 20 | GVAR DEC | Decrease the GVAR indexed by `Operand A` 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` |
| 22 | OVERRIDE_ARMING_SAFETY | Allows to arm on any angle even without GPS fix |
| 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 |
@ -66,111 +66,110 @@ IPF can be edited using INAV Configurator user interface, of 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` |
| 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 | 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` |
| 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` |
| 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 | 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` |
| 45 | FLIGTH_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 | FLIGTH_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 |
| 46 | FLIGTH_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 |
### Operands
| Operand Type | Name | Notes |
|---- |---- |---- |
| 0 | VALUE | Value derived from `value` field |
| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 |
| 2 | FLIGHT | `value` points to flight parameter table |
| 3 | FLIGHT_MODE | `value` points to flight modes table |
| 4 | LC | `value` points to other logic condition ID |
| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
| Operand Type | Name | Notes |
|---------------|-----------------------|-------|
| 0 | VALUE | Value derived from `value` field |
| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 |
| 2 | FLIGHT | `value` points to flight parameter table |
| 3 | FLIGHT_MODE | `value` points to flight modes table |
| 4 | LC | `value` points to other logic condition ID |
| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
#### FLIGHT
| Operand Value | Name | Notes |
|---- |---- |---- |
| 0 | ARM_TIMER | in `seconds` |
| 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` |
| 3 | RSSI | |
| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | |
| 9 | GROUD_SPEED | in `cm/s` |
| 10 | 3D_SPEED | in `cm/s` |
| 11 | AIR_SPEED | in `cm/s` |
| 12 | ALTITUDE | in `cm` |
| 13 | VERTICAL_SPEED | in `cm/s` |
| 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_WP | boolean `0`/`1` |
| 24 | IS_LANDING | boolean `0`/`1` |
| 25 | IS_FAILSAFE | boolean `0`/`1` |
| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` |
| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` |
| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` |
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 35 | LOITER_RADIUS | The current loiter radius in cm. |
| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
| Operand Value | Name | Notes |
|---------------|-------------------------------|-------|
| 0 | ARM_TIMER | in `seconds` |
| 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` |
| 3 | RSSI | |
| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | |
| 9 | GROUD_SPEED | in `cm/s` |
| 10 | 3D_SPEED | in `cm/s` |
| 11 | AIR_SPEED | in `cm/s` |
| 12 | ALTITUDE | in `cm` |
| 13 | VERTICAL_SPEED | in `cm/s` |
| 14 | TROTTLE_POS | in `%` |
| 15 | ATTITUDE_ROLL | in `degrees` |
| 16 | ATTITUDE_PITCH | in `degrees` |
| 17 | IS_ARMED | boolean `0`/`1` |
| 18 | IS_AUTOLAUNCH | boolean `0`/`1` |
| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` |
| 20 | IS_POSITION_CONTROL | boolean `0`/`1` |
| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` |
| 22 | IS_RTH | boolean `0`/`1` |
| 23 | IS_WP | boolean `0`/`1` |
| 24 | IS_LANDING | boolean `0`/`1` |
| 25 | IS_FAILSAFE | boolean `0`/`1` |
| 26 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` |
| 27 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` |
| 28 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` |
| 29 | ACTIVE_WAYPOINT_INDEX | Indexed from `1`. To verify WP is in progress, use `IS_WP` |
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
| 35 | LOITER_RADIUS | The current loiter radius in cm. |
| 36 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
#### ACTIVE_WAYPOINT_ACTION
| Action | Value |
|---- |---- |
| WAYPOINT | 1 |
| HOLD_TIME | 3 |
| RTH | 4 |
| SET_POI | 5 |
| JUMP | 6 |
| SET_HEAD | 7 |
| LAND | 8 |
| Action | Value |
|---------------|-------|
| WAYPOINT | 1 |
| HOLD_TIME | 3 |
| RTH | 4 |
| SET_POI | 5 |
| JUMP | 6 |
| SET_HEAD | 7 |
| LAND | 8 |
#### FLIGHT_MODE
| Operand Value | Name | Notes |
|---- |---- |---- |
| 0 | FAILSAFE | |
| 1 | MANUAL | |
| 2 | RTH | |
| 3 | POSHOLD | |
| 4 | CRUISE | |
| 5 | ALTHOLD | |
| 6 | ANGLE | |
| 7 | HORIZON | |
| 8 | AIR | |
| 9 | USER1 | |
| 10 | USER2 | |
| Operand Value | Name | Notes |
|---------------|-----------|-------|
| 0 | FAILSAFE | |
| 1 | MANUAL | |
| 2 | RTH | |
| 3 | POSHOLD | |
| 4 | CRUISE | |
| 5 | ALTHOLD | |
| 6 | ANGLE | |
| 7 | HORIZON | |
| 8 | AIR | |
| 9 | USER1 | |
| 10 | USER2 | |
### Flags
All flags are reseted on ARM and DISARM event.
| bit | Decimal | Function |
|---- |---- |---- |
| bit | Decimal | Function |
|-------|-----------|-----------|
| 0 | 1 | Latch - after activation LC will stay active until LATCH flag is reseted |
## Global variables

2
src/main/io/osd_hud.c

@ -205,7 +205,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
if (((millis() / 1000) % 6 == 0) && poiType > 0) { // For Radar and WPs, display the difference in altitude
altc = ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) ? constrain(CENTIMETERS_TO_FEET(poiAltitude * 100), -99, 99) : constrain(poiAltitude, -99 , 99);
tfp_sprintf(buff, "%3d", altc);
buff[0] = (poiAltitude >= 0) ? SYM_HUD_ARROWS_U3 : SYM_HUD_ARROWS_D3;
buff[0] = (poiAltitude >= 0) ? SYM_DIRECTION : SYM_DIRECTION+4;
}
else { // Display the distance by default
if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {

1
src/main/target/MAMBAF405_2022A/CMakeLists.txt

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

59
src/main/target/MAMBAF405_2022A/config.c

@ -0,0 +1,59 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "common/axis.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "drivers/sensor.h"
#include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h"
#include "drivers/serial.h"
#include "fc/rc_controls.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "io/serial.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "fc/fc_msp_box.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
}

39
src/main/target/MAMBAF405_2022A/target.c

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

197
src/main/target/MAMBAF405_2022A/target.h

@ -0,0 +1,197 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define USE_TARGET_CONFIG
#define TARGET_BOARD_IDENTIFIER "M42A"
#define USBD_PRODUCT_STRING "MAMBAF405_2022A"
// ******** Board LEDs **********************
#define LED0 PC15
#define LED1 PC14
// ******* Beeper ***********
#define BEEPER PC13
#define BEEPER_INVERTED
// ******* GYRO and ACC ********
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
//MPU6000
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define MPU6000_SPI_BUS BUS_SPI1
#define MPU6000_CS_PIN PA4
//MPU6500
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW180_DEG
#define MPU6500_CS_PIN SPI1_NSS_PIN
#define MPU6500_SPI_BUS BUS_SPI1
//BMI270
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN SPI1_NSS_PIN
#define BMI270_EXTI_PIN GYRO_INT_EXTI
// *************** Baro **************************
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C2_SCL PB8
#define I2C2_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
#define USE_BARO
#define BARO_I2C_BUS DEFAULT_I2C_BUS
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define USE_BARO_DPS310
#define USE_BARO_SPL06
//*********** Magnetometer / Compass *************
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#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
// ******* SERIAL ********
#define USE_VCP
#define USE_UART1
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define USE_UART2
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define USE_UART3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define USE_UART4
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define USE_UART5
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define USE_UART6
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 7
#define USE_UART_INVERTER
#define INVERTER_PIN_UART1_RX PC0
// ******* SPI ********
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_NSS_PIN PA4
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_NSS_PIN PA15
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
// ******* ADC ********
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC3
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define VBAT_SCALE_DEFAULT 1100
// ******* OSD ********
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN PB12
//******* FLASH ********
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_SPI_BUS BUS_SPI3
#define M25P16_CS_PIN PA15
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
//************ LEDSTRIP *****************
#define USE_LED_STRIP
#define WS2811_PIN PB3
// ******* FEATURES ********
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_UART SERIAL_PORT_USART1
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY)
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 6
#define TARGET_MOTOR_COUNT 4
// ESC-related features
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define BNO055_I2C_BUS DEFAULT_I2C_BUS
// *************** PINIO ***************************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC2
#define PINIO2_PIN PC5

6
src/main/target/MAMBAF722/target.h

@ -46,6 +46,12 @@
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
// The 2022 variant of F722_I2C with MPU6500
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW180_DEG
#define MPU6500_CS_PIN SPI1_NSS_PIN
#define MPU6500_SPI_BUS BUS_SPI1
#define USE_I2C
#ifdef MAMBAF722_I2C

1
src/main/target/MAMBAF722_2022A/CMakeLists.txt

@ -0,0 +1 @@
target_stm32f722xe(MAMBAF722_2022A)

65
src/main/target/MAMBAF722_2022A/config.c

@ -0,0 +1,65 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "common/axis.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "drivers/sensor.h"
#include "drivers/pwm_esc_detect.h"
#include "drivers/pwm_output.h"
#include "drivers/serial.h"
#include "fc/rc_controls.h"
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "io/serial.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h"
#include "io/piniobox.h"
void targetConfiguration(void)
{
pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
/*
* UART1 is SerialRX
*/
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
/*
* Enable MSP at 115200 at UART4
*/
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
}

43
src/main/target/MAMBAF722_2022A/target.c

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

190
src/main/target/MAMBAF722_2022A/target.h

@ -0,0 +1,190 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* INAV is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define USE_TARGET_CONFIG
#define TARGET_BOARD_IDENTIFIER "M72A"
#define USBD_PRODUCT_STRING "MAMBAF722_2022A"
// ******** Board LEDs **********************
#define LED0 PC15
#define LED1 PC14
// ******* Beeper ***********
#define BEEPER PB2
#define BEEPER_INVERTED
// ******* GYRO and ACC ********
#define USE_EXTI
#define GYRO_INT_EXTI PC4
#define USE_MPU_DATA_READY_SIGNAL
//MPU6000
#define USE_IMU_MPU6000
#define IMU_MPU6000_ALIGN CW180_DEG
#define MPU6000_CS_PIN SPI1_NSS_PIN
#define MPU6000_SPI_BUS BUS_SPI1
//MPU6500
#define USE_IMU_MPU6500
#define IMU_MPU6500_ALIGN CW180_DEG
#define MPU6500_CS_PIN SPI1_NSS_PIN
#define MPU6500_SPI_BUS BUS_SPI1
//BMI270
#define USE_IMU_BMI270
#define IMU_BMI270_ALIGN CW180_DEG
#define BMI270_SPI_BUS BUS_SPI1
#define BMI270_CS_PIN SPI1_NSS_PIN
#define BMI270_EXTI_PIN GYRO_INT_EXTI
#define USE_I2C
#define USE_I2C_DEVICE_1
#define I2C1_SCL PB8
#define I2C1_SDA PB9
#define DEFAULT_I2C_BUS BUS_I2C1
// *************** Baro **************************
#define USE_BARO
#define BARO_I2C_BUS DEFAULT_I2C_BUS
#define USE_BARO_BMP280
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define USE_BARO_DPS310
#define USE_BARO_SPL06
//*********** Magnetometer / Compass *************
#define USE_MAG
#define MAG_I2C_BUS DEFAULT_I2C_BUS
#define USE_MAG_HMC5883
#define USE_MAG_QMC5883
#define USE_MAG_IST8310
#define USE_MAG_MAG3110
#define USE_MAG_LIS3MDL
// ******* SERIAL ********
#define USE_VCP
#define USE_UART1
#define USE_UART2
#define USE_UART3
#define USE_UART4
#define USE_UART5
#define USE_UART6
#define UART1_TX_PIN PB6
#define UART1_RX_PIN PB7
#define UART2_TX_PIN PA2
#define UART2_RX_PIN PA3
#define UART3_TX_PIN PB10
#define UART3_RX_PIN PB11
#define UART4_TX_PIN PA0
#define UART4_RX_PIN PA1
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
#define UART6_TX_PIN PC6
#define UART6_RX_PIN PC7
#define SERIAL_PORT_COUNT 7
// ******* SPI ********
#define USE_SPI
#define USE_SPI_DEVICE_1
#define SPI1_NSS_PIN PA4
#define SPI1_SCK_PIN PA5
#define SPI1_MISO_PIN PA6
#define SPI1_MOSI_PIN PA7
#define USE_SPI_DEVICE_2
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#define USE_SPI_DEVICE_3
#define SPI3_NSS_PIN PA15
#define SPI3_SCK_PIN PC10
#define SPI3_MISO_PIN PC11
#define SPI3_MOSI_PIN PB5
// ******* ADC ********
#define USE_ADC
#define ADC_CHANNEL_1_PIN PC1
#define ADC_CHANNEL_2_PIN PC3
#define VBAT_ADC_CHANNEL ADC_CHN_1
#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
#define VBAT_SCALE_DEFAULT 1100
// ******* OSD ********
#define USE_MAX7456
#define MAX7456_SPI_BUS BUS_SPI2
#define MAX7456_CS_PIN SPI2_NSS_PIN
//******* FLASH ********
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define M25P16_CS_PIN SPI3_NSS_PIN
#define M25P16_SPI_BUS BUS_SPI3
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
//************ LEDSTRIP *****************
#define USE_LED_STRIP
#define WS2811_PIN PB3
// ******* FEATURES ********
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
#define SERIALRX_UART SERIAL_PORT_USART1
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY)
#define TARGET_IO_PORTA 0xffff
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define MAX_PWM_OUTPUT_PORTS 8
#define TARGET_MOTOR_COUNT 8
// ESC-related features
#define USE_DSHOT
#define USE_ESC_SENSOR
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define USE_RANGEFINDER
#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS
#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS
#define PITOT_I2C_BUS DEFAULT_I2C_BUS
#define BNO055_I2C_BUS DEFAULT_I2C_BUS
// *************** PINIO ***************************
#define USE_PINIO
#define USE_PINIOBOX
#define PINIO1_PIN PC0 // VTX power switcher
#define PINIO2_PIN PC2 // WiFi Switcher
Loading…
Cancel
Save