Skip to content

Commit 9c0ee40

Browse files
qqqlabqqqlab
committed
add macro for cfg parameters, cfg lowercase
1 parent 5d524ee commit 9c0ee40

File tree

16 files changed

+492
-267
lines changed

16 files changed

+492
-267
lines changed

‎extras/TestMadflight/cfg.ino‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,9 +92,9 @@ void setup() {
9292

9393
void loop() {
9494
cfg.begin();
95-
Serial.printf("crc=%d crcCalc=%d len=%d v=%f i=%f press a key to increase i by 1.1\n", cfg.crc(), cfg.crcCalc(), cfg.len(), cfg.BAT_CAL_V, cfg.BAT_CAL_I);
95+
Serial.printf("crc=%d crcCalc=%d len=%d v=%f i=%f press a key to increase i by 1.1\n", cfg.crc(), cfg.crcCalc(), cfg.len(), cfg.bat_cal_v, cfg.bat_cal_i);
9696
if(Serial.available()) {
97-
cfg.BAT_CAL_I += 1.1;
97+
cfg.bat_cal_i += 1.1;
9898
cfg.write();
9999

100100
while(Serial.available()) Serial.read();

‎src/madflight.h‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@ SOFTWARE.
3131

3232
#pragma once
3333

34+
//madflight init
35+
extern const char* madflight_init_commands __attribute__((weak));
36+
3437
#include "madflight/common/MF_Serial.h"
3538
#include "madflight/common/MF_I2C.h"
3639

@@ -137,6 +140,12 @@ void madflight_setup() {
137140

138141
cli.print_boardInfo(); //print board info and pinout
139142

143+
cfg.begin(); //read config from EEPROM
144+
if(madflight_init_commands) {
145+
Serial.println(madflight_init_commands);
146+
cli.cmd_execute_batch(madflight_init_commands);
147+
}
148+
140149
//hardware specific setup for busses: serial, spi and i2c (see hw_xxx.h)
141150
hw_setup();
142151

@@ -145,7 +154,6 @@ void madflight_setup() {
145154
if(!gps_Serial) gps_Serial = new MF_SerialNone();
146155
if(!mf_i2c) mf_i2c = new MF_I2CNone();
147156

148-
cfg.begin(); //read config from EEPROM
149157
cli.print_i2cScan(); //print i2c scan
150158

151159
rcin.setup(); //Initialize radio communication.

‎src/madflight/ahrs/ahrs.h‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -23,14 +23,14 @@ void Ahrs::update() {
2323
// compute euler angles from q
2424

2525
//Low-pass filtered, corrected accelerometer data
26-
ax += B_acc * ((imu.ax - cfg.IMU_CAL_AX) - ax);
27-
ay += B_acc * ((imu.ay - cfg.IMU_CAL_AY) - ay);
28-
az += B_acc * ((imu.az - cfg.IMU_CAL_AZ) - az);
26+
ax += B_acc * ((imu.ax - cfg.imu_cal_ax) - ax);
27+
ay += B_acc * ((imu.ay - cfg.imu_cal_ay) - ay);
28+
az += B_acc * ((imu.az - cfg.imu_cal_az) - az);
2929

3030
//Low-pass filtered, corrected gyro data
31-
gx += B_gyr * ((imu.gx - cfg.IMU_CAL_GX) - gx);
32-
gy += B_gyr * ((imu.gy - cfg.IMU_CAL_GY) - gy);
33-
gz += B_gyr * ((imu.gz - cfg.IMU_CAL_GZ) - gz);
31+
gx += B_gyr * ((imu.gx - cfg.imu_cal_gx) - gx);
32+
gy += B_gyr * ((imu.gy - cfg.imu_cal_gy) - gy);
33+
gz += B_gyr * ((imu.gz - cfg.imu_cal_gz) - gz);
3434

3535
//External Magnetometer
3636
float _mx = mag.x;
@@ -45,9 +45,9 @@ void Ahrs::update() {
4545
//update the mag values
4646
if( ! (_mx == 0 && _my == 0 && _mz == 0) ) {
4747
//Correct the mag values with the calibration values
48-
_mx = (_mx - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX;
49-
_my = (_my - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY;
50-
_mz = (_mz - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ;
48+
_mx = (_mx - cfg.mag_cal_x) * cfg.mag_cal_sx;
49+
_my = (_my - cfg.mag_cal_y) * cfg.mag_cal_sy;
50+
_mz = (_mz - cfg.mag_cal_z) * cfg.mag_cal_sz;
5151
//Low-pass filtered magnetometer data
5252
mx += B_mag * (_mx - mx);
5353
my += B_mag * (_my - my);

‎src/madflight/bat/BatteryADC.h‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,8 @@ ADC Battery Monitor
44
Need to have at least HW_PIN_BAT_V or HW_PIN_BAT_I defined before including this header
55
66
Also needs cfg for:
7-
cfg.BAT_CAL_V //BatteryADC voltage conversion factor, set this to 1 and enable print_bat(), then enter here: Actual Volt / bat_v ADC reading (for example: 8.04/13951 -> set BAT_CAL_V 0.0057630 )
8-
cfg.BAT_CAL_I //BatteryADC current conversion factor, set this to 1 and enable print_bat(), then enter here: Actual Amperes / bat_i ADC reading (for example: 1.0/847 --> set BAT_CAL_I 0.0011806 )
7+
cfg.bat_cal_v //BatteryADC voltage conversion factor, set this to 1 and enable print_bat(), then enter here: Actual Volt / bat_v ADC reading (for example: 8.04/13951 -> set BAT_CAL_V 0.0057630 )
8+
cfg.bat_cal_i //BatteryADC current conversion factor, set this to 1 and enable print_bat(), then enter here: Actual Amperes / bat_i ADC reading (for example: 1.0/847 --> set BAT_CAL_I 0.0011806 )
99
1010
=================================================================================================*/
1111

@@ -26,12 +26,12 @@ class BatteryADC: public Battery {
2626
v = 0;
2727
mah = 0;
2828
wh = 0;
29-
#ifdef HW_PIN_BAT_V
30-
pinMode(HW_PIN_BAT_V, INPUT);
31-
#endif
32-
#ifdef HW_PIN_BAT_I
33-
pinMode(HW_PIN_BAT_I, INPUT);
34-
#endif
29+
if(cfg.pin_bat_v >= 0) {
30+
pinMode((int)cfg.pin_bat_v, INPUT);
31+
}
32+
if(cfg.bat_cal_v >= 0) {
33+
pinMode((int)cfg.bat_cal_v, INPUT);
34+
}
3535
analogReadResolution(16);
3636
}
3737

@@ -43,12 +43,12 @@ class BatteryADC: public Battery {
4343
uint32_t dt = now - ts;
4444
float dt_h = dt / 3600e6;
4545
ts = now;
46-
#ifdef HW_PIN_BAT_V
47-
v = cfg.BAT_CAL_V * analogRead(HW_PIN_BAT_V);
48-
#endif
49-
#ifdef HW_PIN_BAT_I
50-
i = cfg.BAT_CAL_V * analogRead(HW_PIN_BAT_I);
51-
#endif
46+
if(cfg.pin_bat_v >= 0) {
47+
v = cfg.bat_cal_v * analogRead((int)cfg.pin_bat_v);
48+
}
49+
if(cfg.bat_cal_v >= 0) {
50+
i = cfg.bat_cal_v * analogRead((int)cfg.pin_bat_i);
51+
}
5252
w = v * i;
5353
mah += i * dt_h * 1000;
5454
wh += w * dt_h;

‎src/madflight/bat/BatteryINA226.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ class BatteryINA226: public Battery {
1616
// Configure INA226 -> sample time = 2 * 128 * 140us = 36ms => 28Hz
1717
bat_ina226.configure(INA226_AVERAGES_128, INA226_BUS_CONV_TIME_140US, INA226_SHUNT_CONV_TIME_140US, INA226_MODE_SHUNT_BUS_CONT);
1818

19-
float Rshunt = cfg.BAT_CAL_I; //ohm
19+
float Rshunt = cfg.bat_cal_i; //ohm
2020
bat_ina226.calibrate(Rshunt);
2121
}
2222

‎src/madflight/bat/BatteryINA228.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ class BatteryINA228: public Battery {
1212
void begin(MF_I2C *i2c, int8_t i2c_adr) override {
1313
if(i2c_adr == 0) i2c_adr = 0x40; //default address
1414

15-
float Rshunt = cfg.BAT_CAL_I; //ohm
15+
float Rshunt = cfg.bat_cal_i; //ohm
1616

1717
// Configure INA226 -> sample time = 512 * 2 * 50us = 51.2ms => 20Hz
1818
bat_ina228.begin(i2c, i2c_adr);

‎src/madflight/bb/bb_sdcard/bb_sdcard.h‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -918,23 +918,23 @@ class BlackBox_SD : public BlackBox {
918918
BinLog bl("IMU");
919919
bl.keepFree = QUEUE_LENGTH/4; //keep 25% of queue free for other messages
920920
bl.TimeUS(imu.ts);
921-
bl.i16("ax",(imu.ax - cfg.IMU_CAL_AX)*1000, 1e-3, "G"); //G
922-
bl.i16("ay",(imu.ay - cfg.IMU_CAL_AY)*1000, 1e-3, "G"); //G
923-
bl.i16("az",(imu.az - cfg.IMU_CAL_AZ)*1000, 1e-3, "G"); //G
924-
bl.i16("gx",(imu.gx - cfg.IMU_CAL_GX)*10, 1e-1, "deg/s"); //dps
925-
bl.i16("gy",(imu.gy - cfg.IMU_CAL_GY)*10, 1e-1, "deg/s"); //dps
926-
bl.i16("gz",(imu.gz - cfg.IMU_CAL_GZ)*10, 1e-1, "deg/s"); //dps
921+
bl.i16("ax",(imu.ax - cfg.imu_cal_ax)*1000, 1e-3, "G"); //G
922+
bl.i16("ay",(imu.ay - cfg.imu_cal_ay)*1000, 1e-3, "G"); //G
923+
bl.i16("az",(imu.az - cfg.imu_cal_az)*1000, 1e-3, "G"); //G
924+
bl.i16("gx",(imu.gx - cfg.imu_cal_gx)*10, 1e-1, "deg/s"); //dps
925+
bl.i16("gy",(imu.gy - cfg.imu_cal_gy)*10, 1e-1, "deg/s"); //dps
926+
bl.i16("gz",(imu.gz - cfg.imu_cal_gz)*10, 1e-1, "deg/s"); //dps
927927
#if MAG_USE != MAG_USE_NONE
928928
//get from magnetometer
929-
bl.i16("mx",((mag.x - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX)*100, 1e-2, "uT"); //uT
930-
bl.i16("my",((mag.y - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY)*100, 1e-2, "uT"); //uT
931-
bl.i16("mz",((mag.z - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ)*100, 1e-2, "uT"); //uT
929+
bl.i16("mx",((mag.x - cfg.mag_cal_x) * cfg.mag_cal_sx)*100, 1e-2, "uT"); //uT
930+
bl.i16("my",((mag.y - cfg.mag_cal_y) * cfg.mag_cal_sy)*100, 1e-2, "uT"); //uT
931+
bl.i16("mz",((mag.z - cfg.mag_cal_z) * cfg.mag_cal_sz)*100, 1e-2, "uT"); //uT
932932
#else
933933
//get from imu
934934
if(imu.hasMag()) {
935-
bl.i16("mx",((imu.mx - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX)*100, 1e-2, "uT"); //uT
936-
bl.i16("my",((imu.my - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY)*100, 1e-2, "uT"); //uT
937-
bl.i16("mz",((imu.mz - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ)*100, 1e-2, "uT"); //uT
935+
bl.i16("mx",((imu.mx - cfg.mag_cal_x) * cfg.mag_cal_sx)*100, 1e-2, "uT"); //uT
936+
bl.i16("my",((imu.my - cfg.mag_cal_y) * cfg.mag_cal_sy)*100, 1e-2, "uT"); //uT
937+
bl.i16("mz",((imu.mz - cfg.mag_cal_z) * cfg.mag_cal_sz)*100, 1e-2, "uT"); //uT
938938
}
939939
#endif
940940
bl.i16("roll",ahrs.roll*100, 1e-2, "deg"); //deg -180 to 180

0 commit comments

Comments
 (0)