Skip to content

Commit 0140e23

Browse files
qqqlabqqqlab
committed
fix SBUS
1 parent c12e4c5 commit 0140e23

File tree

19 files changed

+679
-178
lines changed

19 files changed

+679
-178
lines changed

‎examples/00.HelloWorld/00.HelloWorld.ino‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
/*#########################################################################################################################
2+
3+
"Hello World" for madflight library
4+
5+
Upload, connect Serial Monitor at 115200 baud and send 'help' to see available commands
6+
7+
See http://madflight.com for detailed description
8+
9+
MIT license
10+
Copyright (c) 2023-2025 https://madflight.com
11+
##########################################################################################################################*/
12+
13+
#include "madflight_config.h" //Edit this header file to setup the pins, hardware, radio, etc. for madflight
14+
#include <madflight.h>
15+
16+
void setup() {
17+
//setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component.
18+
madflight_setup();
19+
}
20+
21+
void loop() {
22+
rcl.update(); // get rc radio commands
23+
bar.update(); // barometer
24+
mag.update(); // magnetometer
25+
gps.update(); // gps
26+
bat.update(); // battery consumption
27+
cli.update(); //process CLI commands
28+
}
29+
30+
//This is __MAIN__ function of this program. It is called when new IMU data is available.
31+
void imu_loop() {
32+
//toggle led on every 1000 samples (normally 1 second)
33+
if(imu.update_cnt % 1000 == 0) led.toggle();
34+
35+
ahr.update(); //Sensor fusion: update ahr.roll, ahr.pitch, and ahr.yaw angle estimates (degrees) from IMU data
36+
}

‎examples/00.HelloWorld/madflight_config.h‎

Lines changed: 190 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,190 @@
1+
//========================================================================================================================//
2+
// PINS CONFIG //
3+
//========================================================================================================================//
4+
//
5+
// You have 3 options to setup the pins (gpio numbers) and busses for the flight controller:
6+
//
7+
// 1) Default - #include <madflight_board.h> and see https://madflight.com for default pinout diagrams for the supported
8+
// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h
9+
//
10+
// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available
11+
// boards. For example: #include <madflight_board_betaflight_MTKS-MATEKH743.h>
12+
//
13+
// 3) Custom - Do not include a board file here, and set your own board definition in the CUSTOM PINS section below.
14+
//
15+
//========================================================================================================================//
16+
17+
#include <madflight_board.h>
18+
19+
20+
//========================================================================================================================//
21+
// HARDWARE CONFIG //
22+
//========================================================================================================================//
23+
//
24+
// Hardware configuration is a simple key-value list. Anything after '#' or '/' is ignored as comment
25+
//
26+
//========================================================================================================================//
27+
28+
#define MADFLIGHT_CONFIG R""(
29+
30+
// IMU - Inertial Measurement Unit (acc/gyro)
31+
32+
// Uncomment ONE bus: SPI or I2C
33+
imu_spi_bus 0 // connect IMU to SPI bus 0
34+
//imu_i2c_bus 1 // connect IMU to I2C bus 1
35+
36+
// IMPORTANT: the IMU sensor should be the ONLY sensor on the selected bus
37+
38+
imu_gizmo NONE // options: NONE, BMI270, MPU6000, MPU6050, MPU6500, MPU9150, MPU9250
39+
imu_align CW90 // options: CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP
40+
imu_i2c_adr 0 // use 0 for default i2c address
41+
42+
// RCL - Remote Controller Link
43+
rcl_gizmo NONE // options: NONE, MAVLINK, CRSF, SBUS, DSM, PPM, PWM
44+
rcl_num_ch 8 // number of channels
45+
rcl_deadband 0 // center stick deadband
46+
47+
// BAR - Barometer
48+
bar_gizmo NONE // options: NONE, BMP390, BMP388, BMP280, MS5611
49+
bar_i2c_adr 0
50+
51+
// MAG - Magnetometer
52+
mag_gizmo NONE // options: NONE, QMC5883
53+
mag_i2c_adr 0
54+
55+
// BAT - Battery Monitor
56+
bat_gizmo NONE // options: NONE, ADC, INA226, INA228
57+
bat_i2c_adr 0
58+
59+
// GPS
60+
gps_gizmo NONE // options: NONE, UBLOX
61+
gps_baud 0 // use 0 for auto baud
62+
63+
// BBX - Black Box Data Logger
64+
bbx_gizmo NONE // options: NONE, SDSPI, SDMMC
65+
66+
)"" // End of MADFLIGHT_CONFIG
67+
68+
69+
//========================================================================================================================//
70+
// CUSTOM PINS CONFIG //
71+
//========================================================================================================================//
72+
73+
/* <-- Add a / here to setup your custom pins below - Check datasheet that what you're doing is actually possible!!!
74+
75+
#define MADFLIGHT_BOARD R""(
76+
77+
// PINOUT
78+
79+
// Serial Pins
80+
pin_ser0_rx -1
81+
pin_ser0_tx -1
82+
pin_ser0_inv -1
83+
pin_ser1_rx -1
84+
pin_ser1_tx -1
85+
pin_ser1_inv -1
86+
pin_ser2_rx -1
87+
pin_ser2_tx -1
88+
pin_ser2_inv -1
89+
pin_ser3_rx -1
90+
pin_ser3_tx -1
91+
pin_ser3_inv -1
92+
pin_ser4_rx -1
93+
pin_ser4_tx -1
94+
pin_ser4_inv -1
95+
pin_ser5_rx -1
96+
pin_ser5_tx -1
97+
pin_ser5_inv -1
98+
pin_ser6_rx -1
99+
pin_ser6_tx -1
100+
pin_ser6_inv -1
101+
pin_ser7_rx -1
102+
pin_ser7_tx -1
103+
pin_ser7_inv -1
104+
105+
// SPI Pins
106+
pin_spi0_miso -1
107+
pin_spi0_mosi -1
108+
pin_spi0_sclk -1
109+
pin_spi1_miso -1
110+
pin_spi1_mosi -1
111+
pin_spi1_sclk -1
112+
pin_spi2_miso -1
113+
pin_spi2_mosi -1
114+
pin_spi2_sclk -1
115+
pin_spi3_miso -1
116+
pin_spi3_mosi -1
117+
pin_spi3_sclk -1
118+
119+
// I2C Pins
120+
pin_i2c0_sda -1
121+
pin_i2c0_scl -1
122+
pin_i2c1_sda -1
123+
pin_i2c1_scl -1
124+
pin_i2c2_sda -1
125+
pin_i2c2_scl -1
126+
pin_i2c3_sda -1
127+
pin_i2c3_scl -1
128+
129+
// OUT Pins
130+
pin_out0 -1
131+
pin_out1 -1
132+
pin_out2 -1
133+
pin_out3 -1
134+
pin_out4 -1
135+
pin_out5 -1
136+
pin_out6 -1
137+
pin_out7 -1
138+
pin_out8 -1
139+
pin_out9 -1
140+
pin_out10 -1
141+
pin_out11 -1
142+
pin_out12 -1
143+
pin_out13 -1
144+
pin_out14 -1
145+
pin_out15 -1
146+
147+
// Other Pins
148+
pin_bat_i -1
149+
pin_bat_v -1
150+
pin_bbx_cs -1
151+
pin_imu_cs -1
152+
pin_imu_int -1
153+
pin_led -1
154+
led_on LOW_IS_ON
155+
pin_mmc_dat -1
156+
pin_mmc_clk -1
157+
pin_mmc_cmd -1
158+
pin_rcl_ppm -1
159+
160+
// BUSSES
161+
162+
// Serial Busses
163+
rcl_ser_bus -1
164+
gps_ser_bus -1
165+
rdr_ser_bus -1
166+
167+
// SPI Busses
168+
imu_spi_bus -1
169+
bbx_spi_bus -1
170+
171+
// I2C Busses
172+
bar_i2c_bus -1
173+
mag_i2c_bus -1
174+
bat_i2c_bus -1
175+
imu_i2c_bus -1
176+
177+
)"" // end of MADFLIGHT_BOARD */
178+
179+
//========================================================================================================================//
180+
// COMPILER OPTIONS //
181+
//========================================================================================================================//
182+
183+
//-- AHRS sensor fusion
184+
#define AHR_USE AHR_USE_MAHONY // Select one: AHRS_USE_MAHONY, AHRS_USE_MAHONY_BF, AHRS_USE_MADGWICK, AHRS_USE_VQF
185+
186+
// Reset config eeprom to defaults (uncomment this, upload, then comment out again)
187+
//#define MF_CONFIG_CLEAR
188+
189+
// Uncomment to print additional debug information and reduce startup delay
190+
//#define MF_DEBUG

‎src/madflight.h‎

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -107,15 +107,22 @@ void madflight_setup() {
107107
#endif
108108
cfg.loadFromEeprom(); //load parameters from EEPROM
109109
if(madflight_config) {
110-
#ifdef MF_DEBUG
111-
Serial.println(madflight_config);
112-
#endif
113110
if(cfg.load_madflight_param(madflight_config)) {
114111
Serial.println("CFG: madflight_config - Loaded");
115112
}else{
116113
Serial.println("CFG: madflight_config - Skipped (was aready stored in eeprom)");
117114
}
118115
}
116+
117+
#ifdef MF_DEBUG
118+
if(madflight_config) {
119+
Serial.println("\nDEBUG: madflight_config ==============\n");
120+
Serial.println(madflight_config);
121+
}
122+
Serial.println("\nDEBUG: cfg.list() ================\n");
123+
cfg.list();
124+
#endif
125+
119126
cfg.printPins();
120127

121128
// LED - Setup LED
@@ -131,7 +138,7 @@ void madflight_setup() {
131138
cli.print_i2cScan(); //print i2c scan
132139

133140
rcl.config.gizmo = (Cfg::rcl_gizmo_enum)cfg.rcl_gizmo; //the gizmo to use
134-
rcl.config.ser_bus = hal_get_ser_bus(cfg.rcl_ser_bus); //serial bus
141+
rcl.config.ser_bus_id = cfg.rcl_ser_bus; //serial bus id
135142
rcl.config.baud = cfg.rcl_baud; //baud rate
136143
rcl.config.ppm_pin = cfg.getValue("pin_ser" + String(cfg.rcl_ser_bus) + "_rx", -1);
137144
rcl.setup(); //Initialize radio communication.

‎src/madflight/bbx/BinLogWriter.cpp‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,9 @@ namespace BinLogWriter {
3434
};
3535

3636
QueueHandle_t queue = nullptr;
37-
StaticQueue_t xStaticQueue = {};
38-
uint8_t ucQueueStorageArea[QUEUE_LENGTH * sizeof(msg_t)] = {};
37+
38+
//StaticQueue_t xStaticQueue = {};
39+
//uint8_t ucQueueStorageArea[QUEUE_LENGTH * sizeof(msg_t)] = {};
3940

4041
TaskHandle_t xHandle;
4142

@@ -60,7 +61,9 @@ namespace BinLogWriter {
6061
}
6162

6263
void setup() {
63-
queue = xQueueCreateStatic(QUEUE_LENGTH, sizeof(msg_t), ucQueueStorageArea, &xStaticQueue);
64+
//queue = xQueueCreateStatic(QUEUE_LENGTH, sizeof(msg_t), ucQueueStorageArea, &xStaticQueue); //not available for STM32
65+
queue = xQueueCreate(QUEUE_LENGTH, sizeof(msg_t));
66+
6467
if(xTaskCreate(bbx_task, "BBX", MF_FREERTOS_DEFAULT_STACK_SIZE, NULL, uxTaskPriorityGet(NULL), &xHandle) != pdPASS ){
6568
Serial.println("BBX: Task creation failed");
6669
}
@@ -273,6 +276,7 @@ namespace BinLogWriter {
273276
}
274277

275278
static void bbx_task(void *pvParameters) {
279+
(void)pvParameters;
276280
msg_t msg;
277281
for(;;) {
278282
if( xQueueReceive(queue, &msg, portMAX_DELAY) == pdPASS ) {

‎src/madflight/bbx/bbx.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ SOFTWARE.
2525
#pragma once
2626

2727
#include "../cfg/cfg.h"
28-
#include "Binlog.h"
28+
#include "BinLog.h"
2929
#include <SPI.h>
3030

3131
struct BbxConfig {

‎src/madflight/cfg/cfg.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -226,7 +226,7 @@ SOFTWARE.
226226
MF_PARAM( rdr_baud, 0, int32_t, 'i') \
227227
\
228228
/*RCL - Remote Control Link*/ \
229-
MF_PARAM( rcl_gizmo, 0, int32_t, 'e', mf_NONE,mf_MAVLINK,mf_CRSF,mf_SBUS,mf_DSM,mf_PPM) \
229+
MF_PARAM( rcl_gizmo, 0, int32_t, 'e', mf_NONE,mf_MAVLINK,mf_CRSF,mf_SBUS,mf_SBUS_NOT_INV,mf_DSM,mf_PPM) \
230230
MF_PARAM( rcl_ser_bus, -1, int32_t, 'i') \
231231
MF_PARAM( rcl_baud, 0, int32_t, 'i') \
232232
MF_PARAM( rcl_num_ch, 8, int32_t, 'i') /*max 20*/ \

‎src/madflight/cli/cli_cpp.h‎

Lines changed: 18 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -133,21 +133,30 @@ void cli_print_imu_Rate() {
133133
uint32_t now = micros();
134134
uint32_t dt = now - ts_last;
135135
ts_last = now;
136-
//Serial.printf("imu%%:%d\t", (int)(100 * (imu.stat_runtime_max - imu.stat_latency) / imu.getSamplePeriod()));
137-
Serial.printf("samp_hz:%d\t", (int)(1000000/imu.getSamplePeriod()));
136+
137+
int hz = 0;
138+
if(imu.getSamplePeriod() > 0) hz = 1000000 / imu.getSamplePeriod();
139+
Serial.printf("samp_hz:%d\t", hz);
140+
141+
if(dt == 0) dt = 1;
138142
Serial.printf("intr_hz:%.0f\t", (float)delta_int/(dt*1e-6));
139143
Serial.printf("loop_hz:%.0f\t", (float)delta_upd/(dt*1e-6));
140-
int miss = (100 - (100 * delta_upd) / delta_int);
144+
145+
int miss = 0;
146+
if(delta_int > 0) miss = (100 - (100 * delta_upd) / delta_int);
141147
Serial.printf("miss%%:%d\t", (miss<0?0:miss));
142-
//Serial.printf("stat_cnt:%d\t", (int)(imu.stat_cnt));
143-
Serial.printf("latency_us:%d\t", (int)(imu.stat_latency/imu.stat_cnt));
144-
Serial.printf("rt_io_us:%d\t", (int)(imu.stat_io_runtime/imu.stat_cnt));
145-
Serial.printf("rt_imu_loop_us:%d\t", (int)((imu.stat_runtime - imu.stat_io_runtime)/imu.stat_cnt));
146-
Serial.printf("rt_us:%d\t", (int)(imu.stat_runtime/imu.stat_cnt));
148+
149+
int stat_cnt = 1;
150+
if(imu.stat_cnt > 0) stat_cnt = imu.stat_cnt;
151+
//Serial.printf("stat_cnt:%d\t", (int)(stat_cnt));
152+
Serial.printf("latency_us:%d\t", (int)(imu.stat_latency / stat_cnt));
153+
Serial.printf("rt_io_us:%d\t", (int)(imu.stat_io_runtime / stat_cnt));
154+
Serial.printf("rt_imu_loop_us:%d\t", (int)((imu.stat_runtime - imu.stat_io_runtime) / stat_cnt));
155+
Serial.printf("rt_us:%d\t", (int)(imu.stat_runtime / stat_cnt));
147156
Serial.printf("rt_max_us:%d\t", (int)imu.stat_runtime_max);
148157
Serial.printf("int_cnt:%d\t", (int)imu.interrupt_cnt);
149158
Serial.printf("upd_cnt:%d\t", (int)imu.update_cnt);
150-
Serial.printf("miss_cnt:%d\t", (int)(imu.interrupt_cnt-imu.update_cnt));
159+
Serial.printf("miss_cnt:%d\t", (int)(imu.interrupt_cnt - imu.update_cnt));
151160
imu.statReset();
152161
}
153162

0 commit comments

Comments
 (0)