Browse Source

Added gyro and acc logics with storage.

master
Englebert 3 years ago
parent
commit
638c9c1d0c
  1. 1
      platformio.ini
  2. 131
      src/baro.cpp
  3. 21
      src/baro.h
  4. 9
      src/config.h
  5. 117
      src/gyro.cpp
  6. 94
      src/gyro.h
  7. 244
      src/main.cpp
  8. 58
      src/main.h
  9. 201
      src/storage.cpp
  10. 39
      src/storage.h

1
platformio.ini

@ -16,4 +16,5 @@ upload_speed = 460800
board_build.f_cpu = 240000000L
board_build.f_flash = 40000000L
board_build.partitions = default_16MB.csv
lib_deps = lorol/LittleFS_esp32@^1.0.6

131
src/baro.cpp

@ -1,12 +1,12 @@
/*
* REF:
* http://www.esp32learning.com/code/esp32-and-bmp280-sensor-example.php
* Refer for Orientation: https://github.com/Yenya/avr-bmp280/blob/master/bmp280/bmp280.c [MAYBE]
*
*/
#include "baro.h"
BARO::BARO(void) {
// Switch the baro to SPI
// If CSB is pulled down, the SPI interface is activated. After
@ -38,15 +38,16 @@ bool BARO::begin(void) {
baro_ok = true;
readCoefficients();
set_sampling();
// set_sampling();
set_sampling(
BARO::MODE_FORCED, // Operation Mode
BARO::SAMPLING_X2, // Temperature Oversampling
BARO::SAMPLING_X16, // Pressure Oversampling
BARO::FILTER_X16, // Filtering
BARO::STANDBY_MS_1 // Standby Time
BARO::MODE_NORMAL, // Operation Mode
BARO::SAMPLING_X1, // Temperature Oversampling (5.5mS ~ 6.4mS)
BARO::SAMPLING_X1, // Pressure Oversampling (5.5mS ~ 6.4mS)
BARO::FILTER_X16, // Filtering
BARO::STANDBY_MS_1 // Standby Time
);
delay(100);
endTransaction();
// chipid = read8(BMP280_REGISTER_CHIPID);
@ -58,27 +59,29 @@ bool BARO::begin(void) {
void BARO::get_readings(void) {
// The sequence to read all must follow as below
sensors_id = read8(BMP280_REGISTER_CHIPID);
// sensors_id = read8(BMP280_REGISTER_CHIPID);
/***
set_sampling(
BARO::MODE_FORCED, // Operation Mode
BARO::SAMPLING_X2, // Temperature Oversampling
BARO::SAMPLING_X16, // Pressure Oversampling
BARO::FILTER_X16, // Filtering
BARO::STANDBY_MS_1 // Standby Time
BARO::MODE_FORCED, // Operation Mode
BARO::SAMPLING_X1, // Temperature Oversampling
BARO::SAMPLING_X1, // Pressure Oversampling
BARO::FILTER_X16, // Filtering
BARO::STANDBY_MS_1 // Standby Time
);
***/
temperature_calc_start = micros();
// temperature_calc_start = micros();
temperature = read_temperature();
temperature_calc_time = micros() - temperature_calc_start;
// temperature_calc_time = micros() - temperature_calc_start;
pressure_calc_start = micros();
// pressure_calc_start = micros();
pressure = read_pressure();
pressure_calc_time = micros() - pressure_calc_start;
// pressure_calc_time = micros() - pressure_calc_start;
altitude_calc_start = micros();
// altitude_calc_start = micros();
altitude = read_altitude();
altitude_calc_time = micros() - altitude_calc_start;
// altitude_calc_time = micros() - altitude_calc_start;
}
@ -289,3 +292,93 @@ inline void BARO::endTransaction() {
GPIO.out1_w1ts.val = ((uint32_t)1 << (BARO_CS - 32));
_SPI.endTransaction();
}
void BARO::baro_common(void) {
static int32_t baroHistTab[BARO_TAB_SIZE];
static uint8_t baroHistIdx;
uint8_t indexplus1 = (baroHistIdx + 1);
if(indexplus1 == BARO_TAB_SIZE)
indexplus1 = 0;
baroHistTab[baroHistIdx] = pressure;
baroPressureSum += baroHistTab[baroHistIdx];
baroPressureSum -= baroHistTab[indexplus1];
baroHistIdx = indexplus1;
}
uint8_t BARO::baro_update(void) {
if(currentTime < nextUpdate)
return 0;
get_readings();
baro_common();
nextUpdate = currentTime + 6000; // 1.5mS margin according to the spec (4.5mS T convertion time)
return 1;
}
uint8_t BARO::getEstimatedAltitude(){
int32_t BaroAlt;
static float baroGroundTemperatureScale,logBaroGroundPressureSum;
static float vel = 0.0f;
static uint16_t previousT;
uint16_t currentT = micros();
uint16_t dTime;
dTime = currentT - previousT;
if(dTime < UPDATE_INTERVAL)
return 0;
previousT = currentT;
if(calibratingB > 0) {
logBaroGroundPressureSum = log(baroPressureSum);
baroGroundTemperatureScale = ((int32_t) temperature + 27315) * (2 * 29.271267f); // 2 * is included here => no need for * 2 on BaroAlt in additional LPF
calibratingB--;
}
// baroGroundPressureSum is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = (logBaroGroundPressureSum - log(baroPressureSum)) * baroGroundTemperatureScale;
alt.EstAlt = (alt.EstAlt * 6 + BaroAlt) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
#if (defined(VARIOMETER) && (VARIOMETER != 2)) || !defined(SUPPRESS_BARO_ALTHOLD)
//P
int16_t error16 = constrain(AltHold - alt.EstAlt, -300, 300);
applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((conf.pid[PIDALT].P8 * error16 >> 7), -150, +150);
//I
errorAltitudeI += conf.pid[PIDALT].I8 * error16 >>6;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += errorAltitudeI >> 9; //I in range +/-60
applyDeadband(accZ, ACC_Z_DEADBAND);
static int32_t lastBaroAlt;
// could only overflow with a difference of 320m, which is highly improbable here
int16_t baroVel = ((alt.EstAlt - lastBaroAlt) * (1000000 / UPDATE_INTERVAL));
lastBaroAlt = alt.EstAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
// Integrator - velocity, cm/sec
vel += accZ * ACC_VelScale * dTime;
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * 0.985f + baroVel * 0.015f;
//D
alt.vario = vel;
applyDeadband(alt.vario, 5);
BaroPID -= constrain(conf.pid[PIDALT].D8 * alt.vario >>4, -150, 150);
#endif
return 1;
}

21
src/baro.h

@ -11,6 +11,20 @@
// SPI ADDRESS/BITS/SETTINGS
#define BMP280_CHIP_ID 0x58 // Default chip ID
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define BARO_TAB_SIZE 21
#define ACC_Z_DEADBAND (ACC_1G>>5) // was 40 instead of 32 now
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
// Registers available on the sensors
enum {
BMP280_REGISTER_DIG_T1 = 0x88,
@ -100,6 +114,7 @@ class BARO {
// inline void endTransaction();
bool begin(void);
void reset(void);
uint8_t getEstimatedAltitude(void);
bool baro_ok, baro_busy;
@ -114,6 +129,7 @@ class BARO {
float read_pressure(void);
float read_altitude(float sea_level_hpa = 1013.25);
void get_readings(void);
uint8_t baro_update(void);
void beginTransaction(void);
void endTransaction(void);
@ -125,6 +141,7 @@ class BARO {
uint16_t error_count = 0;
private:
int16_t accZ;
// Encapsulates the config registers
struct config {
config() : t_sb(STANDBY_MS_1), filter(FILTER_OFF), none(0), spi3w_en(0) {}
@ -150,6 +167,8 @@ class BARO {
ctrl_meas _measReg;
config _configReg;
uint32_t nextUpdate = 0;
int32_t t_fine, t_fine_temporary;
uint8_t read8(byte reg);
@ -161,6 +180,8 @@ class BARO {
void write8(byte reg, byte value);
void readCoefficients(void);
void baro_common(void);
};
#endif

9
src/config.h

@ -11,6 +11,15 @@
// in some cases, this value must be lowered down to 900 for some specific ESCs, otherwise they failed to initiate
#define MINCOMMAND 1000
// If baro exists then ACC = 1
#define USE_GYRO 1
#define USE_ACC 1
#define USE_MPU_DATA_READY_SIGNAL 1
// Enable baro
#define USE_BARO 1
#define ACC_1G 256
/* SECTION 2 - COPTER TYPE SPECIFIC OPTIONS
* === PID Controller ===
* choose one of the alternate PID control algorithms

117
src/gyro.cpp

@ -1,9 +1,5 @@
#include "baro.h"
int16_t gyroADCprevious[3] = {0, 0, 0};
int16_t gyroADCinter[3];
uint16_t timeInterleave = 0;
GYRO::GYRO(void) {
// Switch the baro to SPI
// If CSB is pulled down, the SPI interface is activated. After
@ -49,6 +45,7 @@ void GYRO::begin(void) {
// _SPI.begin();
// digitalWrite(GYRO_CS, LOW);
vTaskDelay(100);
GPIO.out_w1tc = ((uint32_t)1 << GYRO_CS);
delayMicroseconds(10);
// vTaskDelay(5 / portTICK_PERIOD_MS);
@ -117,16 +114,110 @@ uint8_t GYRO::read_register(uint8_t addr) {
}
void GYRO::compute_imu(void) {
uint8_t axis;
timeInterleave = 0;
bool GYRO::write_register(uint8_t addr, uint8_t data) {
beginTransaction();
_SPI.transfer(addr);
_SPI.transfer(data);
endTransaction();
return true;
}
void GYRO::acc_gyro_init(void) {
write_register(PWR_MGMT_1, BIT_RESET); // 0x80 ~ 1000 0000
delay(100);
write_register(SIGNAL_PATH_RESET, 0x07); // BIT_GYRO | BIT_ACC | BIT_TEMP
delay(100); // For ICM-20602 seems like only BIT_ACC | BIT_TEMP
write_register(PWR_MGMT_1, 0x00);
delay(100);
write_register(PWR_MGMT_1, INV_CLK_PLL);
delay(15);
write_register(GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED);
delay(15);
write_register(ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
write_register(CONFIG, 0x07); // 7:0 6:0 5:0 4:0 3:0 2:1 1:1 0:1
delay(15);
write_register(SMPLRT_DIV, 0x00);
delay(100);
// Data ready interrupt configuratio
// write_register(INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
write_register(INT_PIN_CFG, 0x12); // INT_ANYRD_2CLEAR, BYPASS_EN ~ 0001 0010
delay(15);
#if ACC
ACC_getADC();
getEstimatedAttitude();
#ifdef USE_MPU_DATA_READY_SIGNAL
write_register(INT_ENABLE, BIT_RAW_RDY_EN);
delay(15);
#endif
}
void GYRO::read_all(void) {
AcX = read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L);
AcY = read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L);
AcZ = read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L);
GyX = read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L);
GyY = read_register(GYRO_YOUT_H) << 8 | read_register(GYRO_YOUT_L);
GyZ = read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L);
}
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void GYRO::rotateV32(t_int32_t_vector *v, int16_t* delta) {
int16_t X = v->V16.X;
int16_t Y = v->V16.Y;
int16_t Z = v->V16.Z;
#if GYRO
Gyro_getADB();
#endif
v->V32.Z -= (delta[ROLL] * X) + (delta[PITCH] * Y);
v->V32.X += (delta[ROLL] * Z) - (delta[YAW] * Y);
v->V32.Y += (delta[PITCH] * Z) + (delta[YAW] * X);
}
//return angle , unit: 1/10 degree
int16_t GYRO::_atan2(int32_t y, int32_t x){
float z = y;
int16_t a;
uint8_t c;
c = abs(y) < abs(x);
if(c){
z = z / x;
} else {
z = x / z;
}
a = 2046.43 * (z / (3.5714 + z * z));
if(c){
if(x<0){
if(y<0)
a -= 1800;
else
a += 1800;
}
} else {
a = 900 - a;
if(y<0)
a -= 1800;
}
return a;
}
float GYRO::InvSqrt(float x){
union {
int32_t i;
float f;
} conv;
conv.f = x;
conv.i = 0x5f1ffff9 - (conv.i >> 1);
return conv.f * (1.68191409f - 0.703952253f * x * conv.f * conv.f);
}

94
src/gyro.h

@ -21,10 +21,15 @@
*
*/
#if !defined(GYRO_ORIENTATION)
#define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = X; imu.gyroADC[PITCH] = Y; imu.gyroADC[YAW] = Z;}
#endif
#if !defined(ACC_ORIENTATION)
#define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = X; imu.accADC[PITCH] = Y; imu.accADC[YAW] = Z;}
#endif
// Registry Values
#define I2C_IF_DIS 0x40
#define ACC_VelScale (9.80665f / 10000.0f / ACC_1G)
// Registry Address
// #define XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
@ -113,16 +118,99 @@
#define MPU_DLPF_188HZ 0x01
#define MPU_DLPF_256HZ 0x00
// Registry Values
#define I2C_IF_DIS 0x40
#define BIT_RESET 0x80
// #define BIT_INT_ANYRD_2CLEAR (1 << 4)
// #define MPU6500_BIT_BYPASS_EN (1 << 0)
// #define MPU6500_BIT_I2C_IF_DIS (1 << 4)
#define BIT_RAW_RDY_EN 0x01
typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
typedef struct __attribute__ ((__packed__)) mpuContextData_s {
uint16_t chipMagicNumber;
uint8_t lastReadStatus;
uint8_t __padding;
uint8_t accRaw[6]; // MPU_RA_ACCEL_XOUT_H
uint8_t tempRaw[2]; // MPU_RA_TEMP_OUT_H
uint8_t gyroRaw[6]; // MPU_RA_GYRO_XOUT_H
} mpuContextData_t;
typedef struct {
int32_t X,Y,Z;
} t_int32_t_vector_def;
typedef struct {
uint16_t XL; int16_t X;
uint16_t YL; int16_t Y;
uint16_t ZL; int16_t Z;
} t_int16_t_vector_def;
// note: we use implicit first 16 MSB bits 32 -> 16 cast. ie V32.X>>16 = V16.X
typedef union {
int32_t A32[3];
t_int32_t_vector_def V32;
int16_t A16[6];
t_int16_t_vector_def V16;
} t_int32_t_vector;
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
enum fchoice_b {
FCB_DISABLED = 0,
FCB_8800_32,
FCB_3600_32
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
struct gyroDev_s;
struct accDev_s;
class GYRO {
public:
GYRO(void);
void begin(void);
void acc_gyro_init(void);
uint8_t get_id(void);
uint16_t get_readings(void);
void beginTransaction(void);
void endTransaction(void);
uint8_t read_register(uint8_t addr);
void compute_imu(void);
bool write_register(uint8_t addr, uint8_t data);
void read_all(void);
void rotateV32(t_int32_t_vector *v, int16_t* delta);
float InvSqrt(float x);
int16_t _atan2(int32_t y, int32_t x);
private:
};

244
src/main.cpp

@ -1,5 +1,4 @@
#include "Arduino.h"
#include "config.h"
#include "main.h"
BATTERY battery;
@ -7,6 +6,7 @@ BARO baro;
NRF24L01 rx(RX_CE_PIN, RX_CSN_PIN); // Setting up receiver module
GYRO gyro;
WIFI wifi;
STORAGE storage;
WEB web;
// TASK task;
@ -19,10 +19,12 @@ uint16_t rcCommand[RC_CHANS]; // Signal to ESC
uint16_t rcData[RC_CHANS]; // Signal from nRF24L01+
int16_t rcSerial[8]; // Signal from Serial Port
uint16_t failsafeCnt = 0; // To trigger failsave if any issues
int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCinter[3];
uint32_t timeInterleave = 0;
flags_struct_t flags;
// Common variables
uint8_t rcDelayCommand; // This indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
uint8_t rcSticks; // This hold sticks position for command combos
@ -32,6 +34,8 @@ int16_t delta;
int16_t PTerm = 0, ITerm = 0, DTerm, PTermACC, ITermACC;
int16_t lastGyro[2] = {0, 0};
int16_t errorAngleI[2] = {0, 0};
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
#if PID_CONTROLLER == 1
int32_t errorGyroI_YAW;
int16_t delta1[2], delta2[2];
@ -148,7 +152,7 @@ mission_step_struct mission_step;
int16_t nav[2];
int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
#if BARO
#if USE_BARO
int32_t baroPressure;
int16_t baroTemperature;
int32_t baroPressureSum;
@ -158,6 +162,7 @@ alt_t alt;
att_t att;
imu_t imu;
global_conf_t global_conf;
conf_t conf;
static uint8_t dynP8[2], dynD8[2];
@ -169,11 +174,21 @@ int8_t cosZ = 100; // cos(angleZ)*100
int16_t axisPID[3];
int16_t motor[8];
int16_t gyroZero[3] = {0, 0, 0};
// Normal Setup Loop
void setup() {
Serial.begin(115200);
// Starting up storage
if(!storage.begin()) {
Serial.println("Storage need to initialize.");
// return;
} else {
Serial.println("Storage OK");
}
// Start up global SPI protocol
_SPI.begin();
@ -183,12 +198,15 @@ void setup() {
battery.get_reading();
Serial.print("BATTERY(VOLTS): "); Serial.print(battery.volt); Serial.println("V");
baro.begin();
// baro.begin();
gyro.begin();
// gyro.get_id();
gyro.acc_gyro_init();
if(!baro.begin()) {
Serial.println("BARO: FAILED");
}
gyro.get_id();
rx.begin();
// Serial.print("FREE RAM: "); Serial.print(ESP.getFreeHeap()); Serial.println(" MB");
@ -221,7 +239,7 @@ void setup() {
"TaskWebHandler", // Name of the process
4096, // This stack size can be checked & adjusted by reading the Stack HighWater
NULL,
4, // Priority with 3 (configMAX_PRIORITIES - 1) being the highest and 0 being the lowest
8, // Priority with 3 (configMAX_PRIORITIES - 1) being the highest and 0 being the lowest
NULL,
APP_CPU
);
@ -246,6 +264,7 @@ void setup() {
PRO_CPU
);
/****
xTaskCreatePinnedToCore(
taskDebug,
"TaskDebug", // Name of the process
@ -255,6 +274,7 @@ void setup() {
NULL,
APP_CPU
);
****/
xTaskCreatePinnedToCore(
taskMain,
@ -429,6 +449,7 @@ void taskMeasurements(void *pvParameters) {
}
}
/*****
void taskDebug(void *pvParameters) {
(void) pvParameters;
@ -456,7 +477,7 @@ void taskDebug(void *pvParameters) {
}
}
}
****/
// Main Task of the system
void taskMain(void *pvParameters) {
@ -495,7 +516,7 @@ void run_tasks(void) {
previousTime = currentTime;
// Compute IMU
gyro.compute_imu();
compute_imu();
// Experimental FlightModes
#if defined(ACROTRAINER_MODE)
@ -618,9 +639,9 @@ void run_tasks(void) {
// SerialWrite16(2,nav[LAT]+rcCommand[PITCH]);
// SerialWrite16(2,nav[LON]+rcCommand[ROLL]);
// SerialWrite16(2,(nav[LAT]+rcCommand[PITCH])-(nav[LON]+rcCommand[ROLL])); //check
#endif
#endif // End of GPS_SIMULATOR
#endif //GPS
#endif // End of GPS
// **** PITCH & ROLL & YAW PID ****
#if PID_CONTROLLER == 1 // evolved oldschool
@ -847,5 +868,208 @@ void process_rc(void) {
}
void compute_imu(void) {
timeInterleave = 0;
baro.getEstimatedAltitude();
// Reading ACC and Gyro values
gyro.read_all();
// IMU.cpp at multiwii....
ACC_ORIENTATION(AcX, AcY, AcZ);
acc_common();
GYRO_ORIENTATION(GyX, GyY, GyZ); // Range +/- 8192; +/- 2000 deg/sec?
gyro_common();
}
void gyro_common(void) {
static int16_t previousGyroADC[3] = {0, 0, 0};
static int32_t g[3];
uint8_t axis, tilt = 0;
#if defined MMGYRO
// Moving Average Gyros by Magnetron1
//---------------------------------------------------
static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH];
static int32_t mediaMobileGyroADCSum[3];
static uint8_t mediaMobileGyroIDX;
//---------------------------------------------------
#endif
if(calibratingG > 0) {
for(axis = 0; axis < 3; axis++) {
if(calibratingG == 512) { // Reset g[axis] at start of calibration
g[axis]=0;
#if defined(GYROCALIBRATIONFAILSAFE)
previousGyroADC[axis] = imu.gyroADC[axis];
}
if(calibratingG % 10 == 0) {
if(abs(imu.gyroADC[axis] - previousGyroADC[axis]) > 8)
tilt = 1;
previousGyroADC[axis] = imu.gyroADC[axis];
#endif
}
g[axis] += imu.gyroADC[axis]; // Sum up 512 readings
gyroZero[axis]=g[axis] >> 9;
if(calibratingG == 1) {
// SET_ALARM_BUZZER(ALRM_FAC_CONFIRM, ALRM_LVL_CONFIRM_ELSE);
}
}
#if defined(GYROCALIBRATIONFAILSAFE)
if(tilt) {
calibratingG = 1000;
// LEDPIN_ON;
} else {
calibratingG--;
// LEDPIN_OFF;
}
return;
#else
calibratingG--;
#endif
}
#ifdef MMGYRO
mediaMobileGyroIDX = ++mediaMobileGyroIDX % conf.mmgyro;
for(axis = 0; axis < 3; axis++) {
imu.gyroADC[axis] -= gyroZero[axis];
mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX];
// anti gyro glitch, limit the variation between two consecutive readings
mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX];
imu.gyroADC[axis] = mediaMobileGyroADCSum[axis] / conf.mmgyro;
#else
for(axis = 0; axis < 3; axis++) {
imu.gyroADC[axis] -= gyroZero[axis];
// anti gyro glitch, limit the variation between two consecutive readings
imu.gyroADC[axis] = constrain(imu.gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
#endif
previousGyroADC[axis] = imu.gyroADC[axis];
}
#if defined(SENSORS_TILT_45DEG_LEFT)
int16_t temp = ((imu.gyroADC[PITCH] - imu.gyroADC[ROLL]) * 7) / 10;
imu.gyroADC[ROLL] = ((imu.gyroADC[ROLL] + imu.gyroADC[PITCH]) * 7) / 10;
imu.gyroADC[PITCH] = temp;
#endif
#if defined(SENSORS_TILT_45DEG_RIGHT)
int16_t temp = ((imu.gyroADC[PITCH] + imu.gyroADC[ROLL]) * 7) / 10;
imu.gyroADC[ROLL] = ((imu.gyroADC[ROLL] - imu.gyroADC[PITCH]) * 7) / 10;
imu.gyroADC[PITCH] = temp;
#endif
}
void acc_common(void) {
static int32_t a[3];
if(calibratingA > 0){
calibratingA--;
for(uint8_t axis = 0; axis < 3; axis++){
if(calibratingA == 511)
a[axis]=0; // Reset a[axis] at start of calibration
a[axis] += imu.accADC[axis]; // Sum up 512 readings
global_conf.accZero[axis] = a[axis]>>9; // Calculate average, only the last itteration where (calibratingA == 0) is relevant
}
if(calibratingA == 0){
global_conf.accZero[YAW] -= ACC_1G; // shift Z down by ACC_1G and store values in EEPROM at end of calibration
conf.angleTrim[ROLL] = 0;
conf.angleTrim[PITCH] = 0;
writeGlobalSet(1); // write accZero in EEPROM
}
}
#if defined(INFLIGHT_ACC_CALIBRATION)
static int32_t b[3];
static int16_t accZero_saved[3] = {0,0,0};
static int16_t angleTrim_saved[2] = {0, 0};
// Saving old zeropoints before measurement
if(InflightcalibratingA == 50){
accZero_saved[ROLL] = global_conf.accZero[ROLL] ;
accZero_saved[PITCH] = global_conf.accZero[PITCH];
accZero_saved[YAW] = global_conf.accZero[YAW] ;
angleTrim_saved[ROLL] = conf.angleTrim[ROLL] ;
angleTrim_saved[PITCH] = conf.angleTrim[PITCH] ;
}
if(InflightcalibratingA > 0){
for(uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if(InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += imu.accADC[axis];
// Clear global variables for next reading
imu.accADC[axis] = 0;
global_conf.accZero[axis]=0;
}
// all values are measured
if(InflightcalibratingA == 1){
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
SET_ALARM_BUZZER(ALRM_FAC_CONFIRM, ALRM_LVL_CONFIRM_1); // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred
global_conf.accZero[ROLL] = accZero_saved[ROLL] ;
global_conf.accZero[PITCH] = accZero_saved[PITCH];
global_conf.accZero[YAW] = accZero_saved[YAW] ;
conf.angleTrim[ROLL] = angleTrim_saved[ROLL] ;
conf.angleTrim[PITCH] = angleTrim_saved[PITCH] ;
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by ACC_1G and store values in EEPROM at end of calibration
if(AccInflightCalibrationSavetoEEProm == 1){ // the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
global_conf.accZero[ROLL] = b[ROLL] / 50;
global_conf.accZero[PITCH] = b[PITCH] / 50;
global_conf.accZero[YAW] = b[YAW] / 50 - ACC_1G;
conf.angleTrim[ROLL] = 0;
conf.angleTrim[PITCH] = 0;
writeGlobalSet(1); // write accZero in EEPROM
}
#endif
imu.accADC[ROLL] -= global_conf.accZero[ROLL] ;
imu.accADC[PITCH] -= global_conf.accZero[PITCH];
imu.accADC[YAW] -= global_conf.accZero[YAW] ;
#if defined(SENSORS_TILT_45DEG_LEFT)
int16_t temp = ((imu.accADC[PITCH] - imu.accADC[ROLL]) * 7) / 10;
imu.accADC[ROLL] = ((imu.accADC[ROLL] + imu.accADC[PITCH])*7)/10;
imu.accADC[PITCH] = temp;
#endif
#if defined(SENSORS_TILT_45DEG_RIGHT)
int16_t temp = ((imu.accADC[PITCH] + imu.accADC[ROLL] )*7) / 10;
imu.accADC[ROLL] = ((imu.accADC[ROLL] - imu.accADC[PITCH])*7) / 10;
imu.accADC[PITCH] = temp;
#endif
}
void process_modules(void) {
}
uint8_t calculate_sum(uint8_t *cb, uint8_t siz) {
uint8_t sum=0x55; // checksum init
while(--siz) sum += *cb++; // calculate checksum (without checksum byte)
return sum;
}
void writeGlobalSet(uint8_t b) {
global_conf.checksum = calculate_sum((uint8_t*)&global_conf, sizeof(global_conf));
// eeprom_write_block((const void*)&global_conf, (void*)0, sizeof(global_conf));
storage.write_block((uint8_t*)&global_conf, "/globalset", (uint32_t) sizeof(global_conf));
// if(b == 1)
// blinkLED(15,20,1);
// SET_ALARM_BUZZER(ALRM_FAC_CONFIRM, ALRM_LVL_CONFIRM_1);
}

58
src/main.h

@ -106,7 +106,7 @@ typedef struct {
uint8_t VARIO_MODE :1;
#endif
uint8_t GPS_mode: 2; // 0-3 NONE,HOLD, HOME, NAV (see GPS_MODE_* defines
#if BARO || GPS
#if USE_BARO || GPS
uint8_t THROTTLE_IGNORED : 1; // If it is 1 then ignore throttle stick movements in baro mode;
#endif
#if GPS
@ -126,7 +126,7 @@ enum box {
BOXANGLE,
BOXHORIZON,
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
#if USE_BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
BOXBARO,
#endif
#ifdef VARIOMETER
@ -185,6 +185,15 @@ struct servo_conf_ { // this is a generic way to configure a servo, every mul
};
typedef struct {
uint8_t currentSet;
int16_t accZero[3];
int16_t magZero[3];
uint16_t flashsum;
uint8_t checksum; // MUST BE ON LAST POSITION OF STRUCTURE !
} global_conf_t;
enum pid {
PIDROLL,
PIDPITCH,
@ -414,16 +423,6 @@ typedef struct {
#endif
// Headers
#include "wifi.h"
#include "dshot.h"
#include "Motors.h"
#include "battery.h"
#include "nrf24l01.h"
#include "baro.h"
#include "gyro.h"
#include "web.h"
// #include "task.h"
// Task scheduler
#define APP_CPU 1
@ -437,11 +436,16 @@ void taskBatteryMonitor(void *pvParameters);
void taskMeasurements(void *pvParameters);
void taskMain(void *pvParameters); // Main task
void taskDebug(void *pvParameters); // <-- Only for debugging purposes. Will be disabled when done.
/// void taskDebug(void *pvParameters); // <-- Only for debugging purposes. Will be disabled when done.
void compute_imu();
void process_rc(void);
void process_modules(void);
void run_tasks(void);
void acc_common(void);
void gyro_common(void);
uint8_t calculate_sum(uint8_t *cb , uint8_t siz);
void writeGlobalSet(uint8_t b);
/***
void taskReceiver(void *pvParameters);
@ -464,4 +468,32 @@ extern int16_t motor[8];
extern int16_t axisPID[3];
extern conf_t conf;
extern uint16_t failsafeCnt;
extern imu_t imu;
extern int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
extern int32_t baroPressure;
extern int16_t baroTemperature;
extern int32_t baroPressureSum;
extern uint32_t currentTime;
extern uint16_t calibratingB;
extern alt_t alt;
extern uint32_t AltHold; // in cm
extern int16_t BaroPID;
extern int16_t errorAltitudeI;
extern global_conf_t global_conf;
extern int16_t gyroZero[3];
// Headers
#include "wifi.h"
#include "dshot.h"
#include "Motors.h"
#include "battery.h"
#include "nrf24l01.h"
#include "baro.h"
#include "gyro.h"
#include "storage.h"
#include "web.h"
// #include "task.h"
#endif

201
src/storage.cpp

@ -0,0 +1,201 @@
#include "storage.h"
STORAGE::STORAGE(void) {
// Just some initialization
}
bool STORAGE::format(void) {
if(LITTLEFS.format()) {
return true;
} else {
return false;
}
}
bool STORAGE::exists(const char * path) {
if(LITTLEFS.exists(path)) {
return true;
} else {
return false;
}
}
bool STORAGE::begin(void) {
if(!LITTLEFS.begin(FORMAT_LITTLEFS_IF_FAILED)) {
Serial.println("LITTLEFS mount failed");
return false;
}
// Demo and checking only
STORAGE::listDir(LITTLEFS, "/", 0);
if(STORAGE::exists("/config")) {
Serial.println("config FOUND");
} else {
Serial.println("config NOT FOUND");
}
// Check Total storage
Serial.print("Storage size: ");
Serial.print(LITTLEFS.totalBytes());
Serial.println(" Bytes");
Serial.print("Storage used: " );
Serial.print(LITTLEFS.usedBytes());
Serial.println(" Bytes");
return true;
}
void STORAGE::listDir(fs::FS &fs, const char * dirname, uint8_t levels){
Serial.printf("Listing directory: %s\r\n", dirname);
File root = fs.open(dirname);
if(!root){
Serial.println("- failed to open directory");
return;
}
if(!root.isDirectory()){
Serial.println(" - not a directory");
return;
}
File file = root.openNextFile();
while(file){
if(file.isDirectory()){
Serial.print(" DIR : ");
#ifdef CONFIG_LITTLEFS_FOR_IDF_3_2
Serial.println(file.name());
#else
Serial.print(file.name());
time_t t= file.getLastWrite();
struct tm * tmstruct = localtime(&t);
Serial.printf(" LAST WRITE: %d-%02d-%02d %02d:%02d:%02d\n",(tmstruct->tm_year)+1900,( tmstruct->tm_mon)+1, tmstruct->tm_mday,tmstruct->tm_hour , tmstruct->tm_min, tmstruct->tm_sec);
#endif
if(levels){
listDir(fs, file.name(), levels -1);
}
} else {
Serial.print(" FILE: ");
Serial.print(file.name());
Serial.print(" SIZE: ");
#ifdef CONFIG_LITTLEFS_FOR_IDF_3_2
Serial.println(file.size());
#else
Serial.print(file.size());
time_t t= file.getLastWrite();
struct tm * tmstruct = localtime(&t);
Serial.printf(" LAST WRITE: %d-%02d-%02d %02d:%02d:%02d\n",(tmstruct->tm_year)+1900,( tmstruct->tm_mon)+1, tmstruct->tm_mday,tmstruct->tm_hour , tmstruct->tm_min, tmstruct->tm_sec);
#endif
}
file = root.openNextFile();
}
}
void STORAGE::readFile(fs::FS &fs, const char * path){
Serial.printf("Reading file: %s\r\n", path);
File file = fs.open(path);
if(!file || file.isDirectory()){
Serial.println("- failed to open file for reading");
return;
}
Serial.println("- read from file:");
while(file.available()){
Serial.write(file.read());
}
file.close();
}
void STORAGE::appendFile(fs::FS &fs, const char * path, const char * message){
Serial.printf("Appending to file: %s\r\n", path);
File file = fs.open(path, FILE_APPEND);
if(!file){
Serial.println("- failed to open file for appending");
return;
}
if(file.print(message)){
Serial.println("- message appended");
} else {
Serial.println("- append failed");
}
file.close();
}
void STORAGE::deleteFile(fs::FS &fs, const char * path){
Serial.printf("Deleting file: %s\r\n", path);
if(fs.remove(path)){
Serial.println("- file deleted");
} else {
Serial.println("- delete failed");
}
}
void STORAGE::writeFile(fs::FS &fs, const char * path, const char * message){
Serial.printf("Writing file: %s\r\n", path);
File file = fs.open(path, FILE_WRITE);
if(!file){
Serial.println("- failed to open file for writing");
return;
}
if(file.print(message)){
Serial.println("- file written");
} else {
Serial.println("- write failed");
}
file.close();
}
void STORAGE::write_block(uint8_t * data, const char * path, uint32_t len){
uint32_t i;
fs::FS &fs = LITTLEFS;
File file = fs.open(path, FILE_APPEND);
if(!file) {
Serial.println("write_block append failed");
return;
}
for(i = 0; i < len; i++){
// EEPROM.write(address+i, data[i]);
file.print(data[i]);
}
file.close();
}
/****
268 writeFile(LITTLEFS, "/mydir/hello2.txt", "Hello2");
void eeprom_read_block(uint8_t * data, uint32_t address, size_t len){
int i;
for(i=0; i<len; i++){
data[i] = EEPROM.read(address+i);
}
}
void eeprom_write_block(uint8_t * data, uint32_t address, size_t len){
int i;
for(i=0; i<len; i++){
EEPROM.write(address+i, data[i]);
}
EEPROM.commit();
}
****/

39
src/storage.h

@ -0,0 +1,39 @@
#ifndef _STORAGE_H
#define _STORAGE_H
#include "Arduino.h"
#include "main.h"
#include "FS.h"
#include <LITTLEFS.h>
#ifndef CONFIG_LITTLEFS_FOR_IDF_3_2
#include <time.h>
#endif
/* You only need to format LITTLEFS the first time you run a
test or else use the LITTLEFS plugin to create a partition
https://github.com/lorol/arduino-esp32littlefs-plugin */
#define FORMAT_LITTLEFS_IF_FAILED true
class STORAGE {
public:
STORAGE(void);
bool begin(void);
void listDir(fs::FS &fs, const char * dirname, uint8_t levels);
void readFile(fs::FS &fs, const char * path);
void appendFile(fs::FS &fs, const char * path, const char * message);
void deleteFile(fs::FS &fs, const char * path);
void writeFile(fs::FS &fs, const char * path, const char * message);
// Emulate EEPROM write to LITTLEFS
void write_block(uint8_t * data, const char * path, uint32_t len);
bool format(void);
bool exists(const char * path);
private:
};
#endif
Loading…
Cancel
Save