From 1b1271a9f42bb6debdea0753f684e025b198fbbb Mon Sep 17 00:00:00 2001 From: Olivier C Date: Sat, 19 Mar 2022 18:46:19 +0100 Subject: [PATCH 1/6] Update osd_hud.c Previous fix for the up and down arrows for the relative altitude (ESP32 radar) didn't really work because of an INT overflow, source of the issue was in the rer-ordering of the OSD chars --- src/main/io/osd_hud.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 62c8187c6..c2dce6194 100644 --- a/src/main/io/osd_hud.c +++ b/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) { From 527ab2a34110a1ca9c74f125b58bf5e7d2264dd5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 14 Apr 2022 15:01:20 +0200 Subject: [PATCH 2/6] Enable MPU6500 on MambaF722_I2C --- src/main/target/MAMBAF722/target.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 5f4b9035a..b6522ee00 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/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 From eef3524bb3032bf80a38f2344cf744a1d95c4971 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 15 Apr 2022 10:42:27 +0100 Subject: [PATCH 3/6] Added missing notes and tidied --- docs/Programming Framework.md | 217 +++++++++++++++++----------------- 1 file changed, 108 insertions(+), 109 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 3052b4f8e..87fa89b94 100644 --- a/docs/Programming Framework.md +++ b/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 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` | +| 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,109 +66,108 @@ 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` | ### 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 From 3d13b72666ac9d4cef4cf2e1114fddb52cee5d14 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 16 Apr 2022 15:11:43 +0200 Subject: [PATCH 4/6] MAMBAF722_2022A target --- .../target/MAMBAF722_2022A/CMakeLists.txt | 1 + src/main/target/MAMBAF722_2022A/config.c | 65 ++++++ src/main/target/MAMBAF722_2022A/target.c | 43 ++++ src/main/target/MAMBAF722_2022A/target.h | 190 ++++++++++++++++++ 4 files changed, 299 insertions(+) create mode 100644 src/main/target/MAMBAF722_2022A/CMakeLists.txt create mode 100644 src/main/target/MAMBAF722_2022A/config.c create mode 100644 src/main/target/MAMBAF722_2022A/target.c create mode 100644 src/main/target/MAMBAF722_2022A/target.h diff --git a/src/main/target/MAMBAF722_2022A/CMakeLists.txt b/src/main/target/MAMBAF722_2022A/CMakeLists.txt new file mode 100644 index 000000000..202a13f7c --- /dev/null +++ b/src/main/target/MAMBAF722_2022A/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(MAMBAF722_2022A) \ No newline at end of file diff --git a/src/main/target/MAMBAF722_2022A/config.c b/src/main/target/MAMBAF722_2022A/config.c new file mode 100644 index 000000000..4b9b627f1 --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +#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; +} diff --git a/src/main/target/MAMBAF722_2022A/target.c b/src/main/target/MAMBAF722_2022A/target.c new file mode 100644 index 000000000..afe6e3635 --- /dev/null +++ b/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 . + */ + + +#include + +#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]); diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h new file mode 100644 index 000000000..32091afd6 --- /dev/null +++ b/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 . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "M7X8" +#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 \ No newline at end of file From cf4965bc67e8180daee1c06e6e5076b15c759887 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 16 Apr 2022 16:55:02 +0200 Subject: [PATCH 5/6] MAMBAF405_2022A target --- .../target/MAMBAF405_2022A/CMakeLists.txt | 1 + src/main/target/MAMBAF405_2022A/config.c | 59 ++++++ src/main/target/MAMBAF405_2022A/target.c | 39 ++++ src/main/target/MAMBAF405_2022A/target.h | 197 ++++++++++++++++++ 4 files changed, 296 insertions(+) create mode 100644 src/main/target/MAMBAF405_2022A/CMakeLists.txt create mode 100644 src/main/target/MAMBAF405_2022A/config.c create mode 100644 src/main/target/MAMBAF405_2022A/target.c create mode 100644 src/main/target/MAMBAF405_2022A/target.h diff --git a/src/main/target/MAMBAF405_2022A/CMakeLists.txt b/src/main/target/MAMBAF405_2022A/CMakeLists.txt new file mode 100644 index 000000000..418424e65 --- /dev/null +++ b/src/main/target/MAMBAF405_2022A/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MAMBAF405_2022A) diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c new file mode 100644 index 000000000..355605fd9 --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +#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; +} diff --git a/src/main/target/MAMBAF405_2022A/target.c b/src/main/target/MAMBAF405_2022A/target.c new file mode 100644 index 000000000..0e659296e --- /dev/null +++ b/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 . + */ + + +#include + +#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]); diff --git a/src/main/target/MAMBAF405_2022A/target.h b/src/main/target/MAMBAF405_2022A/target.h new file mode 100644 index 000000000..b3d6ad9d4 --- /dev/null +++ b/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 . + */ + +#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 \ No newline at end of file From c619b6b840315070baff66b568b02ea517882301 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 16 Apr 2022 16:56:01 +0200 Subject: [PATCH 6/6] board ientifier update --- src/main/target/MAMBAF722_2022A/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h index 32091afd6..a55fdb58c 100644 --- a/src/main/target/MAMBAF722_2022A/target.h +++ b/src/main/target/MAMBAF722_2022A/target.h @@ -19,7 +19,7 @@ #define USE_TARGET_CONFIG -#define TARGET_BOARD_IDENTIFIER "M7X8" +#define TARGET_BOARD_IDENTIFIER "M72A" #define USBD_PRODUCT_STRING "MAMBAF722_2022A" // ******** Board LEDs **********************