From 8f52f7085c01950d04e5ac81bb7349f5535ff70c Mon Sep 17 00:00:00 2001 From: qqqlab <46283638+qqqlab@users.noreply.github.com> Date: Sun, 24 Nov 2024 16:51:10 +0100 Subject: [PATCH] Add BMP390, add calradio, cleanup examples --- README.md | 35 +- examples/01.Quadcopter/01.Quadcopter.ino | 542 +-- examples/01.Quadcopter/madflight_config.h | 127 + .../02.QuadcopterAdvanced.ino | 623 +--- .../02.QuadcopterAdvanced/madflight_config.h | 127 + examples/03.PID-Tune/03.PID-Tune.ino | 775 ----- examples/03.PID-Tune/pidtune.h | 142 - examples/03.Plane/03.Plane.ino | 462 +++ examples/03.Plane/madflight_config.h | 127 + examples/04.Plane/04.Plane.ino | 793 ----- extras/TestMadflight/baro/baro.ino | 36 + extras/TestMadflight/cfg.ino | 4 +- extras/TestMadflight/rcin/rcin.ino | 3 +- .../TestMadflight/rp2040_crsf/rp2040_crsf.ino | 6 +- src/madflight.h | 100 +- src/madflight/ahrs/ahrs.h | 24 +- src/madflight/baro/baro.h | 75 +- src/madflight/baro/bmp3/LICENSE.md | 44 + src/madflight/baro/bmp3/README.md | 181 + src/madflight/baro/bmp3/bmp3.cpp | 327 ++ src/madflight/baro/bmp3/bmp3.h | 123 + src/madflight/baro/bmp3/bst/bmp3.c | 2960 +++++++++++++++++ src/madflight/baro/bmp3/bst/bmp3.h | 552 +++ src/madflight/baro/bmp3/bst/bmp3_defs.h | 920 +++++ src/madflight/bat/BatteryADC.h | 8 +- src/madflight/bat/bat.h | 2 +- src/madflight/bb/bb_sdcard/bb_sdcard.h | 64 +- src/madflight/cfg/cfg.h | 233 +- src/madflight/cli/cli.h | 219 +- src/madflight/imu/imu.h | 38 +- src/madflight/interface.h | 108 +- src/madflight/led/led.h | 32 +- src/madflight/out/out.h | 59 + src/madflight/rcin/rcin.h | 344 +- src/madflight/rcin/rcin_calibrate.h | 227 ++ src/madflight/rcin/telem.h | 95 + src/madflight_board_custom_DYST-DYSF4PRO_V2.h | 4 +- 37 files changed, 7548 insertions(+), 2993 deletions(-) create mode 100644 examples/01.Quadcopter/madflight_config.h create mode 100644 examples/02.QuadcopterAdvanced/madflight_config.h delete mode 100644 examples/03.PID-Tune/03.PID-Tune.ino delete mode 100644 examples/03.PID-Tune/pidtune.h create mode 100644 examples/03.Plane/03.Plane.ino create mode 100644 examples/03.Plane/madflight_config.h delete mode 100644 examples/04.Plane/04.Plane.ino create mode 100644 extras/TestMadflight/baro/baro.ino create mode 100644 src/madflight/baro/bmp3/LICENSE.md create mode 100644 src/madflight/baro/bmp3/README.md create mode 100644 src/madflight/baro/bmp3/bmp3.cpp create mode 100644 src/madflight/baro/bmp3/bmp3.h create mode 100644 src/madflight/baro/bmp3/bst/bmp3.c create mode 100644 src/madflight/baro/bmp3/bst/bmp3.h create mode 100644 src/madflight/baro/bmp3/bst/bmp3_defs.h create mode 100644 src/madflight/out/out.h create mode 100644 src/madflight/rcin/rcin_calibrate.h create mode 100644 src/madflight/rcin/telem.h diff --git a/README.md b/README.md index c27753c..a383e27 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ -**madflight** is an Arduino library to build ESP32 / ESP32-S3 / RP2350 / RP2040 / STM32 flight controllers. A functional DIY flight controller can be build for under $10 from readily available [development boards](https://madflight.com/Controller-Boards/) and [sensor breakout boards](https://madflight.com/Sensor-Boards/). Ideal if you want to try out new flight control concepts, without first having to setup a build environment and without having to read through thousands lines of code to find the spot where you want to change something. +**madflight** is a toolbox to build high performance flight controllers with Aduino IDE or PlatformIO for ESP32-S3 / ESP32 / RP2350 / RP2040 / STM32. A functional DIY flight controller can be build for under $10 from readily available [development boards](https://madflight.com/Controller-Boards/) and [sensor breakout boards](https://madflight.com/Sensor-Boards/). -`Quadcopter.ino` is a 1000 line demo program for a quadcopter. It has been flight tested on ESP32, ESP32-S3, RP2350, RP2040, and STM32F405 microcontrollers with the Arduino IDE. The program can be easily adapted to control your plane or VTOL craft. The source code has extensive documentation explaning what the settings and functions do. +Flight tested example programs for quadcopter and airplane are included. The example programs are only a couple hundred lines long, but contain the full flight controller logic. The nitty-gritty low-level sensor and input/output management is done by the madflight library. + +The source code and [website](https://madflight.com/) have extensive documentation explaning what the settings and functions do. ---

If you like madflight, give it a ☆ star, or fork it and contribute!

@@ -40,13 +42,13 @@ - Connect your radio receiver according to the configured pins. 2. Install the madflight library in Arduino IDE. (Menu *Tools->Manage Libraries*, then search for **madflight**) 3. Open *Examples for custom libraries->madflight->Quadcopter.ino* in the Arduino IDE. -4. If you're not using the default pinout then setup your board pinout in the CUSTOM PINS section. -5. Edit the HARDWARE section to enable the connected peripherals. -6. Edit the RC RECEIVER section. Either match you RC equipment to the settings, or change the settings to match your RC equipment. -7. Compile Quadcopter.ino and upload it to your board. Connect the Serial Monitor at 115200 baud and check the messages. Type `help` to see the available CLI commands. +4. Edit the HARDWARE section in madflight_config.h to enable the connected peripherals. +5. If you're not using the default pinout then setup your board pinout in the CUSTOM PINS section. +6. Compile Quadcopter.ino and upload it to your board. Connect the Serial Monitor at 115200 baud and check the messages. Type `help` to see the available CLI commands. +7. Type `calradio` and follow the prompts to setup your RC radio receiver. 8. IMPORTANT: Use CLI `calimu` and `calmag` to calibate the sensors. -9. Use CLI print commands like `pimu`, `pahrs`, `pradio` to check that IMU sensor, AHRS and RC Receiver are working correctly. -10. Connect motors (no props) and battery and check that motors are spinning correctly. For debugging, use CLI `pmot` to show motor output. +9. Use CLI commands `pimu`, `pahrs`, `pradio`, `pmot`, etc. and check that IMU sensor, AHRS and RC Receiver are working correctly. +10. Connect motors (no props) and battery and check that motors are spinning correctly. 11. Mount props, go to an wide open space, and FLY! ## Safety First!!! @@ -69,18 +71,19 @@ By default **madflight** has these safety features enabled: - The madflight flight controller runs standard `setup()` and `loop()`. - It mainly uses plain Arduino functionality: Serial, Wire, and SPI. One custom hardware dependent library is used for PWM. Therefor, it can fairly easily ported to other 32 bit microcontrollers that support the Arduino framework. Also porting to other build environments like PlatformIO or CMake should not be a huge effort. - The following modules are used: - - `imu` Inertial Measurement Unit, retrieves accelerometer, gyroscope, and magnetometer sensor data - `ahrs` Attitude Heading Reference System, estimates roll, yaw, pitch - - `rcin` RC INput, retrieves RC receiver data - - `control` PID controller and output mixer - - `out` Output to motors and servos - - `mag` Magnetometer (external) - - `baro` Barometer - - `gps` GPS receiver + - `baro` Barometer sensor + - `bat` Battery monitor - `bb` Black Box data logger - - `cli` Command Line Interface for debugging, configuration and calibration - `cfg` Read and save configuration to flash + - `cli` Command Line Interface for debugging, configuration and calibration + - `gps` GPS receiver - `hw` Hardware specific code for STM32, RP2040 and ESP32 + - `imu` Inertial Measurement Unit, retrieves accelerometer, gyroscope, and magnetometer sensor data + - `led` LED driver + - `mag` Magnetometer sensor (external) + - `out` Output to motors and servos + - `rcin` RC INput, retrieves RC receiver data - Most modules are interfaced through a global object, for example the `imu` object has property `imu.gx` which is the current gyro x-axis rate in degrees per second for the selected IMU chip. - For a quick overview of the objects, see header `src/madflight/interfaces.h` which defines the module interfaces. - The module implementations are in subdirectories of the `src/madflight` directory. Here you find the module header file, e.g. `src/madflight/imu/imu.h`. In the `extras` directory your find test programs for the modules, e.g. `extras/TestMadflight/imu.ino`. diff --git a/examples/01.Quadcopter/01.Quadcopter.ino b/examples/01.Quadcopter/01.Quadcopter.ino index 20e1079..9da6af4 100644 --- a/examples/01.Quadcopter/01.Quadcopter.ino +++ b/examples/01.Quadcopter/01.Quadcopter.ino @@ -1,6 +1,6 @@ /*######################################################################################################################### -Minimal quadcopter demo program for madflight Arduino ESP32 / RP2040 / STM32 Flight Controller +Minimal quadcopter demo program for madflight Arduino ESP32-S3 / ESP32 / RP2350 / RP2040 / STM32 Flight Controller ########################################################################################################################### @@ -9,26 +9,13 @@ See http://madflight.com for detailed description Needs: - IMU sensor (SPI or I2C) - RC receiver with 5 channels (CRSF/ELRS preferred) - - 4 brushless motors with ESCs + - 4 brushless motors with ESCs (or brushed motors with MOSFETs) Connecting: IMU: connect IMU_EXTI, IMU_CS, SPI_MISO, SPI_MOSI, SPI_CLK for SPI (or IMU_EXTI, I2C_SDA, I2C_SCL for I2C) RC receiver: connect RCIN_RX to receiver TX pin ESCs: PWM1-4 to the ESC inputs -Motor order diagram (Betaflight order) - - front - CW --> <-- CCW - 4 2 - \ ^ / - |X| - / - \ - 3 1 -CCW --> <-- CW - -Default flight mode is ACRO (rate). The mode can be changed to ANGLE by changing the PID controller in imu_loop(). Important: calibrate the accelometer before using ANGLE. - Arming: Set throttle low, then flip arm switch from DISARMED to ARMED. Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch". @@ -45,220 +32,63 @@ MIT license Copyright (c) 2023-2024 https://madflight.com ##########################################################################################################################*/ - -//========================================================================================================================// -// PINS // -//========================================================================================================================// -// -// You have 3 options to setup the pins (gpio numbers) for the flight controller: -// -// 1) Default - Leave this section as is and see https://madflight.com for default pinout diagrams for the supported -// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h -// -// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available -// boards. For example: #include -// -// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. -// -//========================================================================================================================// - -/* -//========================================================================================================================// -// CUSTOM PINS // -//========================================================================================================================// - -#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! - -//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used -//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. - -//LED: -#define HW_PIN_LED pp -#define HW_LED_ON 0 //0:low is on, 1:high is on - -//IMU SPI: -#define HW_PIN_SPI_MISO pp -#define HW_PIN_SPI_MOSI pp -#define HW_PIN_SPI_SCLK pp -#define HW_PIN_IMU_CS pp -#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) - -//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) -#define HW_PIN_I2C_SDA pp -#define HW_PIN_I2C_SCL pp - -//Motor/Servo Outputs: -#define HW_OUT_COUNT 4 //number of outputs -#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. - -//Serial debug on USB Serial port (no GPIO pins) - -//RC Receiver: -#define HW_PIN_RCIN_RX pp -#define HW_PIN_RCIN_TX pp -#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets -//*/ - - -//========================================================================================================================// -// USER-SPECIFIED DEFINES // -//========================================================================================================================// - -//--- RC RECEIVER -#define RCIN_USE RCIN_USE_CRSF //RCIN_USE_CRSF, RCIN_USE_SBUS, RCIN_USE_DSM, RCIN_USE_PPM, RCIN_USE_PWM -#define RCIN_NUM_CHANNELS 5 //number of receiver channels (minimal 5) - -//--- IMU SENSOR -#define IMU_USE IMU_USE_SPI_MPU6500 // IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU9250,IMU_USE_SPI_MPU6000, IMU_USE_SPI_BMI270, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000 -//Set sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position. -//If not sure what is needed: use CLI 'proll' and try each setting until roll-right gives positive ahrs.roll, pitch-up gives positive ahrs.pitch, and yaw-right gives positive ahrs.yaw -#define IMU_ALIGN IMU_ALIGN_CW90 //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP -#define IMU_I2C_ADR 0x69 //IMU I2C address. If unknown, use CLI 'i2c' - -//========================================================================================================================// -// RC RECEIVER CONFIG // -//========================================================================================================================// - -//set channels -const int rcin_cfg_thro_channel = 1; //low pwm = zero throttle/stick back, high pwm = full throttle/stick forward -const int rcin_cfg_roll_channel = 2; //low pwm = left, high pwm = right -const int rcin_cfg_pitch_channel = 3; //low pwm = pitch up/stick back, high pwm = pitch down/stick forward -const int rcin_cfg_yaw_channel = 4; //low pwm = left, high pwm = right -const int rcin_cfg_arm_channel = 5; //ARM/DISARM switch - -//throttle pwm values -const int rcin_cfg_thro_low = 1250; //used to set rcin_thro_is_low flag when pwm is below. Note: your craft won't arm if this is too low. -const int rcin_cfg_thro_max = 1900; -const float out_armed_speed = 0.2; //Safety feature: make props spin when armed, the motors spin at this speed when armed and throttle is low. The default 0.2 is probably fairly high, set lower as needed. - -//roll, pitch, yaw pwm values -const int rcin_cfg_pwm_min = 1150; -const int rcin_cfg_pwm_center = 1500; -const int rcin_cfg_pwm_max = 1900; -const int rcin_cfg_pwm_deadband = 0; //Amount of deadband around center, center-deadband to center+deadband will be interpreted as central stick. Set to 15 for PPM or 0 for jitter-free serial protocol receivers. - -//pwm range for arm switch in ARMED position -const int rcin_cfg_arm_min = 1600; -const int rcin_cfg_arm_max = 2200; +#include "madflight_config.h" //Edit this header file to setup the pins, hardware, radio, etc. for madflight +#include //========================================================================================================================// // USER-SPECIFIED VARIABLES // //========================================================================================================================// -//number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos -const int out_MOTOR_COUNT = 4; -//name the outputs, to make code more readable -enum out_enum {MOTOR1, MOTOR2, MOTOR3, MOTOR4}; +//IMPORTANT: This is a safety feature which keeps props spinning when armed, and hopefully reminds the pilot to disarm!!! +const float armed_min_throttle = 0.20; //Minimum throttle when armed, set to a value between ~0.10 and ~0.25 which keeps the props spinning at minimum speed. -const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate - -//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing. -float LP_acc = 70; //Accelerometer (default MPU6050: 50Hz, MPU9250: 70Hz) -float LP_gyr = 60; //Gyro (default MPU6050: 35Hz, MPU9250: 60Hz) -float LP_mag = 1e10; //Magnetometer (default 1e10Hz, i.e. no filtering) -float LP_radio = 400; //Radio Input (default 400Hz) +//Flight Mode: Uncommment only one +#define FLIGHTMODE_RATE //control rate - stick centered will keep current roll/pitch angle +//#define FLIGHTMODE_ANGLE //control angle - stick centered will return to horizontal - IMPORTANT: execute CLI 'calimu' and 'cwrite' before using this!!! //Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees) -float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees) -float maxRollRate = 30.0; //Max roll rate in deg/sec for rate mode -float maxPitchRate = 30.0; //Max pitch rate in deg/sec for rate mode -float maxYawRate = 160.0; //Max yaw rate in deg/sec for angle and rate mode - -float Kp_ro_pi_angle = 0.2; //Roll/Pitch P-gain - angle mode -float Ki_ro_pi_angle = 0.1; //Roll/Pitch I-gain - angle mode -float Kd_ro_pi_angle = 0.05; //Roll/Pitch D-gain - angle mode (has no effect on control_Angle2) -float B_loop_ro_pi = 0.9; //Roll/Pitch damping term for control_Angle2(), lower is more damping (must be between 0 to 1) - -float Kp_ro_pi_rate = 0.15; //Roll/Pitch P-gain - rate mode -float Ki_ro_pi_rate = 0.2; //Roll/Pitch I-gain - rate mode -float Kd_ro_pi_rate = 0.0002; //Roll/Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) - -float Kp_yaw = 0.3; //Yaw P-gain -float Ki_yaw = 0.05; //Yaw I-gain -float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - -//========================================================================================================================// -// DECLARE GLOBAL VARIABLES // -//========================================================================================================================// - -//Radio communication: -int rcin_pwm[RCIN_NUM_CHANNELS]; //filtered raw PWM values -float rcin_thro, rcin_roll, rcin_pitch, rcin_yaw; //rcin_thro 0(cutoff) to 1(full); rcin_roll, rcin_pitch, rcin_yaw -1(left,down) to 1(right,up) with 0 center stick -bool rcin_armed; //status of arm switch, true = armed -bool rcin_thro_is_low; //status of throttle stick, true = throttle low -int rcin_aux; // six position switch connected to aux channel, values 0-5 - -//Controller: -float roll_PID = 0, pitch_PID = 0, yaw_PID = 0; - -//Flight status -bool out_armed = false; //motors will only run if this flag is true - -//Low pass filter parameters -float B_radio; - -//========================================================================================================================// -// INCLUDE MADFLIGHT LIBRARY // -//========================================================================================================================// -//Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern. -#include +const float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) +const float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees) +const float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees) +const float maxRollRate = 60.0; //Max roll rate in deg/sec for rate mode +const float maxPitchRate = 60.0; //Max pitch rate in deg/sec for rate mode +const float maxYawRate = 160.0; //Max yaw rate in deg/sec for angle and rate mode + +//PID Angle Mode +const float Kp_ro_pi_angle = 0.2; //Roll/Pitch P-gain +const float Ki_ro_pi_angle = 0.1; //Roll/Pitch I-gain +const float Kd_ro_pi_angle = 0.05; //Roll/Pitch D-gain +const float Kp_yaw_angle = 0.6; //Yaw P-gain +const float Kd_yaw_angle = 0.1; //Yaw D-gain + +//PID Rate Mode +const float Kp_ro_pi_rate = 0.15; //Roll/Pitch rate P-gain +const float Ki_ro_pi_rate = 0.2; //Roll/Pitch rate I-gain +const float Kd_ro_pi_rate = 0.0002; //Roll/Pitch rate D-gain (be careful when increasing too high, motors will begin to overheat!) +const float Kp_yaw_rate = 0.3; //Yaw rate P-gain +const float Ki_yaw_rate = 0.05; //Yaw rate I-gain +const float Kd_yaw_rate = 0.00015; //Yaw rate D-gain (be careful when increasing too high, motors will begin to overheat!) + +//Yaw to keep in ANGLE mode when yaw stick is centered +float yaw_desired = 0; //========================================================================================================================// // SETUP() // //========================================================================================================================// void setup() { - led.setup(HW_PIN_LED, HW_LED_ON); //Set built in LED to turn on to signal startup - Serial.begin(115200); //start console serial - - //6 second startup delay - for(int i=20;i>0;i--) { - Serial.printf("Quad " MADFLIGHT_VERSION " starting %d ...\n", i); - delay(300); - } - Serial.printf("Arduino library: " HW_ARDUINO_STR "\n"); - - hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) - cfg.begin(); //read config from EEPROM - cli.print_boardInfo(); //print board info and pinout - cli.print_i2cScan(); //print i2c scan - rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. - - //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) - while(true) { - int rv = imu.setup(imu_sample_rate); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning - if(rv<=0) break; - warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); - } - - //set filter parameters after imu.setup(), as imu.setup() can modify requested sample rate - B_radio = Ahrs::lowpass_to_beta(LP_radio, imu.getSampleRate()); //Note: uses imu sample rate because radio filter is applied in imu_loop + //setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component. + madflight_setup(); - //Motors - for(int i=0;i%d ",i+1,pwm_new[i],rcin_pwm[i]); Serial.println(); //uncomment for debugging + if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) { + led.set(!out.armed); //short interval + }else{ + led.set(out.armed); //long interval } } -void rcin_Normalize() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the normalized rcin variables rcin_thro, rcin_roll, rcin_pitch, and rcin_yaw. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. rcin_thro stays within 0 to 1 range. - * rcin_roll, rcin_pitch, and rcin_yaw are scaled to -1 to +1. - */ - - //normalized values - //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle - int pwm = rcin_pwm[rcin_cfg_thro_channel-1]; - rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), 0.0, 1.0); - rcin_thro_is_low = (pwm <= rcin_cfg_thro_low); - - //roll,pitch,yaw - rcin_roll = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_roll_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (roll left, stick left) to 1 (roll right, stick right) - rcin_pitch = - _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_pitch_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (pitch down, stick back) to 1 (pitch up, stick forward) - rcin_yaw = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_yaw_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (yaw left, stick left) to 1 (yaw right, stick right) - - //arm switch - pwm = rcin_pwm[rcin_cfg_arm_channel-1]; - rcin_armed = (rcin_cfg_arm_min <= pwm && pwm <= rcin_cfg_arm_max); -} - -//helper to nomalize a channel based on min,center,max calibration -float _rcin_ChannelNormalize(int val, int min, int center, int max, int deadband) { - int rev = 1; //1=normal, -1=reverse channel - //needs: min < center < max - if(val= 180) { + return fmod(v + 180, 360) - 180; + }else if(v < -180.0) { + return fmod(v - 180, 360) + 180; + } + return v; } void control_Angle(bool zero_integrators) { - //DESCRIPTION: Computes control commands based on state error (angle) + //DESCRIPTION: Computes control commands based on angle error /* - * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in - * rcin_Normalize(). Error is simply the desired state minus the actual state (ex. roll_des - ahrs.roll). Two safety features + * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des. Error + * is simply the desired state minus the actual state (ex. roll_des - ahrs.roll). Two safety features * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until - * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 + * they've maxed out throttle... saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I - * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which - * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in control_Mixer(). + * terms will always start from 0 on takeoff. This function updates the variables PIDroll.PID, PIDpitch.PID, and PIDyaw.PID which + * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in out_Mixer(). */ //inputs: roll_des, pitch_des, yawRate_des - //outputs: roll_PID, pitch_PID, yaw_PID + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID //desired values - float roll_des = rcin_roll * maxRoll; //Between -maxRoll and +maxRoll - float pitch_des = rcin_pitch * maxPitch; //Between -maxPitch and +maxPitch - float yawRate_des = rcin_yaw * maxYawRate; //Between -maxYawRate and +maxYawRate + float roll_des = rcin.roll * maxRoll; //Between -maxRoll and +maxRoll + float pitch_des = rcin.pitch * maxPitch; //Between -maxPitch and +maxPitch + float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate roll_PIDand +maxYawRate //state vars - static float integral_roll, integral_pitch, error_yaw_prev, integral_yaw; + static float integral_roll, integral_pitch, error_yawRate_prev, integral_yawRate; //Zero the integrators (used to don't let integrator build if throttle is too low, or to re-start the controller) if(zero_integrators) { integral_roll = 0; integral_pitch = 0; - integral_yaw = 0; + integral_yawRate = 0; } //Roll PID @@ -405,39 +188,55 @@ void control_Angle(bool zero_integrators) { integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_roll = ahrs.gx; - roll_PID = 0.01 * (Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll - Kd_ro_pi_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + PIDroll.PID = 0.01 * (Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll - Kd_ro_pi_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch PID float error_pitch = pitch_des - ahrs.pitch; integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_pitch = ahrs.gy; - pitch_PID = 0.01 * (Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch - Kd_ro_pi_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range - - //Yaw PID, stablize on rate from GyroZ - TODO: use compass heading, not gyro rate - float error_yaw = yawRate_des - ahrs.gz; - integral_yaw += error_yaw * imu.dt; - integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup - float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; - yaw_PID = 0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range - - //Update derivative variables - error_yaw_prev = error_yaw; + PIDpitch.PID = 0.01 * (Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch - Kd_ro_pi_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + + //Yaw PID + if(-0.02 < rcin.yaw && rcin.yaw < 0.02) { + //on reset, set desired yaw to current yaw + if(zero_integrators) yaw_desired = ahrs.yaw; + + //Yaw stick centered: hold yaw_desired + float error_yaw = degreeModulus(yaw_desired - ahrs.yaw); + float desired_yawRate = error_yaw / 0.5; //set desired yawRate such that it gets us to desired yaw in 0.5 second + float derivative_yaw = desired_yawRate - ahrs.gz; + PIDyaw.PID = 0.01 * (Kp_yaw_angle*error_yaw + Kd_yaw_angle*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + + //update yaw rate controller + error_yawRate_prev = 0; + }else{ + //Yaw stick not centered: stablize on rate from GyroZ + float error_yawRate = yawRate_des - ahrs.gz; + integral_yawRate += error_yawRate * imu.dt; + integral_yawRate = constrain(integral_yawRate, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + float derivative_yawRate = (error_yawRate - error_yawRate_prev) / imu.dt; + PIDyaw.PID = 0.01 * (Kp_yaw_rate*error_yawRate + Ki_yaw_rate*integral_yawRate + Kd_yaw_rate*derivative_yawRate); //Scaled by .01 to bring within -1 to 1 range + + //Update derivative variables + error_yawRate_prev = error_yawRate; + + //update yaw controller: + yaw_desired = ahrs.yaw; //set desired yaw to current yaw, the yaw angle controller will hold this value + } } void control_Rate(bool zero_integrators) { - //DESCRIPTION: Computes control commands based on state error (rate) - /* - * See explanation for control_Angle(). Everything is the same here except the error is now the desired rate - raw gyro reading. - */ + //Computes control commands based on state error (rate) + //See explanation for control_Angle(). Everything is the same here except the error is now: desired rate - raw gyro reading. //inputs: roll_des, pitch_des, yawRate_des - //outputs: roll_PID, pitch_PID, yaw_PID + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID //desired values - float rollRate_des = rcin_roll * maxRollRate; //Between -maxRoll and +maxRoll - float pitchRate_des = rcin_pitch * maxPitchRate; //Between -maxPitch and +maxPitch - float yawRate_des = rcin_yaw * maxYawRate; //Between -maxYawRate and +maxYawRate + float rollRate_des = rcin.roll * maxRollRate; //Between -maxRoll and +maxRoll + float pitchRate_des = rcin.pitch * maxPitchRate; //Between -maxPitch and +maxPitch + float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate and +maxYawRate //state vars static float integral_roll, error_roll_prev; @@ -456,21 +255,21 @@ void control_Rate(bool zero_integrators) { integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_roll = (error_roll - error_roll_prev) / imu.dt; - roll_PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + PIDroll.PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch float error_pitch = pitchRate_des - ahrs.gy; integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt; - pitch_PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + PIDpitch.PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ float error_yaw = yawRate_des - ahrs.gz; integral_yaw += error_yaw * imu.dt; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; - yaw_PID = 0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + PIDyaw.PID = 0.01 * (Kp_yaw_rate*error_yaw + Ki_yaw_rate*integral_yaw + Kd_yaw_rate*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables error_roll_prev = error_roll; @@ -478,20 +277,44 @@ void control_Rate(bool zero_integrators) { error_yaw_prev = error_yaw; } -void control_Mixer() { +void out_KillSwitchAndFailsafe() { + static bool rcin_arm_prev = true; //initial value is true: forces out.armed false on startup even if arm switch is ON + + //Change to ARMED when throttle is zero and radio armed switch was flipped from disamed to armed position + if (!out.armed && rcin.throttle == 0 && rcin.arm && !rcin_arm_prev) { + out.armed = true; + Serial.println("OUT: ARMED"); + bb.start(); //start blackbox logging + } + + //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection + if (out.armed && (!rcin.arm || !rcin.connected())) { + out.armed = false; + if(!rcin.arm) { + Serial.println("OUT: DISARMED"); + bb.stop(); //stop blackbox logging + }else{ + Serial.println("OUT: DISARMED due to lost radio connection"); + //keep on logging to document the crash... + } + } + + rcin_arm_prev = rcin.arm; +} + +void out_Mixer() { //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration /* - * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired - * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors - * should have -roll_PID. Front two should have +pitch_PID and the back two should have -pitch_PID etc... every motor has - * normalized (0 to 1) rcin_thro command for throttle control. Can also apply direct unstabilized commands from the transmitter with - * rcin_xxx variables are to be sent to the motor ESCs and servos. + * Takes PIDroll.PID, PIDpitch.PID, and PIDyaw.PID computed from the PID controller and appropriately mixes them for the desired + * vehicle configuration. For example on a quadcopter, the left two motors should have +PIDroll.PID while the right two motors + * should have -PIDroll.PID. Front two should have +PIDpitch.PID and the back two should have -PIDpitch.PID etc... every motor has + * normalized (0 to 1) rcin.throttle command for throttle control. Can also apply direct unstabilized commands from the transmitter with + * rcin.xxx variables are to be sent to the motor ESCs and servos. * *Relevant variables: - *rcin_thro - direct thottle control - *roll_PID, pitch_PID, yaw_PID - stabilized axis variables - *rcin_roll, rcin_pitch, rcin_yaw - direct unstabilized command passthrough - *rcin_aux - free auxillary channel, can be used to toggle things with an 'if' statement + *rcin.throtle - direct thottle control + *PIDroll.PID, PIDpitch.PID, PIDyaw.PID - stabilized axis variables + *rcin.roll, rcin.pitch, rcin.yaw - direct unstabilized command passthrough */ /* Motor order diagram (Betaflight order) @@ -511,62 +334,13 @@ Roll right (left+ right-) --++ Yaw right (CCW+ CW-) -++- */ - //Quad mixing - out_command[MOTOR1] = rcin_thro - pitch_PID - roll_PID - yaw_PID; //Back Right CW - out_command[MOTOR2] = rcin_thro + pitch_PID - roll_PID + yaw_PID; //Front Right CCW - out_command[MOTOR3] = rcin_thro - pitch_PID + roll_PID + yaw_PID; //Back Left CCW - out_command[MOTOR4] = rcin_thro + pitch_PID + roll_PID - yaw_PID; //Front Left CW -} - -void out_KillSwitchAndFailsafe() { - static bool rcin_armed_prev = true; //initial value is true: forces out_armed false on startup even if arm switch is ON + // IMPORTANT: This is a safety feature to remind the pilot to disarm. + // Set throttle to at least armed_min_throttle, to keep at least one prop spinning when armed. The [out] module will disable motors when out.armed == false + float thr = armed_min_throttle + (1 - armed_min_throttle) * rcin.throttle; //shift throttle range from [0.0 .. 1.0] to [armed_min_throttle .. 1.0] - //Change to ARMED when throttle is low and radio armed switch was flipped from disamed to armed position - if (!out_armed && rcin_thro_is_low && rcin_armed && !rcin_armed_prev) { - out_armed = true; - Serial.println("OUT: ARMED"); - } - - //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection - if (out_armed && (!rcin_armed || !rcin.connected())) { - out_armed = false; - if(!rcin_armed) { - Serial.println("OUT: DISARMED (arm switch)"); - }else{ - Serial.println("OUT: DISARMED (rcin lost connection)"); - } - } - - //If armed and throttle is low -> set motor outputs to out_armed_speed - if(out_armed && rcin_thro_is_low) for(int i=0;i STOP MOTORS - if(!out_armed) for(int i=0;i +// +// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. +// +//========================================================================================================================// + +/* <-- remove this to enable custom pins +//========================================================================================================================// +// CUSTOM PINS CONFIG // +//========================================================================================================================// + +#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! + +//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used +//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. + +//LED: +#define HW_PIN_LED pp +#define HW_LED_ON 0 //0:low is on, 1:high is on + +//IMU SPI: +#define HW_PIN_SPI_MISO pp +#define HW_PIN_SPI_MOSI pp +#define HW_PIN_SPI_SCLK pp +#define HW_PIN_IMU_CS pp +#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) + +//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) +#define HW_PIN_I2C_SDA pp +#define HW_PIN_I2C_SCL pp + +//Motor/Servo Outputs: +#define HW_OUT_COUNT 4 //number of outputs +#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. + +//Serial debug on USB Serial port (no GPIO pins) + +//RC Receiver: +#define HW_PIN_RCIN_RX pp +#define HW_PIN_RCIN_TX pp +#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets + +//GPS: +#define HW_PIN_GPS_RX pp +#define HW_PIN_GPS_TX pp +#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets + +//Battery ADC +#define HW_PIN_BAT_V pp +#define HW_PIN_BAT_I pp + +//Black Box SPI (for sdcard or external flash chip): +#define HW_PIN_SPI2_MISO pp +#define HW_PIN_SPI2_MOSI pp +#define HW_PIN_SPI2_SCLK pp +#define HW_PIN_BB_CS pp + +//Black Box SDCARD via MMC interface: +#define HW_PIN_SDMMC_DATA pp +#define HW_PIN_SDMMC_CLK pp +#define HW_PIN_SDMMC_CMD pp +//*/ + +//RP2040 specific options +//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking + +//ESP32 specific options +//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 + diff --git a/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino b/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino index 49528fd..1e7e37c 100644 --- a/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino +++ b/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino @@ -1,22 +1,13 @@ /*######################################################################################################################### -NOTICE: First get Quadcopter.ino to fly before attempting this program +NOTICE: First get Quadcopter.ino to fly before attempting this program. + +You can copy madflight_config.h from Quadcopter.ino to keep your settings. ########################################################################################################################### See http://madflight.com for detailed description -Motor order diagram (Betaflight order) - - front - CW --> <-- CCW - 4 2 - \ ^ / - |X| - / - \ - 3 1 -CCW --> <-- CW - Arming: Set throttle low, then flip arm switch from DISARMED to ARMED. Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch". @@ -33,300 +24,65 @@ MIT license Copyright (c) 2023-2024 https://madflight.com ##########################################################################################################################*/ - -//========================================================================================================================// -// PINS // -//========================================================================================================================// -// -// You have 3 options to setup the pins (gpio numbers) for the flight controller: -// -// 1) Default - Leave this section as is and see https://madflight.com for default pinout diagrams for the supported -// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h -// -// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available -// boards. For example: #include -// -// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. -// -//========================================================================================================================// - -/* -//========================================================================================================================// -// CUSTOM PINS // -//========================================================================================================================// - -#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! - -//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used -//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. - -//LED: -#define HW_PIN_LED pp -#define HW_LED_ON 0 //0:low is on, 1:high is on - -//IMU SPI: -#define HW_PIN_SPI_MISO pp -#define HW_PIN_SPI_MOSI pp -#define HW_PIN_SPI_SCLK pp -#define HW_PIN_IMU_CS pp -#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) - -//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) -#define HW_PIN_I2C_SDA pp -#define HW_PIN_I2C_SCL pp - -//Motor/Servo Outputs: -#define HW_OUT_COUNT 4 //number of outputs -#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. - -//Serial debug on USB Serial port (no GPIO pins) - -//RC Receiver: -#define HW_PIN_RCIN_RX pp -#define HW_PIN_RCIN_TX pp -#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets - -//GPS: -#define HW_PIN_GPS_RX pp -#define HW_PIN_GPS_TX pp -#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets - -//Battery ADC -#define HW_PIN_BAT_V pp -#define HW_PIN_BAT_I pp - -//Black Box SPI (for sdcard or external flash chip): -#define HW_PIN_SPI2_MISO pp -#define HW_PIN_SPI2_MOSI pp -#define HW_PIN_SPI2_SCLK pp -#define HW_PIN_BB_CS pp - -//Black Box SDCARD via MMC interface: -#define HW_PIN_SDMMC_DATA pp -#define HW_PIN_SDMMC_CLK pp -#define HW_PIN_SDMMC_CMD pp -//*/ - -//RP2040 specific options -//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking - -//ESP32 specific options -//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 - - -//========================================================================================================================// -// HARDWARE // -//========================================================================================================================// - -//--- RC RECEIVER -#define RCIN_USE RCIN_USE_CRSF // RCIN_USE_CRSF, RCIN_USE_SBUS, RCIN_USE_DSM, RCIN_USE_PPM, RCIN_USE_PWM -#define RCIN_NUM_CHANNELS 6 //number of receiver channels (minimal 6) - -//--- IMU SENSOR -#define IMU_USE IMU_USE_SPI_MPU9250 // IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU9250,IMU_USE_SPI_MPU6000, IMU_USE_SPI_BMI270, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000 -//Set sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position. -//If not sure what is needed: use CLI 'proll' and try each setting until roll-right gives positive ahrs.roll, pitch-up gives positive ahrs.pitch, and yaw-right gives positive ahrs.yaw -#define IMU_ALIGN IMU_ALIGN_CW0 //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP -#define IMU_I2C_ADR 0x69 //IMU I2C address. If unknown, use CLI 'i2c' - -//-- AHRS sensor fusion -#define AHRS_USE AHRS_USE_MAHONY // AHRS_USE_MAHONY, AHRS_USE_MAHONY_BF, AHRS_USE_MADGWICK, AHRS_USE_VQF - -//--- GPS -#define GPS_BAUD 115200 - -//--- BAROMETER SENSOR -#define BARO_USE BARO_USE_NONE // BARO_USE_BMP280, BARO_USE_MS5611, BARO_USE_NONE -//#define BARO_I2C_ADR 0x76 //set barometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- EXTERNAL MAGNETOMETER SENSOR -#define MAG_USE MAG_USE_NONE // MAG_USE_QMC5883L, MAG_USE_NONE -//#define MAG_I2C_ADR 0x77 //set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE - -//--- BLACKBOX LOGGER -#define BB_USE BB_USE_NONE //BB_USE_SD spi sdcard, BB_USE_SDMMC mmc sdcard, BB_USE_NONE - -//========================================================================================================================// -// RC RECEIVER // -//========================================================================================================================// - -//set channels -const int rcin_cfg_thro_channel = 1; //low pwm = zero throttle/stick back, high pwm = full throttle/stick forward -const int rcin_cfg_roll_channel = 2; //low pwm = left, high pwm = right -const int rcin_cfg_pitch_channel = 3; //low pwm = pitch up/stick back, high pwm = pitch down/stick forward -const int rcin_cfg_yaw_channel = 4; //low pwm = left, high pwm = right -const int rcin_cfg_arm_channel = 5; //ARM/DISARM switch -const int rcin_cfg_aux_channel = 6; //Fight mode - 6 position switch - -//throttle pwm values -const int rcin_cfg_thro_low = 1250; //used to set rcin_thro_is_low flag when pwm is below. Note: your craft won't arm if this is too low. -const int rcin_cfg_thro_max = 1900; -const float out_armed_speed = 0.2; //Safety feature: make props spin when armed, the motors spin at this speed when armed and throttle is low. The default 0.2 is probably fairly high, set lower as needed. - -//roll, pitch, yaw pwm values -const int rcin_cfg_pwm_min = 1150; -const int rcin_cfg_pwm_center = 1500; -const int rcin_cfg_pwm_max = 1900; -const int rcin_cfg_pwm_deadband = 0; //Amount of deadband around center, center-deadband to center+deadband will be interpreted as central stick. Set to 15 for PPM or 0 for jitter-free serial protocol receivers. - -//pwm range for arm switch in ARMED position -const int rcin_cfg_arm_min = 1600; -const int rcin_cfg_arm_max = 2200; - -//6 position switch on aux channel - Ardupilot switch pwm: 1165,1295,1425,1555,1685,1815 (spacing 130) -//EdgeTx 3-pos SA + 2-pos SB setup: Source:SA weight:52 offset:0, Source:SB weight:13 offset:-1 multiplex: add -OR- Source:SA Weight:26 Offset:-40 Switch:SBdown, Source:SA Weight:26 Offset:36 Switch:SBup Multiplex:Replace -int rcin_cfg_aux_min = 1165; //lowest switch pwm -int rcin_cfg_aux_max = 1815; //higest switch pwm +#include "madflight_config.h" //Edit this header file to setup the pins, hardware, radio, etc. for madflight +#include //========================================================================================================================// // USER-SPECIFIED VARIABLES // //========================================================================================================================// -//number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos -const int out_MOTOR_COUNT = 4; -//name the outputs, to make code more readable -enum out_enum {MOTOR1,MOTOR2,MOTOR3,MOTOR4,SERVO1,SERVO2,SERVO3,SERVO4,SERVO5,SERVO6,SERVO7,SERVO8,SERVO9,SERVO10,SERVO11,SERVO12}; - -//flight modes -enum rcin_fm_enum {RATE, ANGLE}; //available flight modes: RATE stabilize rate, ANGLE stabilize angle -const char* rcin_fm_str[] = {"RATE","ANGLE"}; -rcin_fm_enum rcin_fm_map[6] {RATE, RATE, RATE, ANGLE, ANGLE, ANGLE}; //flightmode mapping from 6 pos switch to flight mode (simulates a 2-pos switch: RATE/ANGLE) -rcin_fm_enum rcin_fm = (rcin_fm_enum)0; //current flight mode - -const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate -const uint32_t baro_sample_rate = 100; //baro sample rate in Hz (default 100) +//IMPORTANT: This is a safety feature which keeps props spinning when armed, and hopefully reminds the pilot to disarm!!! +const float armed_min_throttle = 0.20; //Minimum throttle when armed, set to a value between ~0.10 and ~0.25 which keeps the props spinning at minimum speed. -//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing. -float LP_acc = 70; //Accelerometer (default MPU6050: 50Hz, MPU9250: 70Hz) -float LP_gyr = 60; //Gyro (default MPU6050: 35Hz, MPU9250: 60Hz) -float LP_mag = 1e10; //Magnetometer (default 1e10Hz, i.e. no filtering) -float LP_radio = 400; //Radio Input (default 400Hz) +//Flight modes +enum flightmode_enum {RATE, ANGLE}; //the available flightmodes +const char* flightmode_str[] = {"RATE","ANGLE"}; //define flightmode strings for telemetry +flightmode_enum flightmode_map[6] {RATE, RATE, RATE, ANGLE, ANGLE, ANGLE}; //flightmode mapping from 6 pos switch to flight mode (simulates a 2-pos switch: RATE/ANGLE) +flightmode_enum flightmode = RATE; //current flight mode //Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees) -float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees) -float maxRollRate = 30.0; //Max roll rate in deg/sec for rate mode -float maxPitchRate = 30.0; //Max pitch rate in deg/sec for rate mode -float maxYawRate = 160.0; //Max yaw rate in deg/sec for angle and rate mode +const float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) +const float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees) +const float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees) +const float maxRollRate = 60.0; //Max roll rate in deg/sec for rate mode +const float maxPitchRate = 60.0; //Max pitch rate in deg/sec for rate mode +const float maxYawRate = 160.0; //Max yaw rate in deg/sec for angle and rate mode //PID Angle Mode -float Kp_ro_pi_angle = 0.2; //Roll/Pitch P-gain -float Ki_ro_pi_angle = 0.1; //Roll/Pitch I-gain -float Kd_ro_pi_angle = 0.05; //Roll/Pitch D-gain -float Kp_yaw_angle = 0.6; //Yaw P-gain -float Kd_yaw_angle = 0.1; //Yaw D-gain +const float Kp_ro_pi_angle = 0.2; //Roll/Pitch P-gain +const float Ki_ro_pi_angle = 0.1; //Roll/Pitch I-gain +const float Kd_ro_pi_angle = 0.05; //Roll/Pitch D-gain +const float Kp_yaw_angle = 0.6; //Yaw P-gain +const float Kd_yaw_angle = 0.1; //Yaw D-gain //PID Rate Mode -float Kp_ro_pi_rate = 0.15; //Roll/Pitch rate P-gain -float Ki_ro_pi_rate = 0.2; //Roll/Pitch rate I-gain -float Kd_ro_pi_rate = 0.0002; //Roll/Pitch rate D-gain (be careful when increasing too high, motors will begin to overheat!) -float Kp_yaw_rate = 0.3; //Yaw rate P-gain -float Ki_yaw_rate = 0.05; //Yaw rate I-gain -float Kd_yaw_rate = 0.00015; //Yaw rate D-gain (be careful when increasing too high, motors will begin to overheat!) +const float Kp_ro_pi_rate = 0.15; //Roll/Pitch rate P-gain +const float Ki_ro_pi_rate = 0.2; //Roll/Pitch rate I-gain +const float Kd_ro_pi_rate = 0.0002; //Roll/Pitch rate D-gain (be careful when increasing too high, motors will begin to overheat!) +const float Kp_yaw_rate = 0.3; //Yaw rate P-gain +const float Ki_yaw_rate = 0.05; //Yaw rate I-gain +const float Kd_yaw_rate = 0.00015; //Yaw rate D-gain (be careful when increasing too high, motors will begin to overheat!) -//========================================================================================================================// -// DECLARE GLOBAL VARIABLES // -//========================================================================================================================// - -//Radio communication: -int rcin_pwm[RCIN_NUM_CHANNELS]; //filtered raw PWM values -float rcin_thro; //throttle: 0(cutoff) to 1(full); -float rcin_roll, rcin_pitch, rcin_yaw; // roll,pitch,yaw: -1(left,down) to 1(right,up) with 0 center stick -bool rcin_armed; //status of arm switch, true = armed -bool rcin_thro_is_low; //status of throttle stick, true = throttle low -int rcin_aux; // six position switch connected to aux channel, values 0-5 - -//Controller: -float roll_PID = 0, pitch_PID = 0, yaw_PID = 0, yaw_desired = 0; - -//Flight status -bool out_armed = false; //motors will only run if this flag is true - -//Low pass filter parameters -float B_radio; - -//========================================================================================================================// -// INCLUDE MADFLIGHT LIBRARY // -//========================================================================================================================// -//Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern. -#include +//Yaw to keep in ANGLE mode when yaw stick is centered +float yaw_desired = 0; //========================================================================================================================// // SETUP() // //========================================================================================================================// void setup() { - led.setup(HW_PIN_LED, HW_LED_ON); //Set built in LED to turn on to signal startup - Serial.begin(115200); //start console serial - - //6 second startup delay - for(int i=20;i>0;i--) { - Serial.printf("QuadAdv " MADFLIGHT_VERSION " starting %d ...\n", i); - delay(300); - } - Serial.printf("Arduino library: " HW_ARDUINO_STR "\n"); - - cli.print_boardInfo(); //print board info and pinout - hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) - cfg.begin(); //read config from EEPROM - cli.print_i2cScan(); //print i2c scan - rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. - - //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) - while(true) { - int rv = imu.setup(imu_sample_rate); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning - if(rv<=0) break; - warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); - } - - //set filter parameters after imu.setup(), as imu.setup() can modify requested sample rate - B_radio = Ahrs::lowpass_to_beta(LP_radio, imu.getSampleRate()); //Note: uses imu sample rate because radio filter is applied in imu_loop - - baro.setup(baro_sample_rate); //Barometer sample rate 100Hz - mag.setup(); //External Magnetometer - bat.setup(); //Battery Monitor - bb.setup(); //Black Box - gps_setup(); //GPS - //gps_debug(); //uncomment to debug gps messages + //setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component. + madflight_setup(); - //Servos (set servos first just in case motors overwrite frequency of shared timers) - for(int i=out_MOTOR_COUNT;i 100) { - rcin_telem_ts = millis(); - rcin_telem_cnt++; - String fm_str = String(out_armed ? "*" : "") + rcin_fm_str[rcin_fm]; + static uint32_t telem_ts = 0; + static uint32_t telem_cnt = 0; + if(millis() - telem_ts > 100) { + telem_ts = millis(); + telem_cnt++; + String fm_str = String(out.armed ? "*" : "") + (gps.sat>0 ? String(gps.sat) : String("")) + flightmode_str[flightmode]; rcin_telemetry_flight_mode(fm_str.c_str()); //only first 14 char get transmitted rcin_telemetry_attitude(ahrs.pitch, ahrs.roll, ahrs.yaw); - if(rcin_telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100); - if(rcin_telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h + if(telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100); + if(telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h + } + + //logging + static uint32_t log_ts = 0; + if(millis() - log_ts > 100) { + log_ts = millis(); + bb.log_sys(); } cli.loop(); //process CLI commands @@ -366,7 +129,7 @@ void i2c_sensors_update() { // IMU UPDATE LOOP // //========================================================================================================================// -//This is __MAIN__ function of this program. It is called when new IMU data is available. +//This is the __MAIN__ part of this program. It is called when new IMU data is available, and runs as high priority FreeRTOS task. void imu_loop() { //Blink LED led_Blink(); @@ -374,33 +137,30 @@ void imu_loop() { //Sensor fusion: update ahrs.roll, ahrs.pitch, and ahrs.yaw angle estimates (degrees) from IMU data ahrs.update(); - //Get radio state - rcin_GetCommands(); //Pulls current available radio commands - rcin_Normalize(); //Convert raw commands to normalized values based on saturated control limits - - //Uncomment to debug without remote (and no battery!) - pitch drone up: motors m1,m3 should increase and m2,m4 decrease; bank right: m1,m2 increase; yaw right: m1,m4 increase - //rcin_thro = 0.5; rcin_thro_is_low = false; rcin_roll = 0; rcin_pitch = 0; rcin_yaw = 0; rcin_armed = true; rcin_aux = 0; out_armed = true; + //Get radio commands - Note: don't do this in loop() because loop() is a lower priority task than imu_loop(), so in worst case loop() will not get any processor time. + rcin.update(); + flightmode = flightmode_map[rcin.flightmode]; //map rcin.flightmode (0 to 5) to vehicle flightmode //PID Controller - switch(rcin_fm) { - case ANGLE: - control_Angle(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint + switch(flightmode) { + case ANGLE: + control_Angle(rcin.throttle == 0); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint break; - default: - control_Rate(rcin_thro_is_low); //Stabilize on rate setpoint + default: //RATE + control_Rate(rcin.throttle == 0); //Stabilize on rate setpoint } - //Actuator mixing - control_Mixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - - //Motor output + //Updates out.arm, the output armed flag out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered. - out_SetCommands(); //Sends command pulses to motors (only if out_armed=true) and servos - //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. + //Actuator mixing + out_Mixer(); //Mixes PID outputs and sends command pulses to the motors, if mot.arm == true + + //If IMU uses I2C bus, then get I2C sensor readings here in imu_loop() and not in loop() to prevent I2C bus collisions. + //Alternatively, put the IMU on a separate I2C bus. But really: use SPI for IMU!!! if (imu.usesI2C()) i2c_sensors_update(); - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... + //bb.log_imu(); //uncomment for full speed black box logging of IMU data, but memory will fill up quickly... } //========================================================================================================================// @@ -410,77 +170,17 @@ void imu_loop() { void led_Blink() { //Blink LED once per second, if LED blinks slower then the loop takes too much time, use print_loop_Rate() to investigate. //DISARMED: long off, short on, ARMED: long on, short off - if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) - led.set(!out_armed); //short interval - else - led.set(out_armed); //long interval -} - -void rcin_GetCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the values are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - bool got_new_data = rcin.update(); - - //Low-pass the critical commands and update previous values - for(int i=0; i%d ",i+1,pwm_new[i],rcin_pwm[i]); Serial.println(); //uncomment for debugging + if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) { + led.set(!out.armed); //short interval + }else{ + led.set(out.armed); //long interval } } -void rcin_Normalize() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the normalized rcin variables rcin_thro, rcin_roll, rcin_pitch, and rcin_yaw. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. rcin_thro stays within 0 to 1 range. - * rcin_roll, rcin_pitch, and rcin_yaw are scaled to -1 to +1. - */ - - //normalized values - //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle - int pwm = rcin_pwm[rcin_cfg_thro_channel-1]; - rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), 0.0, 1.0); - rcin_thro_is_low = (pwm <= rcin_cfg_thro_low); - - //roll,pitch,yaw - rcin_roll = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_roll_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (roll left, stick left) to 1 (roll right, stick right) - rcin_pitch = - _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_pitch_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (pitch down, stick back) to 1 (pitch up, stick forward) - rcin_yaw = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_yaw_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (yaw left, stick left) to 1 (yaw right, stick right) - - //arm switch - pwm = rcin_pwm[rcin_cfg_arm_channel-1]; - rcin_armed = (rcin_cfg_arm_min <= pwm && pwm <= rcin_cfg_arm_max); - - //aux 6 position switch (flight mode) - int spacing = (rcin_cfg_aux_max - rcin_cfg_aux_min) / 5; - rcin_aux = constrain( ( rcin_pwm[rcin_cfg_aux_channel-1] - rcin_cfg_aux_min + spacing/2) / spacing, 0, 5); //output 0..5 - rcin_fm = rcin_fm_map[rcin_aux]; -} - -//helper to nomalize a channel based on min,center,max calibration -float _rcin_ChannelNormalize(int val, int min, int center, int max, int deadband) { - int rev = 1; //1=normal, -1=reverse channel - //needs: min < center < max - if(val= 180) { - return fmod(v + 180, 360) - 180; //note: fmod(x,360) returns values between -360 and 360, not 0 and 360 as integer mod() does + return fmod(v + 180, 360) - 180; }else if(v < -180.0) { return fmod(v - 180, 360) + 180; } @@ -488,25 +188,25 @@ float degreeModulus(float v) { } void control_Angle(bool zero_integrators) { - //DESCRIPTION: Computes control commands based on state error (angle) + //DESCRIPTION: Computes control commands based on angle error /* - * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des computed in - * rcin_Normalize(). Error is simply the desired state minus the actual state (ex. roll_des - ahrs.roll). Two safety features + * Basic PID control to stablize on angle setpoint based on desired states roll_des, pitch_des, and yaw_des. Error + * is simply the desired state minus the actual state (ex. roll_des - ahrs.roll). Two safety features * are implimented here regarding the I terms. The I terms are saturated within specified limits on startup to prevent * excessive buildup. This can be seen by holding the vehicle at an angle and seeing the motors ramp up on one side until - * they've maxed out throttle...saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 + * they've maxed out throttle... saturating I to a specified limit fixes this. The second feature defaults the I terms to 0 * if the throttle is at the minimum setting. This means the motors will not start spooling up on the ground, and the I - * terms will always start from 0 on takeoff. This function updates the variables roll_PID, pitch_PID, and yaw_PID which - * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in control_Mixer(). + * terms will always start from 0 on takeoff. This function updates the variables PIDroll.PID, PIDpitch.PID, and PIDyaw.PID which + * can be thought of as 1-D stablized signals. They are mixed to the configuration of the vehicle in out_Mixer(). */ //inputs: roll_des, pitch_des, yawRate_des - //outputs: roll_PID, pitch_PID, yaw_PID + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID //desired values - float roll_des = rcin_roll * maxRoll; //Between -maxRoll and +maxRoll - float pitch_des = rcin_pitch * maxPitch; //Between -maxPitch and +maxPitch - float yawRate_des = rcin_yaw * maxYawRate; //Between -maxYawRate and +maxYawRate + float roll_des = rcin.roll * maxRoll; //Between -maxRoll and +maxRoll + float pitch_des = rcin.pitch * maxPitch; //Between -maxPitch and +maxPitch + float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate roll_PIDand +maxYawRate //state vars static float integral_roll, integral_pitch, error_yawRate_prev, integral_yawRate; @@ -523,22 +223,25 @@ void control_Angle(bool zero_integrators) { integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_roll = ahrs.gx; - roll_PID = 0.01 * (Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll - Kd_ro_pi_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + PIDroll.PID = 0.01 * (Kp_ro_pi_angle*error_roll + Ki_ro_pi_angle*integral_roll - Kd_ro_pi_angle*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch PID float error_pitch = pitch_des - ahrs.pitch; integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_pitch = ahrs.gy; - pitch_PID = 0.01 * (Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch - Kd_ro_pi_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + PIDpitch.PID = 0.01 * (Kp_ro_pi_angle*error_pitch + Ki_ro_pi_angle*integral_pitch - Kd_ro_pi_angle*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw PID - if(-0.02 < rcin_yaw && rcin_yaw < 0.02) { + if(-0.02 < rcin.yaw && rcin.yaw < 0.02) { + //on reset, set desired yaw to current yaw + if(zero_integrators) yaw_desired = ahrs.yaw; + //Yaw stick centered: hold yaw_desired float error_yaw = degreeModulus(yaw_desired - ahrs.yaw); float desired_yawRate = error_yaw / 0.5; //set desired yawRate such that it gets us to desired yaw in 0.5 second float derivative_yaw = desired_yawRate - ahrs.gz; - yaw_PID = 0.01 * (Kp_yaw_angle*error_yaw + Kd_yaw_angle*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + PIDyaw.PID = 0.01 * (Kp_yaw_angle*error_yaw + Kd_yaw_angle*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //update yaw rate controller error_yawRate_prev = 0; @@ -548,7 +251,7 @@ void control_Angle(bool zero_integrators) { integral_yawRate += error_yawRate * imu.dt; integral_yawRate = constrain(integral_yawRate, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_yawRate = (error_yawRate - error_yawRate_prev) / imu.dt; - yaw_PID = 0.01 * (Kp_yaw_rate*error_yawRate + Ki_yaw_rate*integral_yawRate + Kd_yaw_rate*derivative_yawRate); //Scaled by .01 to bring within -1 to 1 range + PIDyaw.PID = 0.01 * (Kp_yaw_rate*error_yawRate + Ki_yaw_rate*integral_yawRate + Kd_yaw_rate*derivative_yawRate); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables error_yawRate_prev = error_yawRate; @@ -563,12 +266,12 @@ void control_Rate(bool zero_integrators) { //See explanation for control_Angle(). Everything is the same here except the error is now: desired rate - raw gyro reading. //inputs: roll_des, pitch_des, yawRate_des - //outputs: roll_PID, pitch_PID, yaw_PID + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID //desired values - float rollRate_des = rcin_roll * maxRollRate; //Between -maxRoll and +maxRoll - float pitchRate_des = rcin_pitch * maxPitchRate; //Between -maxPitch and +maxPitch - float yawRate_des = rcin_yaw * maxYawRate; //Between -maxYawRate and +maxYawRate + float rollRate_des = rcin.roll * maxRollRate; //Between -maxRoll and +maxRoll + float pitchRate_des = rcin.pitch * maxPitchRate; //Between -maxPitch and +maxPitch + float yawRate_des = rcin.yaw * maxYawRate; //Between -maxYawRate and +maxYawRate //state vars static float integral_roll, error_roll_prev; @@ -587,21 +290,21 @@ void control_Rate(bool zero_integrators) { integral_roll += error_roll * imu.dt; integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_roll = (error_roll - error_roll_prev) / imu.dt; - roll_PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range + PIDroll.PID = 0.01 * (Kp_ro_pi_rate*error_roll + Ki_ro_pi_rate*integral_roll + Kd_ro_pi_rate*derivative_roll); //Scaled by .01 to bring within -1 to 1 range //Pitch float error_pitch = pitchRate_des - ahrs.gy; integral_pitch += error_pitch * imu.dt; integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_pitch = (error_pitch - error_pitch_prev) / imu.dt; - pitch_PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range + PIDpitch.PID = 0.01 * (Kp_ro_pi_rate*error_pitch + Ki_ro_pi_rate*integral_pitch + Kd_ro_pi_rate*derivative_pitch); //Scaled by .01 to bring within -1 to 1 range //Yaw, stablize on rate from GyroZ float error_yaw = yawRate_des - ahrs.gz; integral_yaw += error_yaw * imu.dt; integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; - yaw_PID = 0.01 * (Kp_yaw_rate*error_yaw + Ki_yaw_rate*integral_yaw + Kd_yaw_rate*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range + PIDyaw.PID = 0.01 * (Kp_yaw_rate*error_yaw + Ki_yaw_rate*integral_yaw + Kd_yaw_rate*derivative_yaw); //Scaled by .01 to bring within -1 to 1 range //Update derivative variables error_roll_prev = error_roll; @@ -609,20 +312,45 @@ void control_Rate(bool zero_integrators) { error_yaw_prev = error_yaw; } -void control_Mixer() { +void out_KillSwitchAndFailsafe() { + static bool rcin_arm_prev = true; //initial value is true: forces out.armed false on startup even if arm switch is ON + + //Change to ARMED when throttle is zero and radio armed switch was flipped from disamed to armed position + if (!out.armed && rcin.throttle == 0 && rcin.arm && !rcin_arm_prev) { + out.armed = true; + Serial.println("OUT: ARMED"); + bb.start(); //start blackbox logging + } + + //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection + if (out.armed && (!rcin.arm || !rcin.connected())) { + out.armed = false; + if(!rcin.arm) { + Serial.println("OUT: DISARMED"); + bb.stop(); //stop blackbox logging + }else{ + Serial.println("OUT: DISARMED due to lost radio connection"); + //keep on logging to document the crash... + } + } + + rcin_arm_prev = rcin.arm; +} + +void out_Mixer() { //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration /* - * Takes roll_PID, pitch_PID, and yaw_PID computed from the PID controller and appropriately mixes them for the desired - * vehicle configuration. For example on a quadcopter, the left two motors should have +roll_PID while the right two motors - * should have -roll_PID. Front two should have +pitch_PID and the back two should have -pitch_PID etc... every motor has - * normalized (0 to 1) rcin_thro command for throttle control. Can also apply direct unstabilized commands from the transmitter with - * rcin_xxx variables are to be sent to the motor ESCs and servos. + * Takes PIDroll.PID, PIDpitch.PID, and PIDyaw.PID computed from the PID controller and appropriately mixes them for the desired + * vehicle configuration. For example on a quadcopter, the left two motors should have +PIDroll.PID while the right two motors + * should have -PIDroll.PID. Front two should have +PIDpitch.PID and the back two should have -PIDpitch.PID etc... every motor has + * normalized (0 to 1) rcin.throttle command for throttle control. Can also apply direct unstabilized commands from the transmitter with + * rcin.xxx variables are to be sent to the motor ESCs and servos. * *Relevant variables: - *rcin_thro - direct thottle control - *roll_PID, pitch_PID, yaw_PID - stabilized axis variables - *rcin_roll, rcin_pitch, rcin_yaw - direct unstabilized command passthrough - *rcin_aux - free auxillary channel, can be used to toggle things with an 'if' statement + *rcin.throtle - direct thottle control + *PIDroll.PID, PIDpitch.PID, PIDyaw.PID - stabilized axis variables + *rcin.roll, rcin.pitch, rcin.yaw - direct unstabilized command passthrough + *rcin.flight_mode - can be used to toggle things with an 'if' statement */ /* Motor order diagram (Betaflight order) @@ -642,82 +370,13 @@ Roll right (left+ right-) --++ Yaw right (CCW+ CW-) -++- */ - //Quad mixing - EXAMPLE - out_command[MOTOR1] = rcin_thro - pitch_PID - roll_PID - yaw_PID; //Back Right CW - out_command[MOTOR2] = rcin_thro + pitch_PID - roll_PID + yaw_PID; //Front Right CCW - out_command[MOTOR3] = rcin_thro - pitch_PID + roll_PID + yaw_PID; //Back Left CCW - out_command[MOTOR4] = rcin_thro + pitch_PID + roll_PID - yaw_PID; //Front Left CW - - //0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle - //0.5 is centered servo, 0.0 and 1.0 are servo at their extreme positions as set with SERVO_MIN and SERVO_MAX - //out_command[SERVO1] = 0; - //out_command[SERVO2] = 0; -} - -void out_KillSwitchAndFailsafe() { - static bool rcin_armed_prev = true; //initial value is true: forces out_armed false on startup even if arm switch is ON - - //Change to ARMED when throttle is low and radio armed switch was flipped from disamed to armed position - if (!out_armed && rcin_thro_is_low && rcin_armed && !rcin_armed_prev) { - out_armed = true; - Serial.println("OUT: ARMED"); - bb.start(); //start blackbox logging - } - - //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection - if (out_armed && (!rcin_armed || !rcin.connected())) { - out_armed = false; - if(!rcin_armed) { - Serial.println("OUT: DISARMED (arm switch)"); - bb.stop(); //stop blackbox logging - }else{ - Serial.println("OUT: DISARMED (rcin lost connection)"); - } - } - - //If armed and throttle is low -> set motor outputs to out_armed_speed - if(out_armed && rcin_thro_is_low) for(int i=0;i STOP MOTORS - if(!out_armed) for(int i=0;i +// +// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. +// +//========================================================================================================================// + +/* <-- remove this to enable custom pins +//========================================================================================================================// +// CUSTOM PINS CONFIG // +//========================================================================================================================// + +#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! + +//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used +//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. + +//LED: +#define HW_PIN_LED pp +#define HW_LED_ON 0 //0:low is on, 1:high is on + +//IMU SPI: +#define HW_PIN_SPI_MISO pp +#define HW_PIN_SPI_MOSI pp +#define HW_PIN_SPI_SCLK pp +#define HW_PIN_IMU_CS pp +#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) + +//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) +#define HW_PIN_I2C_SDA pp +#define HW_PIN_I2C_SCL pp + +//Motor/Servo Outputs: +#define HW_OUT_COUNT 4 //number of outputs +#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. + +//Serial debug on USB Serial port (no GPIO pins) + +//RC Receiver: +#define HW_PIN_RCIN_RX pp +#define HW_PIN_RCIN_TX pp +#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets + +//GPS: +#define HW_PIN_GPS_RX pp +#define HW_PIN_GPS_TX pp +#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets + +//Battery ADC +#define HW_PIN_BAT_V pp +#define HW_PIN_BAT_I pp + +//Black Box SPI (for sdcard or external flash chip): +#define HW_PIN_SPI2_MISO pp +#define HW_PIN_SPI2_MOSI pp +#define HW_PIN_SPI2_SCLK pp +#define HW_PIN_BB_CS pp + +//Black Box SDCARD via MMC interface: +#define HW_PIN_SDMMC_DATA pp +#define HW_PIN_SDMMC_CLK pp +#define HW_PIN_SDMMC_CMD pp +//*/ + +//RP2040 specific options +//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking + +//ESP32 specific options +//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 + diff --git a/examples/03.PID-Tune/03.PID-Tune.ino b/examples/03.PID-Tune/03.PID-Tune.ino deleted file mode 100644 index f66e4b1..0000000 --- a/examples/03.PID-Tune/03.PID-Tune.ino +++ /dev/null @@ -1,775 +0,0 @@ -// This example a modified QuadcopterAdvanced.ino example, it adds PID-Tuning via RC. -// Search for "pidtune" in this file to see the changes. - -/*######################################################################################################################### - -NOTICE: This program is experimental - first get Quadcopter.ino to fly before attempting this program - -########################################################################################################################### - -See http://madflight.com for detailed description - -Motor order diagram (Betaflight order) - - front - CW --> <-- CCW - 4 2 - \ ^ / - |X| - / - \ - 3 1 -CCW --> <-- CW - -Arming: Set throttle low, then flip arm switch from DISARMED to ARMED. -Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch". - -LED State Meaning ---------- ------- -OFF Not powered -ON Startup (don't move, running gyro calibration) -Blinking long OFF short ON DISARMED -Blinking long ON short OFF ARMED -Blink interval longer than 1 second imu_loop() is taking too much time -fast blinking Something is wrong, connect USB serial for info - -MIT license -Copyright (c) 2023-2024 https://madflight.com -##########################################################################################################################*/ - - -//========================================================================================================================// -// PINS // -//========================================================================================================================// -// -// You have 3 options to setup the pins (gpio numbers) for the flight controller: -// -// 1) Default - Leave this section as is and see https://madflight.com for default pinout diagrams for the supported -// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h -// -// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available -// boards. For example: #include -// -// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. -// -//========================================================================================================================// - -/* -//========================================================================================================================// -// CUSTOM PINS // -//========================================================================================================================// - -#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! - -//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used -//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. - -//LED: -#define HW_PIN_LED pp -#define HW_LED_ON 0 //0:low is on, 1:high is on - -//IMU SPI: -#define HW_PIN_SPI_MISO pp -#define HW_PIN_SPI_MOSI pp -#define HW_PIN_SPI_SCLK pp -#define HW_PIN_IMU_CS pp -#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) - -//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) -#define HW_PIN_I2C_SDA pp -#define HW_PIN_I2C_SCL pp - -//Motor/Servo Outputs: -#define HW_OUT_COUNT 4 //number of outputs -#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. - -//Serial debug on USB Serial port (no GPIO pins) - -//RC Receiver: -#define HW_PIN_RCIN_RX pp -#define HW_PIN_RCIN_TX pp -#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets - -//GPS: -#define HW_PIN_GPS_RX pp -#define HW_PIN_GPS_TX pp -#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets - -//Battery ADC -#define HW_PIN_BAT_V pp -#define HW_PIN_BAT_I pp - -//Black Box SPI (for sdcard or external flash chip): -#define HW_PIN_SPI2_MISO pp -#define HW_PIN_SPI2_MOSI pp -#define HW_PIN_SPI2_SCLK pp -#define HW_PIN_BB_CS pp - -//Black Box SDCARD via MMC interface: -#define HW_PIN_SDMMC_DATA pp -#define HW_PIN_SDMMC_CLK pp -#define HW_PIN_SDMMC_CMD pp -//*/ - -//RP2040 specific options -//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking - -//ESP32 specific options -//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 - - -//========================================================================================================================// -// USER-SPECIFIED DEFINES // -//========================================================================================================================// - -//--- RC RECEIVER -#define RCIN_USE RCIN_USE_CRSF // RCIN_USE_CRSF, RCIN_USE_SBUS, RCIN_USE_DSM, RCIN_USE_PPM, RCIN_USE_PWM -//pidtune disable: #define RCIN_NUM_CHANNELS 6 //number of receiver channels (minimal 6) -#define RCIN_NUM_CHANNELS 7 //pidtune: add one channel for PID-tuning - -//--- IMU SENSOR -#define IMU_USE IMU_USE_SPI_MPU6500 // IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU9250,IMU_USE_SPI_MPU6000, IMU_USE_SPI_BMI270, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000 -//Set sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position. -//If not sure what is needed: use CLI 'proll' and try each setting until roll-right gives positive ahrs.roll, pitch-up gives positive ahrs.pitch, and yaw-right gives positive ahrs.yaw -#define IMU_ALIGN IMU_ALIGN_CW90 //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP -#define IMU_I2C_ADR 0x69 //IMU I2C address. If unknown, use CLI 'i2c' - -//-- AHRS sensor fusion -#define AHRS_USE AHRS_USE_MAHONY // AHRS_USE_MAHONY, AHRS_USE_MAHONY_BF, AHRS_USE_MADGWICK, AHRS_USE_VQF - -//--- GPS -#define GPS_BAUD 115200 - -//--- BAROMETER SENSOR -#define BARO_USE BARO_USE_NONE // BARO_USE_BMP280, BARO_USE_MS5611, BARO_USE_NONE -//#define BARO_I2C_ADR 0x76 //set barometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- EXTERNAL MAGNETOMETER SENSOR -#define MAG_USE MAG_USE_NONE // MAG_USE_QMC5883L, MAG_USE_NONE -//#define MAG_I2C_ADR 0x77 //set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE - -//--- BLACKBOX LOGGER -#define BB_USE BB_USE_NONE //BB_USE_SD spi sdcard, BB_USE_SDMMC mmc sdcard, BB_USE_NONE - -//========================================================================================================================// -// RC RECEIVER CONFIG // -//========================================================================================================================// - -//set channels -const int rcin_cfg_thro_channel = 1; //low pwm = zero throttle/stick back, high pwm = full throttle/stick forward -const int rcin_cfg_roll_channel = 2; //low pwm = left, high pwm = right -const int rcin_cfg_pitch_channel = 3; //low pwm = pitch up/stick back, high pwm = pitch down/stick forward -const int rcin_cfg_yaw_channel = 4; //low pwm = left, high pwm = right -const int rcin_cfg_arm_channel = 5; //ARM/DISARM switch -const int rcin_cfg_aux_channel = 6; //Fight mode - 6 position switch - -//throttle pwm values -const int rcin_cfg_thro_low = 1250; //used to set rcin_thro_is_low flag when pwm is below. Note: your craft won't arm if this is too low. -const int rcin_cfg_thro_max = 1900; -const float out_armed_speed = 0.2; //Safety feature: make props spin when armed, the motors spin at this speed when armed and throttle is low. The default 0.2 is probably fairly high, set lower as needed. - -//roll, pitch, yaw pwm values -const int rcin_cfg_pwm_min = 1150; -const int rcin_cfg_pwm_center = 1500; -const int rcin_cfg_pwm_max = 1900; -const int rcin_cfg_pwm_deadband = 0; //Amount of deadband around center, center-deadband to center+deadband will be interpreted as central stick. Set to 15 for PPM or 0 for jitter-free serial protocol receivers. - -//pwm range for arm switch in ARMED position -const int rcin_cfg_arm_min = 1600; -const int rcin_cfg_arm_max = 2200; - -//6 position switch on aux channel - Ardupilot switch pwm: 1165,1295,1425,1555,1685,1815 (spacing 130) -//EdgeTx 3-pos SA + 2-pos SB setup: Source:SA weight:52 offset:0, Source:SB weight:13 offset:-1 multiplex: add -OR- Source:SA Weight:26 Offset:-40 Switch:SBdown, Source:SA Weight:26 Offset:36 Switch:SBup Multiplex:Replace -int rcin_cfg_aux_min = 1165; //lowest switch pwm -int rcin_cfg_aux_max = 1815; //higest switch pwm - -//========================================================================================================================// -// USER-SPECIFIED VARIABLES // -//========================================================================================================================// - -//number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos -const int out_MOTOR_COUNT = 4; -//name the outputs, to make code more readable -enum out_enum {MOTOR1,MOTOR2,MOTOR3,MOTOR4,SERVO1,SERVO2,SERVO3,SERVO4,SERVO5,SERVO6,SERVO7,SERVO8,SERVO9,SERVO10,SERVO11,SERVO12}; - -const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate -const uint32_t baro_sample_rate = 100; //baro sample rate in Hz (default 100) - -//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing. -float LP_acc = 70; //Accelerometer (default MPU6050: 50Hz, MPU9250: 70Hz) -float LP_gyr = 60; //Gyro (default MPU6050: 35Hz, MPU9250: 60Hz) -float LP_mag = 1e10; //Magnetometer (default 1e10Hz, i.e. no filtering) -float LP_radio = 400; //Radio Input (default 400Hz) - -//Controller parameters (take note of defaults before modifying!): -float i_limit = 25.0; //Integrator saturation level, mostly for safety (default 25.0) -float maxRoll = 30.0; //Max roll angle in degrees for angle mode (maximum ~70 degrees) -float maxPitch = 30.0; //Max pitch angle in degrees for angle mode (maximum ~70 degrees) -float maxRollRate = 30.0; //Max roll rate in deg/sec for rate mode -float maxPitchRate = 30.0; //Max pitch rate in deg/sec for rate mode -float maxYawRate = 160.0; //Max yaw rate in deg/sec for angle and rate mode - -float Kp_ro_pi_angle = 0.2; //Roll/Pitch P-gain - angle mode -float Ki_ro_pi_angle = 0.1; //Roll/Pitch I-gain - angle mode -float Kd_ro_pi_angle = 0.05; //Roll/Pitch D-gain - angle mode (has no effect on control_Angle2) -float B_loop_ro_pi = 0.9; //Roll/Pitch damping term for control_Angle2(), lower is more damping (must be between 0 to 1) - -float Kp_ro_pi_rate = 0.15; //Roll/Pitch P-gain - rate mode -float Ki_ro_pi_rate = 0.2; //Roll/Pitch I-gain - rate mode -float Kd_ro_pi_rate = 0.0002; //Roll/Pitch D-gain - rate mode (be careful when increasing too high, motors will begin to overheat!) - -float Kp_yaw = 0.3; //Yaw P-gain -float Ki_yaw = 0.05; //Yaw I-gain -float Kd_yaw = 0.00015; //Yaw D-gain (be careful when increasing too high, motors will begin to overheat!) - -//========================================================================================================================// -// DECLARE GLOBAL VARIABLES // -//========================================================================================================================// - -//Radio communication: -int rcin_pwm[RCIN_NUM_CHANNELS]; //filtered raw PWM values -float rcin_thro, rcin_roll, rcin_pitch, rcin_yaw; //rcin_thro 0(cutoff) to 1(full); rcin_roll, rcin_pitch, rcin_yaw -1(left,down) to 1(right,up) with 0 center stick -bool rcin_armed; //status of arm switch, true = armed -bool rcin_thro_is_low; //status of throttle stick, true = throttle low -int rcin_aux; // six position switch connected to aux channel, values 0-5 - -//Controller: -float roll_PID = 0, pitch_PID = 0, yaw_PID = 0; - -//Flight status -bool out_armed = false; //motors will only run if this flag is true - -//Low pass filter parameters -float B_radio; - -//========================================================================================================================// -// INCLUDE MADFLIGHT LIBRARY // -//========================================================================================================================// -//Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern. -#include -#include "pidtune.h" - -//========================================================================================================================// -// SETUP() // -//========================================================================================================================// - -void setup() { - led.setup(HW_PIN_LED, HW_LED_ON); //Set built in LED to turn on to signal startup - Serial.begin(115200); //start console serial - - //6 second startup delay - for(int i=20;i>0;i--) { - Serial.printf("PID-Tune " MADFLIGHT_VERSION " starting %d ...\n", i); - delay(300); - } - Serial.printf("Arduino library: " HW_ARDUINO_STR "\n"); - - hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) - cfg.begin(); //read config from EEPROM - cli.print_boardInfo(); //print board info and pinout - cli.print_i2cScan(); //print i2c scan - rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. - - //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) - while(true) { - int rv = imu.setup(imu_sample_rate); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning - if(rv<=0) break; - warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); - } - - //set filter parameters after imu.setup(), as imu.setup() can modify requested sample rate - B_radio = Ahrs::lowpass_to_beta(LP_radio, imu.getSampleRate()); //Note: uses imu sample rate because radio filter is applied in imu_loop - - baro.setup(baro_sample_rate); //Barometer sample rate 100Hz - mag.setup(); //External Magnetometer - bat.setup(); //Battery Monitor - bb.setup(); //Black Box - gps_setup(); //GPS - //gps_debug(); //uncomment to debug gps messages - - //Servos (set servos first just in case motors overwrite frequency of shared timers) - for(int i=out_MOTOR_COUNT;i 100) { - rcin_telem_ts = millis(); - rcin_telem_cnt++; - //pidtune disable: if(out_armed) rcin_telemetry_flight_mode("ARMED"); else rcin_telemetry_flight_mode("madflight"); //only first 14 char get transmitted - rcin_telemetry_attitude(ahrs.pitch, ahrs.roll, ahrs.yaw); - if(rcin_telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100); - if(rcin_telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h - } - - cli.loop(); //process CLI commands -} - -//update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU -void i2c_sensors_update() { - if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. - if(baro.update()) bb.log_baro(); //log if pressure updated - mag.update(); -} - -//========================================================================================================================// -// IMU UPDATE LOOP // -//========================================================================================================================// - -//This is __MAIN__ function of this program. It is called when new IMU data is available. -void imu_loop() { - //Blink LED - led_Blink(); - - //Sensor fusion: update ahrs.roll, ahrs.pitch, and ahrs.yaw angle estimates (degrees) from IMU data - ahrs.update(); - - //Get radio state - rcin_GetCommands(); //Pulls current available radio commands - rcin_Normalize(); //Convert raw commands to normalized values based on saturated control limits - - //Uncomment to debug without remote (and no battery!) - pitch drone up: motors m1,m3 should increase and m2,m4 decrease; bank right: m1,m2 increase; yaw right: m1,m4 increase - //rcin_thro = 0.5; rcin_thro_is_low = false; rcin_roll = 0; rcin_pitch = 0; rcin_yaw = 0; rcin_armed = true; rcin_aux = 0; out_armed = true; - - //PID Controller - SELECT ONE: - control_Angle(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoint, stabilize yaw on rate setpoint //control_Angle2(rcin_thro_is_low); //Stabilize on pitch/roll setpoint using cascaded method. Rate controller must be tuned well first! - //control_Rate(rcin_thro_is_low); //Stabilize on rate setpoint - - //Actuator mixing - control_Mixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - - //Motor output - out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered. - out_SetCommands(); //Sends command pulses to motors (only if out_armed=true) and servos - - //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. - if (imu.usesI2C()) i2c_sensors_update(); - - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... -} - -//========================================================================================================================// -// IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_loop() // -//========================================================================================================================// - -void led_Blink() { - //Blink LED once per second, if LED blinks slower then the loop takes too much time, use print_loop_Rate() to investigate. - //DISARMED: long off, short on, ARMED: long on, short off - if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) - led.set(!out_armed); //short interval - else - led.set(out_armed); //long interval -} - -void rcin_GetCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the values are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - bool got_new_data = rcin.update(); - - //Low-pass the critical commands and update previous values - for(int i=0; i%d ",i+1,pwm_new[i],rcin_pwm[i]); Serial.println(); //uncomment for debugging - } -} - -void rcin_Normalize() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the normalized rcin variables rcin_thro, rcin_roll, rcin_pitch, and rcin_yaw. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. rcin_thro stays within 0 to 1 range. - * rcin_roll, rcin_pitch, and rcin_yaw are scaled to -1 to +1. - */ - - //normalized values - //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle - int pwm = rcin_pwm[rcin_cfg_thro_channel-1]; - rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), 0.0, 1.0); - rcin_thro_is_low = (pwm <= rcin_cfg_thro_low); - - //roll,pitch,yaw - rcin_roll = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_roll_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (roll left, stick left) to 1 (roll right, stick right) - rcin_pitch = - _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_pitch_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (pitch down, stick back) to 1 (pitch up, stick forward) - rcin_yaw = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_yaw_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (yaw left, stick left) to 1 (yaw right, stick right) - - //arm switch - pwm = rcin_pwm[rcin_cfg_arm_channel-1]; - rcin_armed = (rcin_cfg_arm_min <= pwm && pwm <= rcin_cfg_arm_max); - - //aux 6 position switch - int spacing = (rcin_cfg_aux_max - rcin_cfg_aux_min) / 5; - rcin_aux = ( rcin_pwm[rcin_cfg_aux_channel-1] - rcin_cfg_aux_min + spacing/2) / spacing; //output 0,1,2,3,4,5 -} - -//helper to nomalize a channel based on min,center,max calibration -float _rcin_ChannelNormalize(int val, int min, int center, int max, int deadband) { - int rev = 1; //1=normal, -1=reverse channel - //needs: min < center < max - if(val <-- CCW - 4 2 - \ ^ / - |X| - / - \ - 3 1 -CCW --> <-- CW - - M1234 -Pitch up (stick back) (front+ back-) -+-+ -Roll right (left+ right-) --++ -Yaw right (CCW+ CW-) -++- -*/ - - //Quad mixing - EXAMPLE - out_command[MOTOR1] = rcin_thro - pitch_PID - roll_PID - yaw_PID; //Back Right CW - out_command[MOTOR2] = rcin_thro + pitch_PID - roll_PID + yaw_PID; //Front Right CCW - out_command[MOTOR3] = rcin_thro - pitch_PID + roll_PID + yaw_PID; //Back Left CCW - out_command[MOTOR4] = rcin_thro + pitch_PID + roll_PID - yaw_PID; //Front Left CW - - //0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle - //0.5 is centered servo, 0.0 and 1.0 are servo at their extreme positions as set with SERVO_MIN and SERVO_MAX - //out_command[SERVO1] = 0; - //out_command[SERVO2] = 0; -} - -void out_KillSwitchAndFailsafe() { - static bool rcin_armed_prev = true; //initial value is true: forces out_armed false on startup even if arm switch is ON - - //Change to ARMED when throttle is low and radio armed switch was flipped from disamed to armed position - if (!out_armed && rcin_thro_is_low && rcin_armed && !rcin_armed_prev) { - out_armed = true; - Serial.println("OUT: ARMED"); - bb.start(); //start blackbox logging - } - - //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection - if (out_armed && (!rcin_armed || !rcin.connected())) { - out_armed = false; - if(!rcin_armed) { - Serial.println("OUT: DISARMED (arm switch)"); - bb.stop(); //stop blackbox logging - }else{ - Serial.println("OUT: DISARMED (rcin lost connection)"); - } - } - - //If armed and throttle is low -> set motor outputs to out_armed_speed - if(out_armed && rcin_thro_is_low) for(int i=0;i STOP MOTORS - if(!out_armed) for(int i=0;i pidtune_cnt-1) pidtune_idx = 0; - updated = true; - } else if(cmd_last == PIDTUNE_NONE && cmd == PIDTUNE_PREVVAR) { - pidtune_idx--; - if(pidtune_idx < 0) pidtune_idx = pidtune_cnt-1; - updated = true; - } else if(cmd_last == PIDTUNE_NONE && cmd == PIDTUNE_DECVALUE) { - *tunevar.pvalue *= (1.0f/1.1f); //decrease by 10% - updated = true; - } else if(cmd_last == PIDTUNE_NONE && cmd == PIDTUNE_INCVALUE) { - *tunevar.pvalue *= 1.1f; //increase by 10% - updated = true; - } else if(cmd == PIDTUNE_RESETVALUE) { - *tunevar.pvalue = tunevar.value_reset; - updated = true; - } - cmd_last = cmd; - - return updated; -} - -String pidtune_status() { - pidtune_s tunevar = pidtune[pidtune_idx]; - return tunevar.name + String(*tunevar.pvalue,5); -} - -void pidtune_loop() { - if(pidtune_update()) rcin_telemetry_flight_mode(pidtune_status().c_str()); -} diff --git a/examples/03.Plane/03.Plane.ino b/examples/03.Plane/03.Plane.ino new file mode 100644 index 0000000..103c63e --- /dev/null +++ b/examples/03.Plane/03.Plane.ino @@ -0,0 +1,462 @@ +/*######################################################################################################################### + +WARNING: This program is experimental - it was only flight-tested in a couple of flights + +This program is an airplane controller, it has 3 flight modes: MANUAL, ROLL and FBWA. + +## MANUAL Mode + +Regular RC control, no stabilization. All RC inputs are passed through to the servo outputs. + +## ROLL Mode + +Stabilize roll angle + +## FBWA Fly By Wire A Mode (inspired by ArduPilot) + +This is the most popular mode for assisted flying, and is the best mode for inexperienced flyers. In this mode the +plane will hold the roll and pitch specified by the control sticks. So if you hold the aileron stick hard right then the +plane will hold its pitch level and will bank right by the angle specified in the roll limit parameter. It is not possible +to roll the plane past the roll limit, and it is not possible to pitch the plane beyond the pitch limit settings. + +Note that holding level pitch does not mean the plane will hold altitude. How much altitude a plane gains or loses at a +particular pitch depends on its airspeed, which is primarily controlled by throttle. So to gain altitude you should raise +the throttle, and to lose altitude you should lower the throttle. + +In FBWA mode the rudder is under manual control. + +## Setup Procedure + +First edit sections "PINS", "BOARD", "HARDWARE", "RC RECEIVER", "OUTPUTS". Use CLI to verify things work as expected. + +Calibrate gyro, accelerometer, and magnetometer --- important!!! + +Connect power and then let plane sit for 15 seconds, during this time the gyro biases are re-calibrated. + +Do a dry run: + +Set to MANUAL and power up the plane. Move the rc controls and make sure that the aileron, elevator, and rudder move in +the correct direction. Arm the plane, and carefully test the motor, then disarm. +If incorrect: modify the #define OUT_ELEVATOR_DOWN etc. statements. + +Then set to FBWA flight mode, keep the radio sticks centered, and move the plane around, to make sure that the control +surfaces work to oppose the move, that is: pitching the plane down should move elevator up, banking right should deflect +the right aileron down, left aileron up. + +Another thing that needs to be set are the PID parameters. Set to ROLL or FBWA mode and adjust the PID parameters so that the +control surfaces react quickly, but don't oscillate, on changes in attitude. + +########################################################################################################################### + +See http://madflight.com for detailed description + +Arming: Set throttle low, then flip arm switch from DISARMED to ARMED. +Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch". + +LED State Meaning +--------- ------- +OFF Not powered +ON Startup (don't move, running gyro calibration) +Blinking long OFF short ON DISARMED +Blinking long ON short OFF ARMED +Blink interval longer than 1 second imu_loop() is taking too much time +fast blinking Something is wrong, connect USB serial for info + +MIT license +Copyright (c) 2024 https://madflight.com +##########################################################################################################################*/ + +#include "madflight_config.h" //Edit this header file to setup the pins, hardware, radio, etc. for madflight +#include + +//========================================================================================================================// +// OUTPUTS // +//========================================================================================================================// + +//number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos +const int out_MOTOR_COUNT = 1; + +//define outputs and their channels 1..HW_OUT_COUNT (see control_Mixer() for available outputs) +//select output name based on what the output does when pwm is high. For example: If the right aileron goes down on high +//pwm and is connected to output channel 2 use #define OUT_RIGHT_AILERON_DOWN 2 +#define OUT_MOTOR1 1 //full throttle on high pwm +#define OUT_LEFT_AILERON_UP 2 //left aileron deflects up on high pwm (and right aileron down, otherwise use two servo channels) +//#define OUT_RIGHT_AILERON_DOWN 3 //right aileron deflects down on high pwm +#define OUT_ELEVATOR_UP 3 //elevator deflects up on high pwm +#define OUT_RUDDER_LEFT 4 //rudder deflects left on high pwm +#define OUT_FLAPS_UP_HALF 5 //flaps deflect up on high pwm, but only use 0.5 to 1.0 servo range + +//========================================================================================================================// +// USER-SPECIFIED VARIABLES // +//========================================================================================================================// + +//flight modes +enum flightmode_enum {MANUAL, ROLL, FBWA}; //available flight modes: MANUAL send rc commands directly to motor and aileron/pitch/yaw servos, ROLL stabilize roll angle, FBWA stabilize roll/pitch angles +const char* flightmode_str[] = {"MANUAL", "ROLL", "FBWA"}; //flight mode names used for telemetry +flightmode_enum flightmode_map[6] {MANUAL, MANUAL, ROLL, ROLL, FBWA, FBWA}; //flightmode mapping from 6 pos switch to flight mode (simulates a 3-pos switch: MANUAL/ROLL/FBWA) +flightmode_enum flightmode = MANUAL; //current flight mode + +//Controller parameters (take note of defaults before modifying!): +float i_limit = 25; //PID Integrator saturation level, mostly for safety +float maxRoll = 45; //Max roll angle in degrees for ROLL, FBWA modes +float maxPitch = 30; //Max pitch angle in degrees for FBWA mode +float fbwa_pitch_offset = 3; //FBWA pitch up angle for neutral stick + +//roll PID constants +float Kp_roll = (1.0/90); //Roll P-gain - apply full aileron on 90 degree roll error +float Ki_roll = 0; //Roll I-gain +float Kd_roll = (1.0/180); //Roll D-gain - apply full opposite aileron when roll rate is 180 degrees/sec towards desired setpoint + +//pitch PID constants +float Kp_pitch = (1.0/30); //Pitch P-gain - apply full elevator on 30 degree pitch error +float Ki_pitch = 0; //Pitch I-gain +float Kd_pitch = (1.0/90); //Pitch D-gain - apply full opposite elevator when pitch rate is 90 degrees/sec towards desired setpoint + +//Radio communication: +float rcin_flaps; //flaps 0.0:up, 1.0:full down + +//========================================================================================================================// +// SETUP() // +//========================================================================================================================// + +void setup() { + //setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component. + madflight_setup(); + + //Servos (set servos first just in case motors overwrite frequency of shared timers) + for(int i=out_MOTOR_COUNT;i 100) { + telem_ts = millis(); + telem_cnt++; + String fm_str = String(out.armed ? "*" : "") + (gps.sat>0 ? String(gps.sat) : String("")) + flightmode_str[flightmode]; + rcin_telemetry_flight_mode(fm_str.c_str()); //only first 14 char get transmitted + rcin_telemetry_attitude(ahrs.pitch, ahrs.roll, ahrs.yaw); + if(telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100); + if(telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h + } + + //logging + static uint32_t log_ts = 0; + if(millis() - log_ts > 100) { + log_ts = millis(); + bb.log_sys(); + } + + cli.loop(); //process CLI commands +} + +//update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU +void i2c_sensors_update() { + if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. + if(baro.update()) bb.log_baro(); //log if pressure updated + mag.update(); +} + +//========================================================================================================================// +// IMU UPDATE LOOP // +//========================================================================================================================// + +//This is __MAIN__ function of this program. It is called when new IMU data is available. +void imu_loop() { + //Blink LED + led_Blink(); + + //Sensor fusion: update ahrs.roll, ahrs.pitch, and ahrs.yaw angle estimates (degrees) from IMU data + ahrs.update(); + + //Get radio commands - Note: don't do this in loop() because loop() is a lower priority task than imu_loop(), so in worst case loop() will not get any processor time. + rcin.update(); + flightmode = flightmode_map[rcin.flightmode]; //map rcin.flightmode (0 to 5) to vehicle flightmode + + //PID Controller + switch(flightmode) { + case ROLL: + control_ROLL(rcin.throttle == 0); //Stabilize on roll angle setpoints + break; + case FBWA: + control_FBWA(rcin.throttle == 0); //Stabilize on pitch/roll angle setpoints + break; + default: + control_MANUAL(); + } + + //Updates out.arm, the output armed flag + out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered. + + //Actuator mixing + out_Mixer(); //Mixes PID outputs and sends command pulses to the motors, if mot.arm == true + + //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. + if (imu.usesI2C()) i2c_sensors_update(); + + //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... +} + +//========================================================================================================================// +// IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_loop() // +//========================================================================================================================// + +void led_Blink() { + //Blink LED once per second, if LED blinks slower then the loop takes too much time, use print_loop_Rate() to investigate. + //DISARMED: long off, short on, ARMED: long on, short off + if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) + led.set(!out.armed); //short interval + else + led.set(out.armed); //long interval +} + +void control_FBWA(bool zero_integrators) { +/* FBWA Fly By Wire A Mode (inspired by ArduPilot) +This is the most popular mode for assisted flying, and is the best mode for inexperienced flyers. In this mode the +plane will hold the roll and pitch specified by the control sticks. So if you hold the aileron stick hard right then the +plane will hold its pitch level and will bank right by the angle specified in the roll limit parameter. It is not possible +to roll the plane past the roll limit, and it is not possible to pitch the plane beyond the pitch limit settings. + +Note that holding level pitch does not mean the plane will hold altitude. How much altitude a plane gains or loses at a +particular pitch depends on its airspeed, which is primarily controlled by throttle. So to gain altitude you should raise +the throttle, and to lose altitude you should lower the throttle. + +In FBWA mode the rudder is under manual control. +*/ + + //inputs: roll_des, pitch_des, yawRate_des + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID + + //desired values + float roll_des = rcin.roll * maxRoll; //Between -maxRoll and +maxRoll + float pitch_des = rcin.pitch * maxPitch + fbwa_pitch_offset; //Between fbwa_pitch_offset-maxPitch and fbwa_pitch_offset+maxPitch + + //state vars + static float integral_roll, integral_pitch, error_yaw_prev, integral_yaw; + + //Zero the integrators (used to don't let integrator build if throttle is too low, or to re-start the controller) + if(zero_integrators) { + integral_roll = 0; + integral_pitch = 0; + integral_yaw = 0; + } + + //Roll PID - stabilize desired roll angle + float error_roll = roll_des - ahrs.roll; + integral_roll += error_roll * imu.dt; + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + float derivative_roll = ahrs.gx; + PIDroll.PID = Kp_roll*error_roll + Ki_roll*integral_roll - Kd_roll*derivative_roll; //nominal output -1 to 1 (can be larger) + + //Pitch PID - stabilize desired pitch angle + float error_pitch = pitch_des - ahrs.pitch; + integral_pitch += error_pitch * imu.dt; + integral_pitch = constrain(integral_pitch, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + float derivative_pitch = ahrs.gy; + PIDpitch.PID = Kp_pitch*error_pitch + Ki_pitch*integral_pitch - Kd_pitch*derivative_pitch; //nominal output -1 to 1 (can be larger) + + //Yaw PID - passthru rcin + PIDyaw.PID = rcin.yaw; + (void) integral_yaw; + (void) error_yaw_prev; + + /* + //TODO Yaw PID - Stabilize on zero slip, i.e. keep gravity Y component zero + float error_yaw = 0 - ahrs.ay; + integral_yaw += error_yaw * imu.dt; + integral_yaw = constrain(integral_yaw, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + float derivative_yaw = (error_yaw - error_yaw_prev) / imu.dt; + PIDyaw.PID = constrain(0.01 * (Kp_yaw*error_yaw + Ki_yaw*integral_yaw + Kd_yaw*derivative_yaw), -1.0f, 1.0f); //Scaled by .01 to bring within -1 to 1 range + + //Update derivative variables + error_yaw_prev = error_yaw; + */ +} + +void control_ROLL(bool zero_integrators) { + //inputs: roll_des, pitch_des, yawRate_des + //outputs: PIDroll.PID, PIDpitch.PID, PIDyaw.PID + + //desired values + float roll_des = rcin.roll * maxRoll; //Between -maxRoll and +maxRoll + + //state vars + static float integral_roll; + + //Zero the integrators (used to don't let integrator build if throttle is too low, or to re-start the controller) + if(zero_integrators) { + integral_roll = 0; + } + + //Roll PID - stabilize desired roll angle + float error_roll = roll_des - ahrs.roll; + integral_roll += error_roll * imu.dt; + integral_roll = constrain(integral_roll, -i_limit, i_limit); //Saturate integrator to prevent unsafe buildup + float derivative_roll = ahrs.gx; + PIDroll.PID = Kp_roll*error_roll + Ki_roll*integral_roll - Kd_roll*derivative_roll; //nominal output -1 to 1 (can be larger) + + //Pitch PID - passthru rcin + PIDpitch.PID = rcin.pitch; + + //Yaw PID - passthru rcin + PIDyaw.PID = rcin.yaw; +} + + +void control_MANUAL() { +/* MANUAL Mode +Regular RC control, no stabilization. All RC inputs are passed through to the servo outputs. +*/ + //pass rcin through to PID - PID values are -1 to +1, rcin values are -1 to +1 + PIDroll.PID = rcin.roll; //-1 = left, 1 = right + PIDpitch.PID = rcin.pitch; //-1 = pitch up/stick back, 1 = pitch down/stick forward + PIDyaw.PID = rcin.yaw; //-1 = left, 1 = right +} + +void out_KillSwitchAndFailsafe() { + static bool rcin_arm_prev = true; //initial value is true: forces out.armed false on startup even if arm switch is ON + + //Change to ARMED when throttle is zero and radio armed switch was flipped from disamed to armed position + if (!out.armed && rcin.throttle == 0 && rcin.arm && !rcin_arm_prev) { + out.armed = true; + Serial.println("OUT: ARMED"); + bb.start(); //start blackbox logging + } + + //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection + if (out.armed && (!rcin.arm || !rcin.connected())) { + out.armed = false; + if(!rcin.arm) { + Serial.println("OUT: DISARMED"); + bb.stop(); //stop blackbox logging + }else{ + Serial.println("OUT: DISARMED due to lost radio connection"); + //keep on logging to document the crash... + } + } + + rcin_arm_prev = rcin.arm; +} + +void out_Mixer() { + //DESCRIPTION: Mixes scaled commands from PID controller to actuator outputs based on vehicle configuration + /* + * Takes PIDroll.PID, PIDpitch.PID, and PIDyaw.PID computed from the PID controller and appropriately mixes them for the desired + * vehicle configuration. For example on a quadcopter, the left two motors should have +PIDroll.PID while the right two motors + * should have -PIDroll.PID. Front two should have +PIDpitch.PID and the back two should have -PIDpitch.PID etc... every motor has + * normalized (0 to 1) rcin.throttle command for throttle control. Can also apply direct unstabilized commands from the transmitter with + * rcin_xxx variables are to be sent to the motor ESCs and servos. + * + *Relevant variables: + *rcin.throttle - direct thottle control + *PIDroll.PID, PIDpitch.PID, PIDyaw.PID - stabilized axis variables + *rcin.roll, rcin.pitch, rcin.yaw - direct unstabilized command passthrough + + rcin.throttle 0: idle throttle/stick back 1: full throttle/stick forward + PIDroll.PID -1: roll left/stick left 1: roll right/stick right + PIDpitch.PID -1: pitch up/stick back 1: pitch down/stick forward + PIDyaw.PID -1: yaw left/stick left 1: yaw right/stick right + */ + + //Plane mixing - PID values are -1 to +1 (nominal), SERVO values are 0 to 1 (clipped by pwm class) + + //motor: full throttle on rcin.throttle + #ifdef OUT_MOTOR1 //full throttle on high pwm + out.set(OUT_MOTOR1-1, +rcin.throttle); + #endif + #ifdef OUT_MOTOR1_REVERSED //reversed: idle throttle on high pwm + out.set(OUT_MOTOR1_REVERSED-1, 1.0 - rcin.throttle); + #endif + #ifdef OUT_MOTOR2 //full throttle on high pwm + out.set(OUT_MOTOR2-1, +rcin.throttle); + #endif + #ifdef OUT_MOTOR2_REVERSED //reversed: idle throttle on high pwm + out.set(OUT_MOTOR2_REVERSED-1, 1.0 - rcin.throttle); + #endif + + //aileron: when PIDroll.PID positive -> roll right -> deflect left aileron down, deflect right aileron up + #ifdef OUT_LEFT_AILERON_DOWN //left aileron deflects down on high pwm + out.set(OUT_LEFT_AILERON_DOWN-1, 0.5 +PIDroll.PID/2.0); + #endif + #ifdef OUT_RIGHT_AILERON_UP //right aileron deflects up on high pwm + out.set(OUT_RIGHT_AILERON_UP-1, 0.5 +PIDroll.PID/2.0); + #endif + #ifdef OUT_LEFT_AILERON_UP //reversed: left aileron deflects up on high pwm + out.set(OUT_LEFT_AILERON_UP-1, 0.5 -PIDroll.PID/2.0); + #endif + #if defined(OUT_RIGHT_AILERON_DOWN) //reversed: right aileron deflects down on high pwm + out.set(OUT_RIGHT_AILERON_DOWN-1, 0.5 -PIDroll.PID/2.0); + #endif + + //elevator: when PIDpitch.PID is positive -> pitch up -> deflect elevator down + #ifdef OUT_ELEVATOR_DOWN //elevator deflects down on high pwm + out.set(OUT_ELEVATOR_UP-1, +PIDpitch.PID/2.0 + 0.5); + #endif + #ifdef OUT_ELEVATOR_UP //reversed: elevator deflects up on high pwm + out.set(OUT_ELEVATOR_UP-1, -PIDpitch.PID/2.0 + 0.5); + #endif + + //rudder: when PIDyaw.PID is positive -> yaw right -> deflect rudder right + #ifdef OUT_RUDDER_RIGHT //rudder deflects right on high pwm + out.set(OUT_RUDDER_RIGHT-1, +PIDyaw.PID/2.0 + 0.5); + #endif + #ifdef OUT_RUDDER_LEFT //reversed: rudder deflects left on high pwm + out.set(OUT_RUDDER_LEFT-1, -PIDyaw.PID/2.0 + 0.5); + #endif + + //flaps: (rcin_flaps 0.0:up, 1.0:full down) + #ifdef OUT_FLAPS_DOWN //flaps deflect down on high pwm (flaps use full servo range 0.0 to 1.0) + float rcin_flaps = constrain( ((float)(rcin.pwm[OUT_FLAPS_DOWN-1] - 1100)) / (1900 - 1100), 0.0, 1.0); //output: 0.0 to 1.0 + out.set(OUT_FLAPS_DOWN-1, +rcin_flaps); + #endif + #ifdef OUT_FLAPS_DOWN_HALF //flaps deflect down on high pwm (flaps only use servo range 0.5 to 1.0) + float rcin_flaps = constrain( ((float)(rcin.pwm[OUT_FLAPS_DOWN_HALF-1] - 1100)) / (1900 - 1100), 0.0, 1.0); //output: 0.0 to 1.0 + out.set(OUT_FLAPS_DOWN_HALF-1, 0.5 + rcin_flaps/2.0); + #endif + #ifdef OUT_FLAPS_UP //reversed: flaps deflect up on high pwm (flaps use full servo range 0.0 to 1.0) + float rcin_flaps = constrain( ((float)(rcin.pwm[OUT_FLAPS_UP-1] - 1100)) / (1900 - 1100), 0.0, 1.0); //output: 0.0 to 1.0 + out.set(OUT_FLAPS_UP-1, -rcin_flaps); + #endif + #ifdef OUT_FLAPS_UP_HALF //reversed: flaps deflect up on high pwm (flaps only use servo range 0.5 to 1.0) + float rcin_flaps = constrain( ((float)(rcin.pwm[OUT_FLAPS_UP_HALF-1] - 1100)) / (1900 - 1100), 0.0, 1.0); //output: 0.0 to 1.0 + out.set(OUT_FLAPS_UP_HALF-1, 0.5 - rcin_flaps/2.0); + #endif + + //delta wing: + // when PIDroll.PID positive -> roll right -> deflect left elevon down, deflect right elevon up + // when PIDpitch.PID is positive -> pitch up -> deflect left elevon down, deflect right elevon down + #ifdef OUT_LEFT_ELEVON_DOWN //left elevon deflects down on high input + out.set(OUT_LEFT_ELEVON_DOWN-1, 0.5 +PIDroll.PID/2.0 +PIDpitch.PID/2.0); + #endif + #ifdef OUT_RIGHT_ELEVON_UP //right elevon deflects up on high input + out.set(OUT_RIGHT_ELEVON_UP-1, 0.5 +PIDroll.PID/2.0 -PIDpitch.PID/2.0); + #endif + #ifdef OUT_LEFT_ELEVON_UP //reversed: left elevon deflects down on high input + out.set(OUT_LEFT_ELEVON_UP-1, 0.5 -PIDroll.PID/2.0 -PIDpitch.PID/2.0); + #endif + #ifdef OUT_RIGHT_ELEVON_DOWN //reversed: right elevon deflects down on high input + out.set(OUT_RIGHT_ELEVON_DOWN-1, 0.5 -PIDroll.PID/2.0 +PIDpitch.PID/2.0); + #endif + + //0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle + //0.5 is centered servo, 0.0 and 1.0 are servo at their extreme positions as set with SERVO_MIN and SERVO_MAX +} diff --git a/examples/03.Plane/madflight_config.h b/examples/03.Plane/madflight_config.h new file mode 100644 index 0000000..4e06e63 --- /dev/null +++ b/examples/03.Plane/madflight_config.h @@ -0,0 +1,127 @@ +//========================================================================================================================// +// HARDWARE CONFIG // +//========================================================================================================================// + +//--- RC RECEIVER +#define RCIN_USE RCIN_USE_CRSF // Select one: RCIN_USE_CRSF, RCIN_USE_SBUS, RCIN_USE_DSM, RCIN_USE_PPM, RCIN_USE_PWM +#define RCIN_NUM_CHANNELS 8 // Number of channels +#define RCIN_STICK_DEADBAND 0 // Deadband for centering sticksD: pwm center-deadband to pwm center+deadband will be interpreted as central stick. Set to 15 for PPM or 0 for jitter-free serial protocol receivers. + +//--- IMU SENSOR +// IMU_ALIGN is the sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position. +// If not sure what is needed: use CLI 'pahrs' and try each setting until roll-right gives positive ahrs.roll, pitch-up gives positive ahrs.pitch, and yaw-right gives positive ahrs.yaw +#define IMU_USE IMU_USE_SPI_MPU6500 // Select one: IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU9250, IMU_USE_SPI_MPU6000, IMU_USE_SPI_BMI270, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000 +#define IMU_ALIGN IMU_ALIGN_CW90 // Select one: IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP +#define IMU_I2C_ADR 0x69 // IMU I2C address. If unknown, use CLI 'i2c' + +//-- AHRS sensor fusion +#define AHRS_USE AHRS_USE_MAHONY // Select one: AHRS_USE_MAHONY, AHRS_USE_MAHONY_BF, AHRS_USE_MADGWICK, AHRS_USE_VQF + +//--- GPS +#define GPS_BAUD 115200 + +//--- BAROMETER SENSOR +#define BARO_USE BARO_USE_NONE // Select one: BARO_USE_BMP390, BARO_USE_BMP388, BARO_USE_BMP280, BARO_USE_MS5611, BARO_USE_NONE +//#define BARO_I2C_ADR 0x76 // Set barometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' + +//--- EXTERNAL MAGNETOMETER SENSOR +#define MAG_USE MAG_USE_NONE // Select one: MAG_USE_QMC5883L, MAG_USE_NONE +//#define MAG_I2C_ADR 0x77 // Set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' + +//--- BATTERY MONITOR +#define BAT_USE BAT_USE_NONE // Select one: BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE + +//--- BLACKBOX LOGGER +#define BB_USE BB_USE_NONE // Select one: BB_USE_SD, BB_USE_SDMMC, BB_USE_NONE + +//========================================================================================================================// +// MISC CONFIG // +//========================================================================================================================// + +#define IMU_SAMPLE_RATE 1000 //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate +#define BARO_SAMPLE_RATE 100 //baro sample rate in Hz (default 100) + +//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing. +#define IMU_ACC_LP_HZ 70 //Accelerometer (default 70Hz) +#define IMU_GYR_LP_HZ 60 //Gyro (default 60Hz) +#define IMU_MAG_LP_HZ 1e10 //Magnetometer (default 1e10Hz, i.e. no filtering) + +//========================================================================================================================// +// PINS CONFIG // +//========================================================================================================================// +// +// You have 3 options to setup the pins (gpio numbers) for the flight controller: +// +// 1) Default - Leave this section as is and see https://madflight.com for default pinout diagrams for the supported +// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h +// +// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available +// boards. For example: #include +// +// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. +// +//========================================================================================================================// + +/* <-- remove this to enable custom pins +//========================================================================================================================// +// CUSTOM PINS CONFIG // +//========================================================================================================================// + +#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! + +//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used +//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. + +//LED: +#define HW_PIN_LED pp +#define HW_LED_ON 0 //0:low is on, 1:high is on + +//IMU SPI: +#define HW_PIN_SPI_MISO pp +#define HW_PIN_SPI_MOSI pp +#define HW_PIN_SPI_SCLK pp +#define HW_PIN_IMU_CS pp +#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) + +//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) +#define HW_PIN_I2C_SDA pp +#define HW_PIN_I2C_SCL pp + +//Motor/Servo Outputs: +#define HW_OUT_COUNT 4 //number of outputs +#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. + +//Serial debug on USB Serial port (no GPIO pins) + +//RC Receiver: +#define HW_PIN_RCIN_RX pp +#define HW_PIN_RCIN_TX pp +#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets + +//GPS: +#define HW_PIN_GPS_RX pp +#define HW_PIN_GPS_TX pp +#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets + +//Battery ADC +#define HW_PIN_BAT_V pp +#define HW_PIN_BAT_I pp + +//Black Box SPI (for sdcard or external flash chip): +#define HW_PIN_SPI2_MISO pp +#define HW_PIN_SPI2_MOSI pp +#define HW_PIN_SPI2_SCLK pp +#define HW_PIN_BB_CS pp + +//Black Box SDCARD via MMC interface: +#define HW_PIN_SDMMC_DATA pp +#define HW_PIN_SDMMC_CLK pp +#define HW_PIN_SDMMC_CMD pp +//*/ + +//RP2040 specific options +//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking + +//ESP32 specific options +//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 + diff --git a/examples/04.Plane/04.Plane.ino b/examples/04.Plane/04.Plane.ino deleted file mode 100644 index 4164708..0000000 --- a/examples/04.Plane/04.Plane.ino +++ /dev/null @@ -1,793 +0,0 @@ -/*######################################################################################################################### - -WARNING: This program is experimental - it was only flight-tested in a couple of flights - -This program is an airplane controller, it has 3 flight modes: MANUAL, ROLL and FBWA. - -## MANUAL Mode - -Regular RC control, no stabilization. All RC inputs are passed through to the servo outputs. - -## ROLL Mode - -Stabilize roll angle - -## FBWA Fly By Wire A Mode (inspired by ArduPilot) - -This is the most popular mode for assisted flying, and is the best mode for inexperienced flyers. In this mode the -plane will hold the roll and pitch specified by the control sticks. So if you hold the aileron stick hard right then the -plane will hold its pitch level and will bank right by the angle specified in the roll limit parameter. It is not possible -to roll the plane past the roll limit, and it is not possible to pitch the plane beyond the pitch limit settings. - -Note that holding level pitch does not mean the plane will hold altitude. How much altitude a plane gains or loses at a -particular pitch depends on its airspeed, which is primarily controlled by throttle. So to gain altitude you should raise -the throttle, and to lose altitude you should lower the throttle. - -In FBWA mode the rudder is under manual control. - -## Setup Procedure - -First edit sections "PINS", "BOARD", "HARDWARE", "RC RECEIVER", "OUTPUTS". Use CLI to verify things work as expected. - -Calibrate gyro, accelerometer, and magnetometer --- important!!! - -Connect power and then let plane sit for 15 seconds, during this time the gyro biases are re-calibrated. - -Do a dry run: - -Set to MANUAL and power up the plane. Move the rc controls and make sure that the aileron, elevator, and rudder move in -the correct direction. Arm the plane, and carefully test the motor, then disarm. -If incorrect: modify the #define OUT_ELEVATOR_DOWN etc. statements. - -Then set to FBWA flight mode, keep the radio sticks centered, and move the plane around, to make sure that the control -surfaces work to oppose the move, that is: pitching the plane down should move elevator up, banking right should deflect -the right aileron down, left aileron up. - -Another thing that needs to be set are the PID parameters. Set to ROLL or FBWA mode and adjust the PID parameters so that the -control surfaces react quickly, but don't oscillate, on changes in attitude. - -########################################################################################################################### - -See http://madflight.com for detailed description - -Arming: Set throttle low, then flip arm switch from DISARMED to ARMED. -Disarming: Flip arm switch from ARMED to DISARMED, at any throttle position. "Kill switch". - -LED State Meaning ---------- ------- -OFF Not powered -ON Startup (don't move, running gyro calibration) -Blinking long OFF short ON DISARMED -Blinking long ON short OFF ARMED -Blink interval longer than 1 second imu_loop() is taking too much time -fast blinking Something is wrong, connect USB serial for info - -MIT license -Copyright (c) 2024 https://madflight.com -##########################################################################################################################*/ - - -//========================================================================================================================// -// PINS // -//========================================================================================================================// -// -// You have 3 options to setup the pins (gpio numbers) for the flight controller: -// -// 1) Default - Leave this section as is and see https://madflight.com for default pinout diagrams for the supported -// processor families. Default pinouts are defined in the board header files library/src/madflight_board_default_XXX.h -// -// 2) Header - #include the BetaFlight flight controller you want to use. See library/madflight/src for all available -// boards. For example: #include -// -// 3) Custom - Remove /* below to enable the CUSTOM PINS section, and define own pinout. -// -//========================================================================================================================// - -/* -//========================================================================================================================// -// CUSTOM PINS // -//========================================================================================================================// - -#define HW_BOARD_NAME "My Custom Board" //REQUIRED: Give your board a name - without a name the default pinout is loaded!!! - -//Replace 'pp' with the gpio number you want to use, or comment out the #define if the pin is not used -//NOTE: Not all pins can be freely configured. Read the processor datasheet, or use the default pinout. - -//LED: -#define HW_PIN_LED pp -#define HW_LED_ON 0 //0:low is on, 1:high is on - -//IMU SPI: -#define HW_PIN_SPI_MISO pp -#define HW_PIN_SPI_MOSI pp -#define HW_PIN_SPI_SCLK pp -#define HW_PIN_IMU_CS pp -#define HW_PIN_IMU_EXTI pp //REQUIRED: IMU external interrupt pin (required for SPI and I2C sensors) - -//I2C for BARO, MAG, BAT sensors (and for IMU if not using SPI IMU) -#define HW_PIN_I2C_SDA pp -#define HW_PIN_I2C_SCL pp - -//Motor/Servo Outputs: -#define HW_OUT_COUNT 4 //number of outputs -#define HW_PIN_OUT_LIST {pp,pp,pp,pp} //list of output pins, enter exactly HW_OUT_COUNT pins. - -//Serial debug on USB Serial port (no GPIO pins) - -//RC Receiver: -#define HW_PIN_RCIN_RX pp -#define HW_PIN_RCIN_TX pp -#define HW_PIN_RCIN_INVERTER pp //only used for STM32 targets - -//GPS: -#define HW_PIN_GPS_RX pp -#define HW_PIN_GPS_TX pp -#define HW_PIN_GPS_INVERTER pp //only used for STM32 targets - -//Battery ADC -#define HW_PIN_BAT_V pp -#define HW_PIN_BAT_I pp - -//Black Box SPI (for sdcard or external flash chip): -#define HW_PIN_SPI2_MISO pp -#define HW_PIN_SPI2_MOSI pp -#define HW_PIN_SPI2_SCLK pp -#define HW_PIN_BB_CS pp - -//Black Box SDCARD via MMC interface: -#define HW_PIN_SDMMC_DATA pp -#define HW_PIN_SDMMC_CLK pp -#define HW_PIN_SDMMC_CMD pp -//*/ - -//RP2040 specific options -//#define HW_RP2040_SYS_CLK_KHZ 200000 //overclocking - -//ESP32 specific options -//#define USE_ESP32_SOFTWIRE //use bitbang I2C (not hardware I2C) See https://github.com/espressif/esp-idf/issues/499 - - -//========================================================================================================================// -// HARDWARE // -//========================================================================================================================// - -//--- RC RECEIVER -#define RCIN_USE RCIN_USE_CRSF // RCIN_USE_CRSF, RCIN_USE_SBUS, RCIN_USE_DSM, RCIN_USE_PPM, RCIN_USE_PWM -#define RCIN_NUM_CHANNELS 7 //number of receiver channels (minimal 6) - -//--- IMU SENSOR -#define IMU_USE IMU_USE_SPI_MPU9250 // IMU_USE_SPI_MPU6500, IMU_USE_SPI_MPU9250,IMU_USE_SPI_MPU6000, IMU_USE_SPI_BMI270, IMU_USE_I2C_MPU9250, IMU_USE_I2C_MPU9150, IMU_USE_I2C_MPU6500, IMU_USE_I2C_MPU6050, IMU_USE_I2C_MPU6000 -//Set sensor orientation. The label is yaw / roll (in that order) needed to rotate the sensor from it's normal position to it's mounted position. -//If not sure what is needed: use CLI 'proll' and try each setting until roll-right gives positive ahrs.roll, pitch-up gives positive ahrs.pitch, and yaw-right gives positive ahrs.yaw -#define IMU_ALIGN IMU_ALIGN_CW90FLIP //IMU_ALIGN_CW0, IMU_ALIGN_CW90, IMU_ALIGN_CW180, IMU_ALIGN_CW270, IMU_ALIGN_CW0FLIP, IMU_ALIGN_CW90FLIP, IMU_ALIGN_CW180FLIP, IMU_ALIGN_CW270FLIP -#define IMU_I2C_ADR 0x69 //IMU I2C address. If unknown, use CLI 'i2c' - -//-- AHRS sensor fusion -#define AHRS_USE AHRS_USE_MAHONY // AHRS_USE_MAHONY, AHRS_USE_MAHONY_BF, AHRS_USE_MADGWICK, AHRS_USE_VQF - -//--- GPS -#define GPS_BAUD 115200 - -//--- BAROMETER SENSOR -#define BARO_USE BARO_USE_NONE // BARO_USE_BMP280, BARO_USE_MS5611, BARO_USE_NONE -//#define BARO_I2C_ADR 0x76 //set barometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- EXTERNAL MAGNETOMETER SENSOR -#define MAG_USE MAG_USE_NONE // MAG_USE_QMC5883L, MAG_USE_NONE -//#define MAG_I2C_ADR 0x77 //set magnetometer I2C address, leave commented for default address. If unknown, use CLI 'i2c' - -//--- BATTERY MONITOR -#define BAT_USE BAT_USE_NONE // BAT_USE_INA226, BAT_USE_ADC, BAT_USE_NONE - -//--- BLACKBOX LOGGER -#define BB_USE BB_USE_NONE //BB_USE_SD spi sdcard, BB_USE_SDMMC mmc sdcard, BB_USE_NONE - -//========================================================================================================================// -// RC RECEIVER // -//========================================================================================================================// - -//set channels (1..RCIN_NUM_CHANNELS) -const int rcin_cfg_thro_channel = 1; //low pwm = zero throttle/stick back, high pwm = full throttle/stick forward -const int rcin_cfg_roll_channel = 2; //low pwm = left, high pwm = right -const int rcin_cfg_pitch_channel = 3; //low pwm = pitch up/stick back, high pwm = pitch down/stick forward -const int rcin_cfg_yaw_channel = 4; //low pwm = left, high pwm = right -const int rcin_cfg_arm_channel = 5; //ARM/DISARM switch -const int rcin_cfg_aux_channel = 6; //Fight mode - 6 position switch -const int rcin_cfg_flaps_channel = 7; //Flaps - -//throttle pwm values -const int rcin_cfg_thro_low = 1250; //used to set rcin_thro_is_low flag when pwm is below. Note: your craft won't arm if this is too low. -const int rcin_cfg_thro_max = 1900; - -//roll, pitch, yaw pwm values -const int rcin_cfg_pwm_min = 1150; -const int rcin_cfg_pwm_center = 1500; -const int rcin_cfg_pwm_max = 1900; -const int rcin_cfg_pwm_deadband = 0; //Amount of deadband around center, center-deadband to center+deadband will be interpreted as central stick. Set to 15 for PPM or 0 for jitter-free serial protocol receivers. - -//pwm range for arm switch in ARMED position -const int rcin_cfg_arm_min = 1600; -const int rcin_cfg_arm_max = 2200; - -//6 position switch on aux channel - Ardupilot switch pwm: 1165,1295,1425,1555,1685,1815 (spacing 130) -//EdgeTx 3-pos SA + 2-pos SB setup: Source:SA weight:52 offset:0, Source:SB weight:13 offset:-1 multiplex: add -OR- Source:SA Weight:26 Offset:-40 Switch:SBdown, Source:SA Weight:26 Offset:36 Switch:SBup Multiplex:Replace -int rcin_cfg_aux_min = 1165; //lowest switch pwm -int rcin_cfg_aux_max = 1815; //higest switch pwm - -//========================================================================================================================// -// OUTPUTS // -//========================================================================================================================// - -//number of motors - out[0..out_MOTOR_COUNT-1] are motors, out[out_MOTOR_COUNT..HW_OUT_COUNT-1] are servos -const int out_MOTOR_COUNT = 1; - -//define outputs and their channels 1..HW_OUT_COUNT (see control_Mixer() for available outputs) -//select output name based on what the output does when pwm is high. For example: If the right aileron goes down on high -//pwm and is connected to output channel 2 use #define OUT_RIGHT_AILERON_DOWN 2 -#define OUT_MOTOR1 1 //full throttle on high pwm -#define OUT_LEFT_AILERON_UP 2 //left aileron deflects up on high pwm (and right aileron down, otherwise use two servo channels) -//#define OUT_RIGHT_AILERON_DOWN 3 //right aileron deflects down on high pwm -#define OUT_ELEVATOR_UP 3 //elevator deflects up on high pwm -#define OUT_RUDDER_LEFT 4 //rudder deflects left on high pwm -#define OUT_FLAPS_UP_HALF 5 //flaps deflect up on high pwm, but only use 0.5 to 1.0 servo range - -//========================================================================================================================// -// USER-SPECIFIED VARIABLES // -//========================================================================================================================// - -//flight modes -enum rcin_fm_enum {MANUAL, ROLL, FBWA}; //available flight modes: MANUAL send rc commands directly to motor and aileron/pitch/yaw servos, ROLL stabilize roll angle, FBWA stabilize roll/pitch angles -const char* rcin_fm_str[] = {"MANUAL", "ROLL", "FBWA"}; //flight mode names used for telemetry -rcin_fm_enum rcin_fm_map[6] {MANUAL, MANUAL, ROLL, ROLL, FBWA, FBWA}; //flightmode mapping from 6 pos switch to flight mode (simulates a 3-pos switch: MANUAL/ROLL/FBWA) - -const uint32_t imu_sample_rate = 1000; //imu sample rate in Hz (default 1000) NOTE: not all IMU drivers support a different rate -const uint32_t baro_sample_rate = 100; //baro sample rate in Hz (default 100) - -//Low Pass Filter cutoff frequency in Hz. Do not touch unless you know what you are doing. -float LP_acc = 70; //Accelerometer (default MPU6050: 50Hz, MPU9250: 70Hz) -float LP_gyr = 60; //Gyro (default MPU6050: 35Hz, MPU9250: 60Hz) -float LP_mag = 1e10; //Magnetometer (default 1e10Hz, i.e. no filtering) -float LP_radio = 400; //Radio Input (default 400Hz) - -//Controller parameters (take note of defaults before modifying!): -float i_limit = 25; //PID Integrator saturation level, mostly for safety -float maxRoll = 45; //Max roll angle in degrees for ROLL, FBWA modes -float maxPitch = 30; //Max pitch angle in degrees for FBWA mode -float fbwa_pitch_offset = 3; //FBWA pitch up angle for neutral stick - -//roll PID constants -float Kp_roll = (1.0/90); //Roll P-gain - apply full aileron on 90 degree roll error -float Ki_roll = 0; //Roll I-gain -float Kd_roll = (1.0/180); //Roll D-gain - apply full opposite aileron when roll rate is 180 degrees/sec towards desired setpoint - -//pitch PID constants -float Kp_pitch = (1.0/30); //Pitch P-gain - apply full elevator on 30 degree pitch error -float Ki_pitch = 0; //Pitch I-gain -float Kd_pitch = (1.0/90); //Pitch D-gain - apply full opposite elevator when pitch rate is 90 degrees/sec towards desired setpoint - -//float Kp_yaw = 0.3; //Yaw P-gain -//float Ki_yaw = 0.05; //Yaw I-gain -//float Kd_yaw = 0.00015; //Yaw D-gain - -//========================================================================================================================// -// DECLARE GLOBAL VARIABLES // -//========================================================================================================================// - -//Radio communication: -int rcin_pwm[RCIN_NUM_CHANNELS]; //filtered raw PWM values -float rcin_thro; //throttle: 0(cutoff) to 1(full); -float rcin_roll, rcin_pitch, rcin_yaw; // roll,pitch,yaw: -1(left,down) to 1(right,up) with 0 center stick -bool rcin_armed; //status of arm switch, true = armed -bool rcin_thro_is_low; //status of throttle stick, true = throttle low -int rcin_aux; // six position switch connected to aux channel, values 0-5 -float rcin_flaps; //flaps 0.0:up, 1.0:full down - -//PID controller output -float roll_PID = 0, pitch_PID = 0, yaw_PID = 0; - -//Flight status -rcin_fm_enum rcin_fm = (rcin_fm_enum)0; //current flight mode -bool out_armed = false; //motors will only run if this flag is true - -//Low pass filter parameters -float B_radio; - -//========================================================================================================================// -// INCLUDE MADFLIGHT LIBRARY // -//========================================================================================================================// -//Note: most madflight modules are header only. By placing the madflight include here allows the modules to access the global variables without declaring them extern. -#include - -//========================================================================================================================// -// SETUP() // -//========================================================================================================================// - -void setup() { - led.setup(HW_PIN_LED, HW_LED_ON); //Set built in LED to turn on to signal startup - Serial.begin(115200); //start console serial - - //6 second startup delay - for(int i=20;i>0;i--) { - Serial.printf("Plane " MADFLIGHT_VERSION " starting %d ...\n", i); - delay(300); - } - Serial.printf("Arduino library: " HW_ARDUINO_STR "\n"); - - hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) - cfg.begin(); //read config from EEPROM - cli.print_boardInfo(); //print board info and pinout - cli.print_i2cScan(); //print i2c scan - rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. - - //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) - while(true) { - int rv = imu.setup(imu_sample_rate); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning - if(rv<=0) break; - warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); - } - - //set filter parameters after imu.setup(), as imu.setup() can modify requested sample rate - B_radio = Ahrs::lowpass_to_beta(LP_radio, imu.getSampleRate()); //Note: uses imu sample rate because radio filter is applied in imu_loop - - baro.setup(baro_sample_rate); //Barometer sample rate 100Hz - mag.setup(); //External Magnetometer - bat.setup(); //Battery Monitor - bb.setup(); //Black Box - gps_setup(); //GPS - //gps_debug(); //uncomment to debug gps messages - - //Servos (set servos first just in case motors overwrite frequency of shared timers) - for(int i=out_MOTOR_COUNT;i 100) { - rcin_telem_ts = millis(); - rcin_telem_cnt++; - String fm_str = String(out_armed ? "*" : "") + rcin_fm_str[rcin_fm]; - rcin_telemetry_flight_mode(fm_str.c_str()); //only first 14 char get transmitted - rcin_telemetry_attitude(ahrs.pitch, ahrs.roll, ahrs.yaw); - if(rcin_telem_cnt % 10 == 0) rcin_telemetry_battery(bat.v, bat.i, bat.mah, 100); - if(rcin_telem_cnt % 10 == 5) rcin_telemetry_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h - } - - cli.loop(); //process CLI commands -} - -//update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU -void i2c_sensors_update() { - if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. - if(baro.update()) bb.log_baro(); //log if pressure updated - mag.update(); -} - -//========================================================================================================================// -// IMU UPDATE LOOP // -//========================================================================================================================// - -//This is __MAIN__ function of this program. It is called when new IMU data is available. -void imu_loop() { - //Blink LED - led_Blink(); - - //Sensor fusion: update ahrs.roll, ahrs.pitch, and ahrs.yaw angle estimates (degrees) from IMU data - ahrs.update(); - - //Get radio state - rcin_GetCommands(); //Pulls current available radio commands - rcin_Normalize(); //Convert raw commands to normalized values based on saturated control limits - - //Uncomment to debug without remote (and no battery!) - pitch drone up: motors m1,m3 should increase and m2,m4 decrease; bank right: m1,m2 increase; yaw right: m1,m4 increase - //rcin_thro = 0.5; rcin_thro_is_low = false; rcin_roll = 0; rcin_pitch = 0; rcin_yaw = 0; rcin_armed = true; rcin_aux = 0; out_armed = true; - - //PID Controller - switch(rcin_fm) { - case ROLL: - control_ROLL(rcin_thro_is_low); //Stabilize on roll angle setpoints - break; - case FBWA: - control_FBWA(rcin_thro_is_low); //Stabilize on pitch/roll angle setpoints - break; - default: - control_MANUAL(); - } - - //Actuator mixing - control_Mixer(); //Mixes PID outputs to scaled actuator commands -- custom mixing assignments done here - - //Motor output - out_KillSwitchAndFailsafe(); //Cut all motor outputs if DISARMED or failsafe triggered. - out_SetCommands(); //Sends command pulses to motors (only if out_armed=true) and servos - - //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. - if (imu.usesI2C()) i2c_sensors_update(); - - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... -} - -//========================================================================================================================// -// IMU UPDATE LOOP FUNCTIONS - in same order as they are called from imu_loop() // -//========================================================================================================================// - -void led_Blink() { - //Blink LED once per second, if LED blinks slower then the loop takes too much time, use print_loop_Rate() to investigate. - //DISARMED: long off, short on, ARMED: long on, short off - if(imu.update_cnt % imu.getSampleRate() <= imu.getSampleRate() / 10) - led.set(!out_armed); //short interval - else - led.set(out_armed); //long interval -} - -void rcin_GetCommands() { - //DESCRIPTION: Get raw PWM values for every channel from the radio - /* - * Updates radio PWM commands in loop based on current available commands. channel_x_pwm is the raw command used in the rest of - * the loop. If using a PWM or PPM receiver, the radio commands are retrieved from a function in the readPWM file separate from this one which - * is running a bunch of interrupts to continuously update the radio readings. If using an SBUS receiver, the values are pulled from the SBUS library directly. - * The raw radio commands are filtered with a first order low-pass filter to eliminate any really high frequency noise. - */ - - bool got_new_data = rcin.update(); - - //Low-pass the critical commands and update previous values - for(int i=0; i%d ",i+1,pwm_new[i],rcin_pwm[i]); Serial.println(); //uncomment for debugging - } -} - -void rcin_Normalize() { - //DESCRIPTION: Normalizes desired control values to appropriate values - /* - * Updates the normalized rcin variables rcin_thro, rcin_roll, rcin_pitch, and rcin_yaw. These are computed by using the raw - * RC pwm commands and scaling them to be within our limits defined in setup. rcin_thro stays within 0 to 1 range. - * rcin_roll, rcin_pitch, and rcin_yaw are scaled to -1 to +1. - */ - - //normalized values - //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle - int pwm = rcin_pwm[rcin_cfg_thro_channel-1]; - rcin_thro = constrain( ((float)(pwm - rcin_cfg_thro_low)) / (rcin_cfg_thro_max - rcin_cfg_thro_low), 0.0, 1.0); - rcin_thro_is_low = (pwm <= rcin_cfg_thro_low); - - //roll,pitch,yaw - rcin_roll = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_roll_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (roll left, stick left) to 1 (roll right, stick right) - rcin_pitch = - _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_pitch_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (pitch down, stick back) to 1 (pitch up, stick forward) - rcin_yaw = _rcin_ChannelNormalize(rcin_pwm[rcin_cfg_yaw_channel-1], rcin_cfg_pwm_min, rcin_cfg_pwm_center, rcin_cfg_pwm_max, rcin_cfg_pwm_deadband); // output: -1 (yaw left, stick left) to 1 (yaw right, stick right) - - //arm switch - pwm = rcin_pwm[rcin_cfg_arm_channel-1]; - rcin_armed = (rcin_cfg_arm_min <= pwm && pwm <= rcin_cfg_arm_max); //output: true/false - - //aux 6 position switch (flight mode) - int spacing = (rcin_cfg_aux_max - rcin_cfg_aux_min) / 5; - rcin_aux = constrain( ( rcin_pwm[rcin_cfg_aux_channel-1] - rcin_cfg_aux_min + spacing/2) / spacing, 0, 5); //output 0..5 - rcin_fm = rcin_fm_map[rcin_aux]; - - //flaps - rcin_flaps = constrain( ((float)(rcin_pwm[rcin_cfg_flaps_channel-1] - rcin_cfg_pwm_min)) / (rcin_cfg_pwm_max - rcin_cfg_pwm_min), 0.0, 1.0); //output: 0.0 to 1.0 -} - -//helper to nomalize a channel based on min,center,max calibration -float _rcin_ChannelNormalize(int val, int min, int center, int max, int deadband) { - int rev = 1; //1=normal, -1=reverse channel - //needs: min < center < max - if(val roll right -> deflect left aileron down, deflect right aileron up - #ifdef OUT_LEFT_AILERON_DOWN //left aileron deflects down on high pwm - out_command[OUT_LEFT_AILERON_DOWN-1] = +roll_PID/2.0 + 0.5; - #endif - #ifdef OUT_RIGHT_AILERON_UP //right aileron deflects up on high pwm - out_command[OUT_RIGHT_AILERON_UP-1] = +roll_PID/2.0 + 0.5; - #endif - #ifdef OUT_LEFT_AILERON_UP //reversed: left aileron deflects up on high pwm - out_command[OUT_LEFT_AILERON_UP-1] = -roll_PID/2.0 + 0.5; - #endif - #if defined(OUT_RIGHT_AILERON_DOWN) //reversed: right aileron deflects down on high pwm - out_command[OUT_RIGHT_AILERON_DOWN-1] = -roll_PID/2.0 + 0.5; - #endif - - //elevator: when pitch_PID is positive -> pitch up -> deflect elevator down - #ifdef OUT_ELEVATOR_DOWN //elevator deflects down on high pwm - out_command[OUT_ELEVATOR_UP-1] = +pitch_PID/2.0 + 0.5; - #endif - #ifdef OUT_ELEVATOR_UP //reversed: elevator deflects up on high pwm - out_command[OUT_ELEVATOR_UP-1] = -pitch_PID/2.0 + 0.5; - #endif - - //rudder: when yaw_PID is positive -> yaw right -> deflect rudder right - #ifdef OUT_RUDDER_RIGHT //rudder deflects right on high pwm - out_command[OUT_RUDDER_RIGHT-1] = +yaw_PID/2.0 + 0.5; - #endif - #ifdef OUT_RUDDER_LEFT //reversed: rudder deflects left on high pwm - out_command[OUT_RUDDER_LEFT-1] = -yaw_PID/2.0 + 0.5; - #endif - - //flaps: - #ifdef OUT_FLAPS_DOWN //flaps deflect down on high pwm - out_command[OUT_FLAPS_DOWN-1] = +rcin_flaps; - #endif - #ifdef OUT_FLAPS_DOWN_HALF //flaps deflect down on high pwm (but only use servo range 0.5 to 1.0) - out_command[OUT_FLAPS_DOWN_HALF-1] = +rcin_flaps/2.0 + 0.5; - #endif - #ifdef OUT_FLAPS_UP //reversed: flaps deflect up on high pwm - out_command[OUT_FLAPS_UP-1] = -rcin_flaps; - #endif - #ifdef OUT_FLAPS_UP_HALF //flaps deflect up on high pwm (but only use servo range 0.5 to 1.0) - out_command[OUT_FLAPS_UP_HALF-1] = -rcin_flaps/2.0 + 0.5; - #endif - - //delta wing: - // when roll_PID positive -> roll right -> deflect left elevon down, deflect right elevon up - // when pitch_PID is positive -> pitch up -> deflect left elevon down, deflect right elevon down - #ifdef OUT_LEFT_ELEVON_DOWN //left elevon deflects down on high input - out_command[OUT_LEFT_ELEVON_DOWN-1] = +roll_PID/2.0 +pitch_PID/2.0 + 0.5; - #endif - #ifdef OUT_RIGHT_ELEVON_UP //right elevon deflects up on high input - out_command[OUT_RIGHT_ELEVON_UP-1] = +roll_PID/2.0 -pitch_PID/2.0 + 0.5; - #endif - #ifdef OUT_LEFT_ELEVON_UP //reversed: left elevon deflects down on high input - out_command[OUT_LEFT_ELEVON_UP-1] = -roll_PID/2.0 -pitch_PID/2.0 + 0.5; - #endif - #ifdef OUT_RIGHT_ELEVON_DOWN //reversed: right elevon deflects down on high input - out_command[OUT_RIGHT_ELEVON_DOWN-1] = -roll_PID/2.0 +pitch_PID/2.0 + 0.5; - #endif - - //0.0 is zero throttle if connecting to ESC for conventional PWM, 1.0 is max throttle - //0.5 is centered servo, 0.0 and 1.0 are servo at their extreme positions as set with SERVO_MIN and SERVO_MAX -} - -void out_KillSwitchAndFailsafe() { - static bool rcin_armed_prev = true; //initial value is true: forces out_armed false on startup even if arm switch is ON - - //Change to ARMED when throttle is low and radio armed switch was flipped from disamed to armed position - if (!out_armed && rcin_thro_is_low && rcin_armed && !rcin_armed_prev) { - out_armed = true; - Serial.println("OUT: ARMED"); - bb.start(); //start blackbox logging - } - - //Change to DISARMED when radio armed switch is in disarmed position, or if radio lost connection - if (out_armed && (!rcin_armed || !rcin.connected())) { - out_armed = false; - if(!rcin_armed) { - Serial.println("OUT: DISARMED (arm switch)"); - bb.stop(); //stop blackbox logging - }else{ - Serial.println("OUT: DISARMED (rcin lost connection)"); - } - } - - //IF DISARMED -> STOP MOTORS - if(!out_armed) for(int i=0;i + +void setup() { + Serial.begin(115200); + while(!Serial); + + hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) + + int rv = baro.setup(100); + if(rv) { + while(1) { + Serial.printf("sensor error %d\n",rv); + delay(1000); + } + } +} + +uint32_t ts = 0; + +void loop() { + if(baro.update()) { + uint32_t now = micros(); + Serial.printf("ts:%d\tp:%f\tt:%f\talt:%f\n",(int)(now-ts),baro.press,baro.temp,baro.alt); + ts = now; + } +} + diff --git a/extras/TestMadflight/cfg.ino b/extras/TestMadflight/cfg.ino index 1b43940..11c0612 100644 --- a/extras/TestMadflight/cfg.ino +++ b/extras/TestMadflight/cfg.ino @@ -92,9 +92,9 @@ void setup() { void loop() { cfg.begin(); - 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); + 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); if(Serial.available()) { - cfg.bat_cal_i += 1.1; + cfg.BAT_CAL_I += 1.1; cfg.write(); while(Serial.available()) Serial.read(); diff --git a/extras/TestMadflight/rcin/rcin.ino b/extras/TestMadflight/rcin/rcin.ino index cb3832b..6a5128b 100644 --- a/extras/TestMadflight/rcin/rcin.ino +++ b/extras/TestMadflight/rcin/rcin.ino @@ -13,7 +13,7 @@ #define HW_PIN_RCIN_INVERTER -1 //only used for STM32 targets //*/ -#define MF_TEST MF_TEST_RCIN +#define MF_TEST MF_TEST_RCIN // | MF_TEST_CLI #include @@ -21,6 +21,7 @@ void setup() { Serial.begin(115200); while(!Serial); + //cli.print_boardInfo(); //print board info and pinout hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) rcin.setup(); //Initialize radio communication. Set correct USE_RCIN_xxx user specified defines above. Note: rcin_Setup() function is defined in rcin.h, but normally no changes needed there. diff --git a/extras/TestMadflight/rp2040_crsf/rp2040_crsf.ino b/extras/TestMadflight/rp2040_crsf/rp2040_crsf.ino index 24fd89f..67d4b2d 100644 --- a/extras/TestMadflight/rp2040_crsf/rp2040_crsf.ino +++ b/extras/TestMadflight/rp2040_crsf/rp2040_crsf.ino @@ -71,7 +71,7 @@ void loop() { Serial.print(" dt="); Serial.print(dt); Serial.print(" imurt="); - Serial.print(imu.runtime_tot_max); + Serial.print(imu.stat_runtime_max); Serial.println(); delay(100); } @@ -322,7 +322,7 @@ void loop() { //Serial.printf(" baud:%d ", (int)uart.baud_actual); Serial.printf(" cnt_imu_loop:%d", (int)cnt_imu_loop); cnt_imu_loop=0; Serial.printf(" az:%f", imu.az); - Serial.printf(" runtime_tot_max:%d", (int)imu.runtime_tot_max); imu.runtime_tot_max=0; + Serial.printf(" stat_runtime_max:%d", (int)imu.stat_runtime_max); imu.stat_runtime_max=0; //Serial.printf(" bytes:%d", (int)cnt_rxbytes); cnt_rxbytes=0; Serial.printf(" avail:%d", (int)rcin_Serial->available()); @@ -384,7 +384,7 @@ void loop() { //Serial.printf(" baud:%d ", (int)uart.baud_actual); Serial.printf(" cnt_imu_loop:%d", (int)cnt_imu_loop); cnt_imu_loop=0; Serial.printf(" az:%f", imu.az); - Serial.printf(" runtime_tot_max:%d", (int)imu.runtime_tot_max); imu.runtime_tot_max=0; + Serial.printf(" stat_runtime_max:%d", (int)imu.stat_runtime_max); imu.stat_runtime_max=0; //Serial.printf(" bytes:%d", (int)cnt_rxbytes); cnt_rxbytes=0; Serial.printf(" avail:%d", (int)uart.available()); diff --git a/src/madflight.h b/src/madflight.h index ce5cec1..2bb3526 100644 --- a/src/madflight.h +++ b/src/madflight.h @@ -1,11 +1,11 @@ -#define MADFLIGHT_VERSION "madflight v1.2.2" +#define MADFLIGHT_VERSION "madflight v1.3.0-DEV" /*========================================================================================== -madflight - Flight Controller for ESP32 / RP2040 / STM32 +madflight - Flight Controller for ESP32 / ESP32-S3 / RP2350 / RP2040 / STM32 MIT License -Copyright (c) 2023-2024 qqqlab - https://github.com/qqqlab +Copyright (c) 2023-2024 https://madflight.com Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -39,9 +39,6 @@ SOFTWARE. #error "Unknown hardware architecture, expected ESP32 / RP2040 / STM32" #endif -//Outputs: -float out_command[HW_OUT_COUNT] = {0}; //Mixer outputs (values: 0.0 to 1.0) -PWM out[HW_OUT_COUNT]; //ESC and Servo outputs (values: 0.0 to 1.0) //for testing individual modules use: #define MF_TEST MF_TEST_LED | MF_TEST_RCIN #define MF_TEST_BASE 0x0000 @@ -56,7 +53,7 @@ PWM out[HW_OUT_COUNT]; //ESC and Servo outputs (values: 0.0 to 1.0) #define MF_TEST_BAT 0x0100 #define MF_TEST_BB 0x0200 #define MF_TEST_CLI 0x0400 - +#define MF_TEST_OUT 0x0800 //include all modules. Before including madflight.h define the all module options, for example: #define IMU_USE IMU_USE_SPI_MPU6500 //load config first, so that cfg.xxx can be used by other modules @@ -73,6 +70,7 @@ PWM out[HW_OUT_COUNT]; //ESC and Servo outputs (values: 0.0 to 1.0) #endif #if !defined(MF_TEST) || ((MF_TEST) & MF_TEST_RCIN) #include "madflight/rcin/rcin.h" + #include "madflight/rcin/telem.h" #endif #if !defined(MF_TEST) || ((MF_TEST) & MF_TEST_IMU) #include "madflight/imu/imu.h" @@ -92,8 +90,96 @@ PWM out[HW_OUT_COUNT]; //ESC and Servo outputs (values: 0.0 to 1.0) #if !defined(MF_TEST) || ((MF_TEST) & MF_TEST_BB) #include "madflight/bb/bb.h" #endif +#if !defined(MF_TEST) || ((MF_TEST) & MF_TEST_OUT) + #include "madflight/out/out.h" +#endif #if !defined(MF_TEST) || ((MF_TEST) & MF_TEST_CLI) #include "madflight/cli/cli.h" #endif +#if !defined(MF_TEST) + + extern void __attribute__((weak)) imu_loop(); + + #ifndef MF_SETUP_DELAY_S + #define MF_SETUP_DELAY_S 6 + #endif + + + + //=============================================================================================== + // HELPERS + //=============================================================================================== + + void madflight_warn_or_die(String msg, bool never_return) { + bool do_print = true; + do{ + if(do_print) Serial.print(msg + "\n"); + for(int i=0;i<20;i++) { + led.toggle(); + uint32_t ts = millis(); + while(millis() - ts < 50) { + if(cli.loop()) do_print = false; //process CLI commands, stop error output after first command + } + } + } while(never_return); + } + void madflight_die(String msg) { madflight_warn_or_die(msg, true); } + void madflight_warn(String msg) { madflight_warn_or_die(msg, false); } + + //=============================================================================================== + // madflight_setup() + //=============================================================================================== + + void madflight_setup() { + led.setup(); //Set built in LED to turn on to signal startup + Serial.begin(115200); //start console serial + + #ifndef MF_SETUP_FAST + //6 second startup delay + for(int i = 12; i > 0; i--) { + Serial.printf(MADFLIGHT_VERSION " starting %d ...\n", i); + delay(500); + } + #endif + Serial.printf("Arduino library: " HW_ARDUINO_STR "\n"); + + cli.print_boardInfo(); //print board info and pinout + hw_setup(); //hardware specific setup for spi and Wire (see hw_xxx.h) + cfg.begin(); //read config from EEPROM + cli.print_i2cScan(); //print i2c scan + rcin.setup(); //Initialize radio communication. + + //IMU: keep on trying until no error is returned (some sensors need a couple of tries...) + while(true) { + int rv = imu.setup(IMU_SAMPLE_RATE); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning + if(rv<=0) break; + madflight_warn("IMU: init failed rv= " + String(rv) + ". Retrying...\n"); + } + + baro.setup(BARO_SAMPLE_RATE); //Barometer sample rate 100Hz + mag.setup(); //External Magnetometer + bat.setup(); //Battery Monitor + bb.setup(); //Black Box + gps_setup(); //GPS + + ahrs.setup(IMU_GYR_LP_HZ, IMU_ACC_LP_HZ, IMU_MAG_LP_HZ); //setup low pass filters for Mahony/Madgwick filters + ahrs.setInitalOrientation(); //do this before IMU update handler is started + + //start IMU update handler + if(!imu_loop) Serial.println("=== WARNING === 'void imu_loop()' not defined."); + imu.onUpdate = imu_loop; + if(!imu.waitNewSample()) madflight_die("IMU interrupt not firing. Is HW_PIN_IMU_EXTI connected?"); + + #ifndef MF_SETUP_FAST + //Calibrate for zero gyro readings, assuming vehicle not moving when powered up. Comment out to only use cfg values. (Use CLI to calibrate acc.) + cli.calibrate_gyro(); + #endif + + cli.welcome(); + + led.enable(); //Set LED off to signal end of mf.startup, and enable blinking by imu_loop() + } + +#endif \ No newline at end of file diff --git a/src/madflight/ahrs/ahrs.h b/src/madflight/ahrs/ahrs.h index caf5247..8a9bf7e 100644 --- a/src/madflight/ahrs/ahrs.h +++ b/src/madflight/ahrs/ahrs.h @@ -55,14 +55,14 @@ void Ahrs::update() { // compute euler angles from q //Low-pass filtered, corrected accelerometer data - ax = (1.0 - B_acc) * ax + B_acc * (imu.ax - cfg.imu_cal_ax); - ay = (1.0 - B_acc) * ay + B_acc * (imu.ay - cfg.imu_cal_ay); - az = (1.0 - B_acc) * az + B_acc * (imu.az - cfg.imu_cal_az); + ax += B_acc * ((imu.ax - cfg.IMU_CAL_AX) - ax); + ay += B_acc * ((imu.ay - cfg.IMU_CAL_AY) - ay); + az += B_acc * ((imu.az - cfg.IMU_CAL_AZ) - az); //Low-pass filtered, corrected gyro data - gx = (1.0 - B_gyr) * gx + B_gyr * (imu.gx - cfg.imu_cal_gx); - gy = (1.0 - B_gyr) * gy + B_gyr * (imu.gy - cfg.imu_cal_gy); - gz = (1.0 - B_gyr) * gz + B_gyr * (imu.gz - cfg.imu_cal_gz); + gx += B_gyr * ((imu.gx - cfg.IMU_CAL_GX) - gx); + gy += B_gyr * ((imu.gy - cfg.IMU_CAL_GY) - gy); + gz += B_gyr * ((imu.gz - cfg.IMU_CAL_GZ) - gz); //External Magnetometer float _mx = mag.x; @@ -77,13 +77,13 @@ void Ahrs::update() { //update the mag values if( ! (_mx == 0 && _my == 0 && _mz == 0) ) { //Correct the mag values with the calibration values - _mx = (_mx - cfg.mag_cal_x) * cfg.mag_cal_sx; - _my = (_my - cfg.mag_cal_y) * cfg.mag_cal_sy; - _mz = (_mz - cfg.mag_cal_z) * cfg.mag_cal_sz; + _mx = (_mx - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX; + _my = (_my - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY; + _mz = (_mz - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ; //Low-pass filtered magnetometer data - mx = (1.0 - B_mag) * mx + B_mag * _mx; - my = (1.0 - B_mag) * my + B_mag * _my; - mz = (1.0 - B_mag) * mz + B_mag * _mz; + mx += B_mag * (_mx - mx); + my += B_mag * (_my - my); + mz += B_mag * (_mz - mz); }else{ mx = 0; my = 0; diff --git a/src/madflight/baro/baro.h b/src/madflight/baro/baro.h index cbb589a..0df8c08 100644 --- a/src/madflight/baro/baro.h +++ b/src/madflight/baro/baro.h @@ -6,11 +6,14 @@ Each BARO_USE_xxx section in this file defines a specific Barometer class #define BARO_USE_NONE 1 #define BARO_USE_BMP280 2 -#define BARO_USE_MS5611 3 +#define BARO_USE_BMP388 3 +#define BARO_USE_BMP390 4 +#define BARO_USE_MS5611 5 + class BarometerSensor { public: - virtual int setup() = 0; + virtual int setup(uint32_t sampleRate) = 0; virtual bool update(float *press, float *temp) = 0; //returns true if pressure was updated }; @@ -25,7 +28,8 @@ class BarometerSensor { #if BARO_USE == BARO_USE_NONE || !defined BARO_USE class BarometerNone: public BarometerSensor { public: - int setup() { + int setup(uint32_t sampleRate) { + (void) sampleRate; Serial.println("BARO: BARO_USE_NONE"); return 0; } @@ -52,8 +56,8 @@ Adafruit_BMP280 baro_BMP280(i2c); class BarometerBMP280: public BarometerSensor { public: - int setup() { - Serial.println(); + int setup(uint32_t sampleRate) { + (void) sampleRate; unsigned status; status = baro_BMP280.begin(BARO_I2C_ADR, BMP280_CHIPID); Serial.printf("BARO: BARO_USE_BMP280 BARO_I2C_ADR: 0x%02X SensorID: 0x%02X\n", BARO_I2C_ADR, baro_BMP280.sensorID()); @@ -88,6 +92,58 @@ class BarometerBMP280: public BarometerSensor { BarometerBMP280 baro_sensor; +//================================================================================================= +// BMP390 / BMP388 +//================================================================================================= +#elif BARO_USE == BARO_USE_BMP390 || BARO_USE == BARO_USE_BMP388 + +#include "bmp3/bmp3.h" + +bfs::Bmp3 bmp(&Wire, (bfs::Bmp3::I2cAddr)BARO_I2C_ADR); + +class BarometerBMP390: public BarometerSensor { + +public: + int setup(uint32_t sampleRate) { + Serial.printf("BARO: BARO_USE_BMP390/BARO_USE_BMP388 BARO_I2C_ADR: 0x%02X\n", BARO_I2C_ADR); + if (!bmp.Begin()) { + Serial.println("BARO: BARO_USE_BMP390/BARO_USE_BMP388 sensor not found"); + return 1; + } + + bmp.ConfigFilterCoef(bfs::Bmp3::FILTER_COEF_OFF); + + //set equal or next higher sample rate + if(sampleRate > 100) { + //f = 200; + bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_1X_TEMP_1X); + }else if(sampleRate > 50) { + //f = 100; + bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_2X_TEMP_1X); + }else if(sampleRate > 25) { + //f = 50; + bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_8X_TEMP_1X); + }else if(sampleRate > 12) { + //f = 25; + bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_16X_TEMP_2X); + }else { + //f = 12.5; + bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_32X_TEMP_2X); //12.5Hz + } + + return 0; + } + + bool update(float *press, float *temp) { + if (!bmp.Read()) return false; + *press = bmp.pres_pa(); + *temp = bmp.die_temp_c(); + return true; + } +}; + +BarometerBMP390 baro_sensor; + //================================================================================================= // MS5611 //================================================================================================= @@ -103,7 +159,8 @@ class BarometerMS5611: public BarometerSensor { //float press_pa = 0; //float temp_c = 0; - int setup() { + int setup(uint32_t sampleRate) { + (void) sampleRate; // Initialize MS5611 sensor // Ultra high resolution: MS5611_ULTRA_HIGH_RES // (default) High resolution: MS5611_HIGH_RES @@ -147,7 +204,7 @@ int Barometer::setup(uint32_t sampleRate, float filterAltHertz, float filterVzHe B_vz = constrain(1 - exp(-2 * PI * filterVzHertz / sampleRate), 0.0f, 1.0f); dt = 0; ts = micros(); - int rv = baro_sensor.setup(); + int rv = baro_sensor.setup(sampleRate); if(rv != 0) { press = 0; temp = 0; @@ -168,9 +225,9 @@ bool Barometer::update() { dt = (tsnew - ts) / 1000000.0; baro_sensor.update(&press, &temp); altRaw = (101325.0 - press) / 12.0; - float altnew = (1.0 - B_alt) * alt + B_alt * altRaw; //Low-pass filtered altitude + float altnew = alt + B_alt * (altRaw - alt); //Low-pass filtered altitude float vznew = (altnew - alt) / dt; - vz = (1.0 - B_vz) * vz + B_vz * vznew; //Low-pass filtered velocity + vz += B_vz * (vznew - vz); //Low-pass filtered velocity alt = altnew; ts = tsnew; return true; diff --git a/src/madflight/baro/bmp3/LICENSE.md b/src/madflight/baro/bmp3/LICENSE.md new file mode 100644 index 0000000..f7ca02f --- /dev/null +++ b/src/madflight/baro/bmp3/LICENSE.md @@ -0,0 +1,44 @@ +## License for: src/bme280.cpp, src/bme280.h: + +The MIT License (MIT) + +Copyright (c) 2022 Bolder Flight Systems Inc + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +## License for src/bst/bmp3.c, src/bst/bmp3_defs.h, src/bst/bmp3.h: + +Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. + +BSD-3-Clause + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/madflight/baro/bmp3/README.md b/src/madflight/baro/bmp3/README.md new file mode 100644 index 0000000..6f71750 --- /dev/null +++ b/src/madflight/baro/bmp3/README.md @@ -0,0 +1,181 @@ +[![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) + +![Bolder Flight Systems Logo](img/logo-words_75.png)     ![Arduino Logo](img/arduino_logo_75.png) + +# Bmp3 +This library communicates with the BMP3xy series of pressure sensors, such as the [BMP388](https://www.bosch-sensortec.com/products/environmental-sensors/pressure-sensors/bmp388/) and is compatible with Arduino and CMake build systems. + * [License](LICENSE.md) + * [Changelog](CHANGELOG.md) + * [Contributing guide](CONTRIBUTING.md) + +# Usage +This library supports both I2C and SPI commmunication with the BMP3xy sensors and supports collecting pressure and temperature data. The BMP3xy temperature data should be treated as die temperature, and is labled as such within this library. + +# Installation + +## Arduino +Simply clone or download this library into your Arduino/libraries folder. The library is added as: + +```C++ +#include "bmp3.h" +``` + +Example Arduino executables are located in: *examples/arduino/*. Teensy 3.x, 4.x, and LC devices are used for testing under Arduino and this library should be compatible with other Arduino devices. + +## CMake +CMake is used to build this library, which is exported as a library target called *bmp3*. The header is added as: + +```C++ +#include "bmp3.h" +``` + +The library can be also be compiled stand-alone using the CMake idiom of creating a *build* directory and then, from within that directory issuing: + +``` +cmake .. -DMCU=MK66FX1M0 +make +``` + +This will build the library and example executables called *i2c_example* and *spi_example*. The example executable source files are located at *examples/cmake/i2c.cc* and *examples/cmake/spi.cc*. Notice that the *cmake* command includes a define specifying the microcontroller the code is being compiled for. This is required to correctly configure the code, CPU frequency, and compile/linker options. The available MCUs are: + * MK20DX128 + * MK20DX256 + * MK64FX512 + * MK66FX1M0 + * MKL26Z64 + * IMXRT1062_T40 + * IMXRT1062_T41 + * IMXRT1062_MMOD + +These are known to work with the same packages used in Teensy products. Also switching packages is known to work well, as long as it's only a package change. + +The example targets create executables for communicating with the sensor using I2C or SPI communication. Each target also has a *_hex*, for creating the hex file to upload to the microcontroller, and an *_upload* for using the [Teensy CLI Uploader](https://www.pjrc.com/teensy/loader_cli.html) to flash the Teensy. Please note that instructions for setting up your build environment can be found in our [build-tools repo](https://github.com/bolderflight/build-tools). + +# Namespace +This library is within the namespace *bfs*. + +# Bmp3 + +## Methods + +**Bmp3()** Default constructor, requires calling the Config method to setup the I2C or SPI bus and I2C address or SPI chip select pin. + +**Bmp3(TwoWire *i2c, const I2cAddr addr)** Creates a Bmp3 object. This constructor is used for the I2C communication interface. A pointer to the I2C bus object is passed along with an enum of the I2C address of the sensor. Use I2C_ADDR_PRIM if the SDO pin is grounded and I2C_ADDR_SEC if the SDO pin is pulled high. + +```C++ +bfs::Bmp3 bmp(&Wire, bfs::Bmp3::I2C_ADDR_PRIM); +``` + +**Bmp3(SPIClass *spi, const uint8_t cs)** Creates a Bmp3 object. This constructor is used for the SPI communication interface. A pointer to the SPI bus object is passed along with the chip select pin of the sensor. Any pin capable of digital I/O can be used as a chip select pin. + +```C++ +bfs::Bmp3 bmp(&SPI, 2); +``` + +**void Config(TwoWire *bus, const I2cAddr addr)** This is required when using the default constructor and sets up the I2C bus and I2C address. + +**void Config(SPIClass *spi, const uint8_t cs)** This is required when using the default constructor and sets up the SPI bus and chip select pin. + +**int8_t error_code()** The *Begin*, *ConfigOsMode*, *ConfigFilterCoef*, and *Read* methods return boolean values, true if the method is successful (and in the case of *Read*, if new data is available) and false otherwise. This method returns the error code from the last operation, which can help in debugging. The error codes are: + +| Error Code | Value | +| --- | --- | +| BMP3_OK | 0 | +| BMP3_E_NULL_PTR | -1 | +| BMP3_E_COMM_FAIL | -2 | +| BMP3_E_INVALID_ODR_OSR_SETTINGS | -3 | +| BMP3_E_CMD_EXEC_FAILED | -4 | +| BMP3_E_CONFIGURATION_ERR | -5 | +| BMP3_E_INVALID_LEN | -6 | +| BMP3_E_DEV_NOT_FOUND | -7 | +| BMP3_E_FIFO_WATERMARK_NOT_REACHED | -8 | + +**bool Begin()** Initializes communication with the sensor and configures the default sampling rates, oversampling and low pass filter settings. True is returned if communication is able to be established with the sensor and configuration completes successfully, otherwise, false is returned. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus. + +```C++ +Wire.begin(); +Wire.setClock(400000); +bool status = bmp.Begin(); +if (!status) { + // ERROR +} +``` + +The BMP3xy features programmable oversampling, filtering, and output data rate. By default, these are set to the following values: + +| Settings | | +| --- | --- | +| Oversampling | pressure x 8, temperature x 1 | +| IIR Filter Coefficient | 2 | +| Output Data Rate | 50 Hz | + +**bool ConfigOsMode(const OsMode mode)** Configures the pressure oversampling, temperature oversampling, and output data rate: + +| Enum | Pressure Oversampling | Temperature Oversampling | Output Data Rate | +| --- | --- | --- | --- | +| OS_MODE_PRES_1X_TEMP_1X | 1x | 1x | 200 Hz | +| OS_MODE_PRES_2X_TEMP_1X | 2x | 1x | 100 Hz | +| OS_MODE_PRES_4X_TEMP_1X | 4x | 1x | 50 Hz | +| OS_MODE_PRES_8X_TEMP_1X | 8x | 1x | 50 Hz | +| OS_MODE_PRES_16X_TEMP_2X | 16x | 2x | 25 Hz | +| OS_MODE_PRES_32X_TEMP_2X | 32x | 2x | 12.5 Hz | + +```C++ +bool status = bmp.ConfigOsMode(bfs::Bmp3::OS_MODE_PRES_32X_TEMP_2X); +if (!status) { + // ERROR +} +``` + +**OsMode os_mode()** Returns the current oversampling mode. + +**bool ConfigFilterCoef(const FilterCoef val)** Sets the digital low pass filter IIR coefficient. This filter is applied to all measurements. The filter is given by the following equation: + +*data_filtered = (data_filtered_old * (filter_coefficient - 1) + data) / filter_coefficient* + +The following enumerated filter coefficients are supported: + +| Filter Coefficient Name | Filter Coefficient Value | +| --- | --- | +| FILTER_COEF_OFF | 1 | +| FILTER_COEF_2 | 2 | +| FILTER_COEF_4 | 4 | +| FILTER_COEF_8 | 8 | +| FILTER_COEF_16 | 16 | +| FILTER_COEF_32 | 32 | +| FILTER_COEF_64 | 64 | +| FILTER_COEF_128 | 128 | + +```C++ +bool status = bmp.ConfigFilterCoef(bfs::Bmp3::FILTER_COEF_16); +if (!status) { + // ERROR +} +``` + +**FilterCoef filter_coef()** Returns the current filter coefficient value. + +**bool Reset()** Performs a soft reset of the BMP3xy. True is returned on success. + +**bool Read()** Reads data from the BMP3xy and stores the data in the Bmp3 object. Returns true if data is successfully read, otherwise, returns false. + +```C++ +/* Read the sensor data */ +if (bmp.Read()) { +} +``` + +**float pres_pa()** Returns the pressure data from the Bmp3 object in units of Pa. + +```C++ +float pressure = bmp.pres_pa(); +``` + +**float die_temp_c** Returns the die temperature of the sensor from the Bmp3 object in units of degrees C. + +```C++ +float temperature = bmp.die_temp_c(); +``` + +## Example List +* **i2c**: demonstrates declaring a *Bmp3* object, initializing the sensor, and collecting data. I2C is used to communicate with the BMP3xy sensor. +* **spi**: demonstrates declaring a *Bmp3* object, initializing the sensor, and collecting data. SPI is used to communicate with the BMP3xy sensor. diff --git a/src/madflight/baro/bmp3/bmp3.cpp b/src/madflight/baro/bmp3/bmp3.cpp new file mode 100644 index 0000000..755b40f --- /dev/null +++ b/src/madflight/baro/bmp3/bmp3.cpp @@ -0,0 +1,327 @@ +/* +* Brian R Taylor +* brian.taylor@bolderflight.com +* +* Copyright (c) 2022 Bolder Flight Systems Inc +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the “Software”), to +* deal in the Software without restriction, including without limitation the +* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +* sell copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +* IN THE SOFTWARE. +*/ + +#include "bmp3.h" // NOLINT +#if defined(ARDUINO) +#include +#include "Wire.h" +#include "SPI.h" +#else +#include +#include +#include "core/core.h" +#endif + +namespace bfs { + +Bmp3::Bmp3(TwoWire *i2c, const I2cAddr addr) { + i2c_intf_.i2c = i2c; + i2c_intf_.addr = static_cast(addr); + dev_.intf_ptr = &i2c_intf_; + dev_.intf = BMP3_I2C_INTF; + dev_.read = I2cReadRegisters; + dev_.write = I2cWriteRegisters; + dev_.delay_us = Delay_us; +} + +Bmp3::Bmp3(SPIClass *spi, const uint8_t cs) { + spi_intf_.spi = spi; + spi_intf_.cs = cs; + dev_.intf_ptr = &spi_intf_; + dev_.intf = BMP3_SPI_INTF; + dev_.read = SpiReadRegisters; + dev_.write = SpiWriteRegisters; + dev_.delay_us = Delay_us; +} + +void Bmp3::Config(TwoWire *i2c, const I2cAddr addr) { + i2c_intf_.i2c = i2c; + i2c_intf_.addr = static_cast(addr); + dev_.intf_ptr = &i2c_intf_; + dev_.intf = BMP3_I2C_INTF; + dev_.read = I2cReadRegisters; + dev_.write = I2cWriteRegisters; + dev_.delay_us = Delay_us; +} + +void Bmp3::Config(SPIClass *spi, const uint8_t cs) { + spi_intf_.spi = spi; + spi_intf_.cs = cs; + dev_.intf_ptr = &spi_intf_; + dev_.intf = BMP3_SPI_INTF; + dev_.read = SpiReadRegisters; + dev_.write = SpiWriteRegisters; + dev_.delay_us = Delay_us; +} + +bool Bmp3::Begin() { + if (dev_.intf == BMP3_SPI_INTF) { + pinMode(spi_intf_.cs, OUTPUT); + } + /* Initialize communication */ + err_ = bmp3_init(&dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Set defaults */ + req_settings_.press_en = BMP3_ENABLE; + req_settings_.temp_en = BMP3_ENABLE; + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_8X; + req_settings_.odr_filter.temp_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.odr = BMP3_ODR_50_HZ; + req_settings_.odr_filter.iir_filter = BMP3_IIR_FILTER_COEFF_1; + req_settings_.int_settings.drdy_en = BMP3_ENABLE; + req_settings_.int_settings.latch = BMP3_INT_PIN_NON_LATCH; + req_settings_.int_settings.level = BMP3_INT_PIN_ACTIVE_HIGH; + req_settings_.int_settings.output_mode = BMP3_INT_PIN_PUSH_PULL; + settings_select_ = BMP3_SEL_PRESS_EN | BMP3_SEL_TEMP_EN | BMP3_SEL_PRESS_OS | + BMP3_SEL_TEMP_OS | BMP3_SEL_IIR_FILTER | BMP3_SEL_ODR | + BMP3_SEL_OUTPUT_MODE | BMP3_SEL_LEVEL | BMP3_SEL_LATCH | + BMP3_SEL_DRDY_EN; + err_ = bmp3_set_sensor_settings(settings_select_, &req_settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Get the sensor config */ + err_ = bmp3_get_sensor_settings(&settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Set the power mode */ + req_settings_.op_mode = BMP3_MODE_NORMAL; + err_ = bmp3_set_op_mode(&req_settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Get the power mode */ + err_ = bmp3_get_op_mode(&power_mode_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + os_mode_ = OS_MODE_PRES_8X_TEMP_1X; + settings_.op_mode = power_mode_; + return true; +} + +bool Bmp3::ConfigOsMode(const OsMode mode) { + switch (mode) { + case OS_MODE_PRES_1X_TEMP_1X: { + req_settings_.odr_filter.press_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.temp_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.odr = BMP3_ODR_200_HZ; + break; + } + case OS_MODE_PRES_2X_TEMP_1X: { + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_2X; + req_settings_.odr_filter.temp_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.odr = BMP3_ODR_100_HZ; + break; + } + case OS_MODE_PRES_4X_TEMP_1X: { + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_4X; + req_settings_.odr_filter.temp_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.odr = BMP3_ODR_50_HZ; + break; + } + case OS_MODE_PRES_8X_TEMP_1X: { + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_8X; + req_settings_.odr_filter.temp_os = BMP3_NO_OVERSAMPLING; + req_settings_.odr_filter.odr = BMP3_ODR_50_HZ; + break; + } + case OS_MODE_PRES_16X_TEMP_2X: { + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_16X; + req_settings_.odr_filter.temp_os = BMP3_OVERSAMPLING_2X; + req_settings_.odr_filter.odr = BMP3_ODR_25_HZ; + break; + } + case OS_MODE_PRES_32X_TEMP_2X: { + req_settings_.odr_filter.press_os = BMP3_OVERSAMPLING_32X; + req_settings_.odr_filter.temp_os = BMP3_OVERSAMPLING_2X; + req_settings_.odr_filter.odr = BMP3_ODR_12_5_HZ; + break; + } + } + settings_select_ = BMP3_SEL_PRESS_OS | BMP3_SEL_TEMP_OS | BMP3_SEL_ODR; + err_ = bmp3_set_sensor_settings(settings_select_, &req_settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Get the sensor config */ + err_ = bmp3_get_sensor_settings(&settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + os_mode_ = mode; + return true; +} + +bool Bmp3::ConfigFilterCoef(const FilterCoef val) { + req_settings_ = settings_; + req_settings_.odr_filter.iir_filter = static_cast(val); + settings_select_ = BMP3_SEL_IIR_FILTER; + err_ = bmp3_set_sensor_settings(settings_select_, &req_settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + /* Get the sensor config */ + err_ = bmp3_get_sensor_settings(&settings_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + return true; +} + +bool Bmp3::Read() { + err_ = bmp3_get_status(&status_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + if (status_.intr.drdy) { + err_ = bmp3_get_sensor_data(BMP3_PRESS_TEMP, &data_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + err_ = bmp3_get_status(&status_, &dev_); + if (err_ != BMP3_OK) { + return false; + } + return true; + } + return false; +} + +void Bmp3::Delay_us(uint32_t period, void *intf_ptr) { + delayMicroseconds(period); +} + +int8_t Bmp3::I2cWriteRegisters(uint8_t reg, const uint8_t * data, + uint32_t len, void * intf) { + /* NULL pointer check */ + if ((!data) || (!intf)) { + return BMP3_E_NULL_PTR; + } + /* Check length */ + if (len == 0) { + return BMP3_E_INVALID_LEN; + } + I2cIntf *iface = static_cast(intf); + iface->i2c->beginTransmission(iface->addr); + if (iface->i2c->write(reg) != 1) { + return BMP3_E_COMM_FAIL; + } + if (iface->i2c->write(data, len) != len) { + return BMP3_E_COMM_FAIL; + } + iface->i2c->endTransmission(); + return BMP3_OK; +} + +int8_t Bmp3::I2cReadRegisters(uint8_t reg, uint8_t * data, uint32_t len, + void * intf) { + /* NULL pointer check */ + if ((!data) || (!intf)) { + return BMP3_E_NULL_PTR; + } + /* Check length */ + if (len == 0) { + return BMP3_E_INVALID_LEN; + } + I2cIntf *iface = static_cast(intf); + iface->i2c->beginTransmission(iface->addr); + if (iface->i2c->write(reg) != 1) { + return BMP3_E_COMM_FAIL; + } + iface->i2c->endTransmission(false); + if (iface->i2c->requestFrom(iface->addr, len) != len) { + return BMP3_E_COMM_FAIL; + } + for (size_t i = 0; i < len; i++) { + data[i] = iface->i2c->read(); + } + return BMP3_OK; +} + +int8_t Bmp3::SpiWriteRegisters(uint8_t reg, const uint8_t * data, + uint32_t len, void * intf) { + /* NULL pointer check */ + if ((!data) || (!intf)) { + return BMP3_E_NULL_PTR; + } + /* Check length */ + if (len == 0) { + return BMP3_E_INVALID_LEN; + } + SpiIntf *iface = static_cast(intf); + iface->spi->beginTransaction(SPISettings(SPI_CLK_, MSBFIRST, SPI_MODE3)); + #if defined(TEENSYDUINO) + digitalWriteFast(iface->cs, LOW); + #else + digitalWrite(iface->cs, LOW); + #endif + iface->spi->transfer(reg); + for (size_t i = 0; i < len; i++) { + iface->spi->transfer(data[i]); + } + #if defined(TEENSYDUINO) + digitalWriteFast(iface->cs, HIGH); + #else + digitalWrite(iface->cs, HIGH); + #endif + iface->spi->endTransaction(); + return BMP3_OK; +} + +int8_t Bmp3::SpiReadRegisters(uint8_t reg, uint8_t * data, uint32_t len, + void * intf) { + /* NULL pointer check */ + if ((!data) || (!intf)) { + return BMP3_E_NULL_PTR; + } + /* Check length */ + if (len == 0) { + return BMP3_E_INVALID_LEN; + } + SpiIntf *iface = static_cast(intf); + iface->spi->beginTransaction(SPISettings(SPI_CLK_, MSBFIRST, SPI_MODE3)); + #if defined(TEENSYDUINO) + digitalWriteFast(iface->cs, LOW); + #else + digitalWrite(iface->cs, LOW); + #endif + iface->spi->transfer(reg); + for (size_t i = 0; i < len; i++) { + data[i] = iface->spi->transfer(0x00); + } + #if defined(TEENSYDUINO) + digitalWriteFast(iface->cs, HIGH); + #else + digitalWrite(iface->cs, HIGH); + #endif + iface->spi->endTransaction(); + return BMP3_OK; +} + +} // namespace bfs diff --git a/src/madflight/baro/bmp3/bmp3.h b/src/madflight/baro/bmp3/bmp3.h new file mode 100644 index 0000000..7421575 --- /dev/null +++ b/src/madflight/baro/bmp3/bmp3.h @@ -0,0 +1,123 @@ +/* +* Brian R Taylor +* brian.taylor@bolderflight.com +* +* Copyright (c) 2022 Bolder Flight Systems Inc +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the “Software”), to +* deal in the Software without restriction, including without limitation the +* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +* sell copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: +* +* The above copyright notice and this permission notice shall be included in +* all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +* IN THE SOFTWARE. +*/ + +#ifndef BMP3_SRC_BMP3_H_ // NOLINT +#define BMP3_SRC_BMP3_H_ + +#if defined(ARDUINO) +#include +#include "Wire.h" +#include "SPI.h" +#else +#include +#include +#include "core/core.h" +#endif +#include "bst/bmp3.h" + +namespace bfs { + +class Bmp3 { + public: + enum I2cAddr : uint8_t { + I2C_ADDR_PRIM = BMP3_ADDR_I2C_PRIM, + I2C_ADDR_SEC = BMP3_ADDR_I2C_SEC + }; + enum OsMode : uint8_t { + OS_MODE_PRES_1X_TEMP_1X, + OS_MODE_PRES_2X_TEMP_1X, + OS_MODE_PRES_4X_TEMP_1X, + OS_MODE_PRES_8X_TEMP_1X, + OS_MODE_PRES_16X_TEMP_2X, + OS_MODE_PRES_32X_TEMP_2X + }; + enum FilterCoef : uint8_t { + FILTER_COEF_OFF = BMP3_IIR_FILTER_DISABLE, + FILTER_COEF_2 = BMP3_IIR_FILTER_COEFF_1, + FILTER_COEF_4 = BMP3_IIR_FILTER_COEFF_3, + FILTER_COEF_8 = BMP3_IIR_FILTER_COEFF_7, + FILTER_COEF_16 = BMP3_IIR_FILTER_COEFF_15, + FILTER_COEF_32 = BMP3_IIR_FILTER_COEFF_31, + FILTER_COEF_64 = BMP3_IIR_FILTER_COEFF_63, + FILTER_COEF_128 = BMP3_IIR_FILTER_COEFF_127, + }; + Bmp3() {} + Bmp3(TwoWire *i2c, const I2cAddr addr); + Bmp3(SPIClass *spi, const uint8_t cs); + void Config(TwoWire *i2c, const I2cAddr addr); + void Config(SPIClass *spi, const uint8_t cs); + bool Begin(); + bool ConfigOsMode(const OsMode mode); + inline OsMode os_mode() const {return os_mode_;} + bool ConfigFilterCoef(const FilterCoef val); + inline FilterCoef filter_coef() const { + return static_cast(settings_.odr_filter.iir_filter); + } + bool Read(); + inline bool Reset() {return (bmp3_soft_reset(&dev_) == BMP3_OK);} + inline double pres_pa() const {return data_.pressure;} + inline double die_temp_c() const {return data_.temperature;} + inline int8_t error_code() const {return err_;} + + private: + /* Error codes */ + int8_t err_; + /* BMP3 device settings */ + bmp3_dev dev_; + /* BMP3 data */ + bmp3_data data_; + /* BMP2 config */ + OsMode os_mode_; + uint8_t power_mode_; + uint32_t settings_select_; + bmp3_settings req_settings_, settings_; + bmp3_status status_; + /* Description of I2C and SPI interfaces */ + struct I2cIntf { + uint8_t addr; + TwoWire *i2c; + } i2c_intf_; + struct SpiIntf { + uint8_t cs; + SPIClass *spi; + } spi_intf_; + /* SPI clock speed, 10 MHz */ + static constexpr int32_t SPI_CLK_ = 10000000; + /* Function prototype for delaying, ms */ + static void Delay_us(uint32_t period, void *intf_ptr); + /* Function prototypes for reading and writing I2C and SPI data */ + static int8_t I2cWriteRegisters(uint8_t reg, const uint8_t * data, + uint32_t len, void * intf); + static int8_t I2cReadRegisters(uint8_t reg, uint8_t * data, uint32_t len, + void * intf); + static int8_t SpiWriteRegisters(uint8_t reg, const uint8_t * data, + uint32_t len, void * intf); + static int8_t SpiReadRegisters(uint8_t reg, uint8_t * data, uint32_t len, + void * intf); +}; + +} // namespace bfs + +#endif // BMP3_SRC_BMP3_H_ NOLINT diff --git a/src/madflight/baro/bmp3/bst/bmp3.c b/src/madflight/baro/bmp3/bst/bmp3.c new file mode 100644 index 0000000..c37e9dd --- /dev/null +++ b/src/madflight/baro/bmp3/bst/bmp3.c @@ -0,0 +1,2960 @@ +/** +* Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmp3.c +* @date 2022-04-01 +* @version v2.0.6 +* +*/ + +/*! @file bmp3.c + * @brief Sensor driver for BMP3 sensor */ + +#include "bmp3.h" + +/***************** Static function declarations ******************************/ + +/*! + * @brief This internal API reads the calibration data from the sensor, parse + * it then compensates it and store in the device structure. + * + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t get_calib_data(struct bmp3_dev *dev); + +/*! + * @brief This internal API is used to parse the calibration data, compensates + * it and store it in device structure. + * + * @param[in] dev : Structure instance of bmp3_dev. + * @param[out] reg_data : Contains calibration data to be parsed. + * + */ +static void parse_calib_data(const uint8_t *reg_data, struct bmp3_dev *dev); + +/*! + * @brief This internal API gets the over sampling, ODR and filter settings + * from the sensor. + * + * @param[in] settings : Structure instance of bmp3_settings. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t get_odr_filter_settings(struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * @brief This internal API is used to parse the pressure and temperature data + * and store it in the bmp3_uncomp_data structure instance. + * + * @param[in] reg_data : Contains the register data which needs to be parsed. + * @param[out] uncomp_data : Contains the uncompensated press and temp data. + * + */ +static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data); + +/*! + * @brief This internal API is used to compensate the pressure or temperature + * or both the data according to the component selected by the user. + * + * @param[in] sensor_comp : Used to select pressure or temperature. + * @param[in] uncomp_data : Contains the uncompensated pressure and + * temperature data. + * @param[out] comp_data : Contains the compensated pressure and + * temperature data. + * @param[in] calib_data : Pointer to the calibration data structure. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t compensate_data(uint8_t sensor_comp, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_data *comp_data, + struct bmp3_calib_data *calib_data); + +#ifdef BMP3_FLOAT_COMPENSATION + +/*! + * @brief This internal API is used to compensate the raw temperature data and + * return the compensated temperature data. + * + * @param[out] temperature : Compensated temperature data in double. + * @param[in] uncomp_data : Contains the uncompensated temperature data. + * @param[in] calib_data : Pointer to calibration data structure. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + * + */ +static int8_t compensate_temperature(double *temperature, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_calib_data *calib_data); + +/*! + * @brief This internal API is used to compensate the pressure data and return + * the compensated pressure data. + * + * @param[out] comp_pressure : Compensated pressure data in double. + * @param[in] uncomp_data : Contains the uncompensated pressure data. + * @param[in] calib_data : Pointer to the calibration data structure. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t compensate_pressure(double *pressure, + const struct bmp3_uncomp_data *uncomp_data, + const struct bmp3_calib_data *calib_data); + +/*! + * @brief This internal API is used to calculate the power functionality for + * double precision floating point values. + * + * @param[in] base : Contains the base value. + * @param[in] power : Contains the power value. + * + * @return Output of power function. + * @retval Calculated power function output in float. + */ +static float pow_bmp3(double base, uint8_t power); + +#else + +/*! + * @brief This internal API is used to compensate the raw temperature data and + * return the compensated temperature data in integer data type. + * + * @param[out] temperature : Compensated temperature data in integer. + * @param[in] uncomp_data : Contains the uncompensated temperature data. + * @param[in] calib_data : Pointer to calibration data structure. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t compensate_temperature(int64_t *temperature, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_calib_data *calib_data); + +/*! + * @brief This internal API is used to compensate the pressure data and return + * the compensated pressure data in integer data type. + * + * @param[out] comp_pressure : Compensated pressure data in integer. + * @param[in] uncomp_data : Contains the uncompensated pressure data. + * @param[in] calib_data : Pointer to the calibration data structure. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t compensate_pressure(uint64_t *pressure, + const struct bmp3_uncomp_data *uncomp_data, + const struct bmp3_calib_data *calib_data); + +/*! + * @brief This internal API is used to calculate the power functionality. + * + * @param[in] base : Contains the base value. + * @param[in] power : Contains the power value. + * + * @return Output of power function. + * @retval Calculated power function output in integer. + */ +static uint32_t pow_bmp3(uint8_t base, uint8_t power); + +#endif /* BMP3_FLOAT_COMPENSATION */ + +/*! + * @brief This internal API is used to identify the settings which the user + * wants to modify in the sensor. + * + * @param[in] sub_settings : Contains the settings subset to identify particular + * group of settings which the user is interested to change. + * @param[in] settings : Contains the user specified settings. + * + * @return Indicates whether user is interested to modify the settings which + * are related to sub_settings. + * @retval True -> User wants to modify this group of settings + * @retval False -> User does not want to modify this group of settings + */ +static uint8_t are_settings_changed(uint32_t sub_settings, uint32_t settings); + +/*! + * @brief This internal API interleaves the register address between the + * register data buffer for burst write operation. + * + * @param[in] reg_addr : Contains the register address array. + * @param[out] temp_buff : Contains the temporary buffer to store the + * register data and register address. + * @param[in] reg_data : Contains the register data to be written in the + * temporary buffer. + * @param[in] len : No of bytes of data to be written for burst write. + * + */ +static void interleave_reg_addr(const uint8_t *reg_addr, uint8_t *temp_buff, const uint8_t *reg_data, uint32_t len); + +/*! + * @brief This internal API sets the pressure enable and + * temperature enable settings of the sensor. + * + * @param[in] desired_settings : Contains the settings which user wants to + * change. + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_pwr_ctrl_settings(uint32_t desired_settings, + const struct bmp3_settings *settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API sets the over sampling, ODR and filter settings of + * the sensor based on the settings selected by the user. + * + * @param[in] desired_settings : Variable used to select the settings which + * are to be set. + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_odr_filter_settings(uint32_t desired_settings, struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * @brief This internal API sets the interrupt control (output mode, level, + * latch and data ready) settings of the sensor based on the settings + * selected by the user. + * + * @param[in] desired_settings : Variable used to select the settings which + * are to be set. + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_int_ctrl_settings(uint32_t desired_settings, + const struct bmp3_settings *settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API sets the advance (i2c_wdt_en, i2c_wdt_sel) + * settings of the sensor based on the settings selected by the user. + * + * @param[in] desired_settings : Variable used to select the settings which + * are to be set. + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_advance_settings(uint32_t desired_settings, const struct bmp3_settings *settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API fills the register address and register data of the + * the over sampling settings for burst write operation. + * + * @param[in] desired_settings : Variable which specifies the settings which + * are to be set in the sensor. + * @param[out] addr : To store the address to fill in register buffer. + * @param[out] reg_data : To store the osr register data. + * @param[out] len : To store the len for burst write. + * @param[in] settings : Structure instance of bmp3_settings + * + */ +static void fill_osr_data(uint32_t desired_settings, + uint8_t *addr, + uint8_t *reg_data, + uint8_t *len, + const struct bmp3_settings *settings); + +/*! + * @brief This internal API fills the register address and register data of the + * the ODR settings for burst write operation. + * + * @param[out] addr : To store the address to fill in register buffer. + * @param[out] reg_data : To store the register data to set the odr data. + * @param[out] len : To store the len for burst write. + * @param[in] settings : Structure instance of bmp3_settings + * + */ +static void fill_odr_data(uint8_t *addr, uint8_t *reg_data, uint8_t *len, struct bmp3_settings *settings); + +/*! + * @brief This internal API fills the register address and register data of the + * the filter settings for burst write operation. + * + * @param[out] addr : To store the address to fill in register buffer. + * @param[out] reg_data : To store the register data to set the filter. + * @param[out] len : To store the len for burst write. + * @param[in] settings : Structure instance of bmp3_settings + * + */ +static void fill_filter_data(uint8_t *addr, uint8_t *reg_data, uint8_t *len, const struct bmp3_settings *settings); + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t null_ptr_check(const struct bmp3_dev *dev); + +/*! + * @brief This internal API parse the power control(power mode, pressure enable + * and temperature enable), over sampling, ODR, filter and interrupt control + * settings and store in the device structure. + * + * @param[in] reg_data : Register data to be parsed. + * @param[in] settings : Structure instance of bmp3_settings + */ +static void parse_sett_data(const uint8_t *reg_data, struct bmp3_settings *settings); + +/*! + * @brief This internal API parse the power control(power mode, pressure enable + * and temperature enable) settings and store in the device structure. + * + * @param[in] reg_data : Pointer variable which stores the register data to + * parse. + * @param[out] settings : Structure instance of bmp3_settings. + */ +static void parse_pwr_ctrl_settings(const uint8_t *reg_data, struct bmp3_settings *settings); + +/*! + * @brief This internal API parse the over sampling, ODR and filter + * settings and store in the device structure. + * + * @param[in] reg_data : Pointer variable which stores the register data to + * parse. + * @param[out] settings : Structure instance of bmp3_odr_filter_settings. + */ +static void parse_odr_filter_settings(const uint8_t *reg_data, struct bmp3_odr_filter_settings *settings); + +/*! + * @brief This internal API parse the interrupt control(output mode, level, + * latch and data ready) settings and store in the device structure. + * + * @param[in] reg_data : Pointer variable which stores the register data to + * parse. + * @param[out] settings : Structure instance of bmp3_int_ctrl_settings. + */ +static void parse_int_ctrl_settings(const uint8_t *reg_data, struct bmp3_int_ctrl_settings *settings); + +/*! + * @brief This internal API parse the advance (i2c_wdt_en, i2c_wdt_sel) + * settings and store in the device structure. + * + * @param[in] reg_data : Pointer variable which stores the register data to + * parse. + * @param[out] settings : Structure instance of bmp3_adv_settings. + */ +static void parse_advance_settings(const uint8_t *reg_data, struct bmp3_adv_settings *settings); + +/*! + * @brief This internal API validate the normal mode settings of the sensor. + * + * @param[out] settings : Structure instance of bmp3_settings. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t validate_normal_mode_settings(struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * @brief This internal API validate the over sampling, ODR settings of the + * sensor. + * + * @param[out] settings : Structure instance of bmp3_settings. + * + * @return Indicates whether ODR and OSR settings are valid or not. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static int8_t validate_osr_and_odr_settings(const struct bmp3_settings *settings); + +/*! + * @brief This internal API calculates the pressure measurement duration of the + * sensor. + * + * @param[out] settings : Structure instance of bmp3_settings. + * + * @return Pressure measurement time + * @retval Pressure measurement time in microseconds + */ +static uint32_t calculate_press_meas_time(const struct bmp3_settings *settings); + +/*! + * @brief This internal API calculates the temperature measurement duration of + * the sensor. + * + * @param[out] settings : Structure instance of bmp3_settings. + * + * @return Temperature measurement time + * @retval Temperature measurement time in microseconds + */ +static uint32_t calculate_temp_meas_time(const struct bmp3_settings *settings); + +/*! + * @brief This internal API checks whether the measurement time and ODR duration + * of the sensor are proper. + * + * @param[in] meas_t : Pressure and temperature measurement time in microseconds. + * @param[in] odr_duration : Duration in microseconds corresponding to the ODR + * value. + * + * @return Indicates whether ODR and OSR settings are valid or not. + * @retval 0 -> Success + * @retval >0 -> Warning + */ +static int8_t verify_meas_time_and_odr_duration(uint32_t meas_t, uint32_t odr_duration); + +/*! + * @brief This internal API puts the device to sleep mode. + * + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t put_device_to_sleep(struct bmp3_dev *dev); + +/*! + * @brief This internal API sets the normal mode in the sensor. + * + * @param[in] settings : Structure instance of bmp3_settings. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t set_normal_mode(struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * @brief This internal API writes the power mode in the sensor. + * + * @param[out] settings : Structure instance of bmp3_settings. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Fail + */ +static int8_t write_power_mode(const struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * @brief This internal API fills the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en) settings in the + * reg_data variable so as to burst write in the sensor. + * + * @param[in] desired_settings : Variable which specifies the settings which + * are to be set in the sensor. + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev + */ +static int8_t fill_fifo_config_1(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API fills the fifo_config_2(fifo_sub_sampling, + * data_select) settings in the reg_data variable so as to burst write + * in the sensor. + * + * @param[in] desired_settings : Variable which specifies the settings which + * are to be set in the sensor. + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev + */ +static int8_t fill_fifo_config_2(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API fills the fifo interrupt control(fwtm_en, ffull_en) + * settings in the reg_data variable so as to burst write in the sensor. + * + * @param[in] desired_settings : Variable which specifies the settings which + * are to be set in the sensor. + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev + */ +static int8_t fill_fifo_int_ctrl(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * @brief This internal API is used to parse the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings and store it in device structure + * + * @param[in] reg_data : Pointer variable which stores the fifo settings data + * read from the sensor. + * @param[out] fifo_settings : Structure instance of bmp3_fifo_settings which + * contains the fifo settings after parsing. + */ +static void parse_fifo_settings(const uint8_t *reg_data, struct bmp3_fifo_settings *fifo_settings); + +/*! + * @brief This internal API parse the FIFO data frame from the fifo buffer and + * fills the byte count, uncompensated pressure and/or temperature data and no + * of parsed frames. + * + * @param[in] header : Pointer variable which stores the fifo settings data + * read from the sensor. + * @param[in,out] fifo : Structure instance of bmp3_fifo + * @param[out] byte_index : Byte count which is incremented according to the + * of data. + * @param[out] uncomp_data : Uncompensated pressure and/or temperature data + * which is stored after parsing fifo buffer data. + * @param[out] parsed_frames : Total number of parsed frames. + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static uint8_t parse_fifo_data_frame(uint8_t header, + struct bmp3_fifo_data *fifo, + uint16_t *byte_index, + struct bmp3_uncomp_data *uncomp_data, + uint8_t *parsed_frames); + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count, uncompensated pressure and/or temperature data. + * + * @param[out] byte_index : Byte count of fifo buffer. + * @param[in] fifo_buffer : FIFO buffer from where the temperature and pressure + * frames are unpacked. + * @param[out] uncomp_data : Uncompensated temperature and pressure data after + * unpacking from fifo buffer. + */ +static void unpack_temp_press_frame(uint16_t *byte_index, + const uint8_t *fifo_buffer, + struct bmp3_uncomp_data *uncomp_data); + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count and uncompensated pressure data. + * + * @param[out] byte_index : Byte count of fifo buffer. + * @param[in] fifo_buffer : FIFO buffer from where the pressure frames are + * unpacked. + * @param[out] uncomp_data : Uncompensated pressure data after unpacking from + * fifo buffer. + */ +static void unpack_press_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, struct bmp3_uncomp_data *uncomp_data); + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count and uncompensated temperature data. + * + * @param[out] byte_index : Byte count of fifo buffer. + * @param[in] fifo_buffer : FIFO buffer from where the temperature frames + * are unpacked. + * @param[out] uncomp_data : Uncompensated temperature data after unpacking from + * fifo buffer. + */ +static void unpack_temp_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, struct bmp3_uncomp_data *uncomp_data); + +/*! + * @brief This internal API unpacks the time frame from the fifo data buffer and + * fills the byte count and update the sensor time variable. + * + * @param[out] byte_index : Byte count of fifo buffer. + * @param[in] fifo_buffer : FIFO buffer from where the sensor time frames + * are unpacked. + * @param[out] sensor_time : Variable used to store the sensor time. + */ +static void unpack_time_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, uint32_t *sensor_time); + +/*! + * @brief This internal API parses the FIFO buffer and gets the header info. + * + * @param[out] header : Variable used to store the fifo header data. + * @param[in] fifo_buffer : FIFO buffer from where the header data is retrieved. + * @param[out] byte_index : Byte count of fifo buffer. + */ +static void get_header_info(uint8_t *header, const uint8_t *fifo_buffer, uint16_t *byte_index); + +/*! + * @brief This internal API parses the FIFO data frame from the fifo buffer and + * fills uncompensated temperature and/or pressure data. + * + * @param[in] sensor_comp : Variable used to select either temperature or + * pressure or both while parsing the fifo frames. + * @param[in] fifo_buffer : FIFO buffer where the temperature or pressure or + * both the data to be parsed. + * @param[out] uncomp_data : Uncompensated temperature or pressure or both the + * data after unpacking from fifo buffer. + */ +static void parse_fifo_sensor_data(uint8_t sensor_comp, const uint8_t *fifo_buffer, + struct bmp3_uncomp_data *uncomp_data); + +/*! + * @brief This internal API resets the FIFO buffer, start index, + * parsed frame count, configuration change, configuration error and + * frame_not_available variables. + * + * @param[out] fifo : FIFO structure instance where the fifo related variables + * are reset. + */ +static void reset_fifo_index(struct bmp3_fifo_data *fifo); + +/*! + * @brief This API gets the command ready, data ready for pressure and + * temperature, power on reset status from the sensor. + * + * @param[out] : Structure instance of bmp3_status + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static int8_t get_sensor_status(struct bmp3_status *status, struct bmp3_dev *dev); + +/*! + * @brief This API gets the interrupt (fifo watermark, fifo full, data ready) + * status from the sensor. + * + * @param[out] : Structure instance of bmp3_status + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static int8_t get_int_status(struct bmp3_status *status, struct bmp3_dev *dev); + +/*! + * @brief This API gets the fatal, command and configuration error + * from the sensor. + * + * @param[out] : Structure instance of bmp3_status + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static int8_t get_err_status(struct bmp3_status *status, struct bmp3_dev *dev); + +/*! + * @brief This internal API converts the no. of frames required by the user to + * bytes so as to write in the watermark length register. + * + * @param[out] watermark_len : Pointer variable which contains the watermark + * length. + * @param[in] fifo : Structure instance of bmp3_fifo_data + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Fail + */ +static int8_t convert_frames_to_bytes(uint16_t *watermark_len, + const struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings); + +/****************** Global Function Definitions *******************************/ + +/*! + * @brief This API is the entry point. + * It performs the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id and calibration data of the sensor. + */ +int8_t bmp3_init(struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t chip_id = 0; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if (rslt == BMP3_OK) + { + /* Read mechanism according to selected interface */ + if (dev->intf != BMP3_I2C_INTF) + { + /* If SPI interface is selected, read extra byte */ + dev->dummy_byte = 1; + } + else + { + /* If I2C interface is selected, no need to read + * extra byte */ + dev->dummy_byte = 0; + } + + /* Read the chip-id of bmp3 sensor */ + rslt = bmp3_get_regs(BMP3_REG_CHIP_ID, &chip_id, 1, dev); + + /* Proceed if everything is fine until now */ + if (rslt == BMP3_OK) + { + /* Check for chip id validity */ + if ((chip_id == BMP3_CHIP_ID) || (chip_id == BMP390_CHIP_ID)) + { + dev->chip_id = chip_id; + + /* Reset the sensor */ + rslt = bmp3_soft_reset(dev); + if (rslt == BMP3_OK) + { + /* Read the calibration data */ + rslt = get_calib_data(dev); + } + } + else + { + rslt = BMP3_E_DEV_NOT_FOUND; + } + } + } + + return rslt; +} + +/*! + * @brief This API reads the data from the given register address of the sensor. + */ +int8_t bmp3_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, struct bmp3_dev *dev) +{ + int8_t rslt; + uint32_t idx; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if ((rslt == BMP3_OK) && (reg_data != NULL)) + { + uint32_t temp_len = len + dev->dummy_byte; + uint8_t temp_buff[len + dev->dummy_byte]; + + /* If interface selected is SPI */ + if (dev->intf != BMP3_I2C_INTF) + { + reg_addr = reg_addr | 0x80; + + /* Read the data from the register */ + dev->intf_rslt = dev->read(reg_addr, temp_buff, temp_len, dev->intf_ptr); + for (idx = 0; idx < len; idx++) + { + reg_data[idx] = temp_buff[idx + dev->dummy_byte]; + } + } + else + { + /* Read the data using I2C */ + dev->intf_rslt = dev->read(reg_addr, reg_data, len, dev->intf_ptr); + } + + /* Check for communication error */ + if (dev->intf_rslt != BMP3_INTF_RET_SUCCESS) + { + rslt = BMP3_E_COMM_FAIL; + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the given data to the register address + * of the sensor. + */ +int8_t bmp3_set_regs(uint8_t *reg_addr, const uint8_t *reg_data, uint32_t len, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t temp_buff[len * 2]; + uint32_t temp_len; + uint8_t reg_addr_cnt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Check for arguments validity */ + if ((rslt == BMP3_OK) && (reg_addr != NULL) && (reg_data != NULL)) + { + if (len != 0) + { + temp_buff[0] = reg_data[0]; + + /* If interface selected is SPI */ + if (dev->intf == BMP3_SPI_INTF) + { + for (reg_addr_cnt = 0; reg_addr_cnt < len; reg_addr_cnt++) + { + reg_addr[reg_addr_cnt] = reg_addr[reg_addr_cnt] & 0x7F; + } + } + + /* Burst write mode */ + if (len > 1) + { + /* Interleave register address w.r.t data for + * burst write*/ + interleave_reg_addr(reg_addr, temp_buff, reg_data, len); + temp_len = len * 2; + } + else + { + temp_len = len; + } + + dev->intf_rslt = dev->write(reg_addr[0], temp_buff, temp_len, dev->intf_ptr); + + /* Check for communication error */ + if (dev->intf_rslt != BMP3_INTF_RET_SUCCESS) + { + rslt = BMP3_E_COMM_FAIL; + } + } + else + { + rslt = BMP3_E_INVALID_LEN; + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the power control(pressure enable and + * temperature enable), over sampling, ODR and filter + * settings in the sensor. + */ +int8_t bmp3_set_sensor_settings(uint32_t desired_settings, struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt = BMP3_OK; + + if (settings != NULL) + { + + if (are_settings_changed(BMP3_POWER_CNTL, desired_settings)) + { + /* Set the power control settings */ + rslt = set_pwr_ctrl_settings(desired_settings, settings, dev); + } + + if (are_settings_changed(BMP3_ODR_FILTER, desired_settings)) + { + /* Set the over sampling, ODR and filter settings */ + rslt = set_odr_filter_settings(desired_settings, settings, dev); + } + + if (are_settings_changed(BMP3_INT_CTRL, desired_settings)) + { + /* Set the interrupt control settings */ + rslt = set_int_ctrl_settings(desired_settings, settings, dev); + } + + if (are_settings_changed(BMP3_ADV_SETT, desired_settings)) + { + /* Set the advance settings */ + rslt = set_advance_settings(desired_settings, settings, dev); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the power control(power mode, pressure enable and + * temperature enable), over sampling, ODR, filter, interrupt control and + * advance settings from the sensor. + */ +int8_t bmp3_get_sensor_settings(struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t settings_data[BMP3_LEN_GEN_SETT]; + + if (settings != NULL) + { + rslt = bmp3_get_regs(BMP3_REG_INT_CTRL, settings_data, BMP3_LEN_GEN_SETT, dev); + + if (rslt == BMP3_OK) + { + /* Parse the settings data */ + parse_sett_data(settings_data, settings); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings in the sensor. + */ +int8_t bmp3_set_fifo_settings(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + /* Proceed if null check is fine */ + if ((rslt == BMP3_OK) && (fifo_settings != NULL)) + { + if (are_settings_changed(BMP3_FIFO_CONFIG_1, desired_settings)) + { + /* Fill the FIFO config 1 register data */ + rslt = fill_fifo_config_1(desired_settings, fifo_settings, dev); + } + + if (are_settings_changed(desired_settings, BMP3_FIFO_CONFIG_2)) + { + /* Fill the FIFO config 2 register data */ + rslt = fill_fifo_config_2(desired_settings, fifo_settings, dev); + } + + if (are_settings_changed(desired_settings, BMP3_FIFO_INT_CTRL)) + { + /* Fill the FIFO interrupt ctrl register data */ + rslt = fill_fifo_int_ctrl(desired_settings, fifo_settings, dev); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings from the sensor. + */ +int8_t bmp3_get_fifo_settings(struct bmp3_fifo_settings *fifo_settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t fifo_sett[3]; + uint8_t len = 3; + + /* Proceed if null check is fine */ + if (fifo_settings != NULL) + { + rslt = bmp3_get_regs(BMP3_REG_FIFO_CONFIG_1, fifo_sett, len, dev); + + /* Parse the fifo settings */ + parse_fifo_settings(fifo_sett, fifo_settings); + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the fifo data from the sensor. + */ +int8_t bmp3_get_fifo_data(struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint16_t fifo_len; + + if ((fifo != NULL) && (fifo_settings != NULL)) + { + reset_fifo_index(fifo); + + /* Get the total number of bytes available in FIFO */ + rslt = bmp3_get_fifo_length(&fifo_len, dev); + + if (rslt == BMP3_OK) + { + /* For sensor time frame , add additional overhead bytes */ + if (fifo_settings->time_en == TRUE) + { + fifo_len = fifo_len + BMP3_SENSORTIME_OVERHEAD_BYTES; + } + + /* Update the fifo length in the fifo structure */ + fifo->byte_count = fifo_len; + + /* Read the fifo data */ + rslt = bmp3_get_regs(BMP3_REG_FIFO_DATA, fifo->buffer, fifo_len, dev); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the fifo watermark length according to the frames count + * set by the user in the device structure. Refer below for usage. + */ +int8_t bmp3_set_fifo_watermark(const struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data[2]; + uint8_t reg_addr[2] = { BMP3_REG_FIFO_WM, BMP3_REG_FIFO_WM + 1 }; + uint16_t watermark_len; + + if ((fifo != NULL) && (fifo_settings != NULL)) + { + rslt = convert_frames_to_bytes(&watermark_len, fifo, fifo_settings); + + if (rslt == BMP3_OK) + { + reg_data[0] = BMP3_GET_LSB(watermark_len); + reg_data[1] = BMP3_GET_MSB(watermark_len) & 0x01; + rslt = bmp3_set_regs(reg_addr, reg_data, 2, dev); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the fifo watermark length according to the frames count + * set by the user in the device structure. Refer below for usage. + */ +int8_t bmp3_get_fifo_watermark(uint16_t *watermark_len, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data[2]; + uint8_t reg_addr = BMP3_REG_FIFO_WM; + + if (watermark_len != NULL) + { + rslt = bmp3_get_regs(reg_addr, reg_data, 2, dev); + if (rslt == BMP3_OK) + { + *watermark_len = (reg_data[0]) + (reg_data[1] << 8); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API extracts the temperature and/or pressure data from the FIFO + * data which is already read from the fifo. + */ +int8_t bmp3_extract_fifo_data(struct bmp3_data *data, struct bmp3_fifo_data *fifo, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t header; + uint8_t parsed_frames = 0; + uint8_t t_p_frame; + struct bmp3_uncomp_data uncomp_data = { 0 }; + + rslt = null_ptr_check(dev); + + if ((rslt == BMP3_OK) && (fifo != NULL) && (data != NULL)) + { + uint16_t byte_index = fifo->start_idx; + + while (byte_index < fifo->byte_count) + { + get_header_info(&header, fifo->buffer, &byte_index); + t_p_frame = parse_fifo_data_frame(header, fifo, &byte_index, &uncomp_data, &parsed_frames); + + /* If the frame is pressure and/or temperature data */ + if (t_p_frame != FALSE) + { + /* Compensate temperature and pressure data */ + rslt = compensate_data(t_p_frame, &uncomp_data, &data[parsed_frames - 1], &dev->calib_data); + } + } + + /* Check if any frames are parsed in FIFO */ + if (parsed_frames != 0) + { + /* Update the bytes parsed in the device structure */ + fifo->start_idx = byte_index; + fifo->parsed_frames += parsed_frames; + } + else + { + /* No frames are there to parse. It is time to + * read the FIFO, if more frames are needed */ + fifo->frame_not_available = TRUE; + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the command ready, data ready for pressure and + * temperature and interrupt (fifo watermark, fifo full, data ready) and + * error status from the sensor. + */ +int8_t bmp3_get_status(struct bmp3_status *status, struct bmp3_dev *dev) +{ + int8_t rslt; + + if (status != NULL) + { + rslt = get_sensor_status(status, dev); + + /* Proceed further if the earlier operation is fine */ + if (rslt == BMP3_OK) + { + rslt = get_int_status(status, dev); + + /* Proceed further if the earlier operation is fine */ + if (rslt == BMP3_OK) + { + /* Get the error status */ + rslt = get_err_status(status, dev); + } + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the fifo length from the sensor. + */ +int8_t bmp3_get_fifo_length(uint16_t *fifo_length, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data[2]; + + if (fifo_length != NULL) + { + rslt = bmp3_get_regs(BMP3_REG_FIFO_LENGTH, reg_data, 2, dev); + + /* Proceed if read from register is fine */ + if (rslt == BMP3_OK) + { + /* Update the fifo length */ + *fifo_length = BMP3_CONCAT_BYTES(reg_data[1], reg_data[0]); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs the soft reset of the sensor. + */ +int8_t bmp3_soft_reset(struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_CMD; + + /* 0xB6 is the soft reset command */ + uint8_t soft_rst_cmd = BMP3_SOFT_RESET; + uint8_t cmd_rdy_status; + uint8_t cmd_err_status; + + /* Check for command ready status */ + rslt = bmp3_get_regs(BMP3_REG_SENS_STATUS, &cmd_rdy_status, 1, dev); + + /* Device is ready to accept new command */ + if ((cmd_rdy_status & BMP3_CMD_RDY) && (rslt == BMP3_OK)) + { + /* Write the soft reset command in the sensor */ + rslt = bmp3_set_regs(®_addr, &soft_rst_cmd, 1, dev); + + /* Proceed if everything is fine until now */ + if (rslt == BMP3_OK) + { + /* Wait for 2 ms */ + dev->delay_us(2000, dev->intf_ptr); + + /* Read for command error status */ + rslt = bmp3_get_regs(BMP3_REG_ERR, &cmd_err_status, 1, dev); + + /* check for command error status */ + if ((cmd_err_status & BMP3_REG_CMD) || (rslt != BMP3_OK)) + { + /* Command not written hence return + * error */ + rslt = BMP3_E_CMD_EXEC_FAILED; + } + } + } + + return rslt; +} + +/*! + * @brief This API performs the soft reset of the sensor. + */ +int8_t bmp3_fifo_flush(struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_CMD; + + uint8_t fifo_flush_cmd = BMP3_FIFO_FLUSH; + uint8_t cmd_rdy_status; + uint8_t cmd_err_status; + + /* Check for command ready status */ + rslt = bmp3_get_regs(BMP3_REG_SENS_STATUS, &cmd_rdy_status, 1, dev); + + /* Device is ready to accept new command */ + if ((cmd_rdy_status & BMP3_CMD_RDY) && (rslt == BMP3_OK)) + { + /* Write the soft reset command in the sensor */ + rslt = bmp3_set_regs(®_addr, &fifo_flush_cmd, 1, dev); + + /* Proceed if everything is fine until now */ + if (rslt == BMP3_OK) + { + /* Wait for 2 ms */ + dev->delay_us(2000, dev->intf_ptr); + + /* Read for command error status */ + rslt = bmp3_get_regs(BMP3_REG_ERR, &cmd_err_status, 1, dev); + + /* check for command error status */ + if ((cmd_err_status & BMP3_REG_CMD) || (rslt != BMP3_OK)) + { + /* Command not written hence return + * error */ + rslt = BMP3_E_CMD_EXEC_FAILED; + } + } + } + + return rslt; +} + +/*! + * @brief This API sets the power mode of the sensor. + */ +int8_t bmp3_set_op_mode(struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t last_set_mode; + + /* Check for null pointer in the device structure */ + rslt = null_ptr_check(dev); + + if ((rslt == BMP3_OK) && (settings != NULL)) + { + uint8_t curr_mode = settings->op_mode; + + rslt = bmp3_get_op_mode(&last_set_mode, dev); + + /* If the sensor is not in sleep mode put the device to sleep + * mode */ + if ((last_set_mode != BMP3_MODE_SLEEP) && (rslt == BMP3_OK)) + { + /* Device should be put to sleep before transiting to + * forced mode or normal mode */ + rslt = put_device_to_sleep(dev); + + /* Give some time for device to go into sleep mode */ + dev->delay_us(5000, dev->intf_ptr); + } + + /* Set the power mode */ + if (rslt == BMP3_OK) + { + if (curr_mode == BMP3_MODE_NORMAL) + { + /* Set normal mode and validate + * necessary settings */ + rslt = set_normal_mode(settings, dev); + } + else if (curr_mode == BMP3_MODE_FORCED) + { + /* Set forced mode */ + rslt = write_power_mode(settings, dev); + } + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the power mode of the sensor. + */ +int8_t bmp3_get_op_mode(uint8_t *op_mode, struct bmp3_dev *dev) +{ + int8_t rslt; + + if (op_mode != NULL) + { + /* Read the power mode register */ + rslt = bmp3_get_regs(BMP3_REG_PWR_CTRL, op_mode, 1, dev); + + /* Assign the power mode in the device structure */ + *op_mode = BMP3_GET_BITS(*op_mode, BMP3_OP_MODE); + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the pressure, temperature or both data from the + * sensor, compensates the data and store it in the bmp3_data structure + * instance passed by the user. + */ +int8_t bmp3_get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data, struct bmp3_dev *dev) +{ + int8_t rslt; + + /* Array to store the pressure and temperature data read from + * the sensor */ + uint8_t reg_data[BMP3_LEN_P_T_DATA] = { 0 }; + struct bmp3_uncomp_data uncomp_data = { 0 }; + + if (comp_data != NULL) + { + /* Read the pressure and temperature data from the sensor */ + rslt = bmp3_get_regs(BMP3_REG_DATA, reg_data, BMP3_LEN_P_T_DATA, dev); + + if (rslt == BMP3_OK) + { + /* Parse the read data from the sensor */ + parse_sensor_data(reg_data, &uncomp_data); + + /* Compensate the pressure/temperature/both data read + * from the sensor */ + rslt = compensate_data(sensor_comp, &uncomp_data, comp_data, &dev->calib_data); + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +/****************** Static Function Definitions *******************************/ + +/*! + * @brief This internal API converts the no. of frames required by the user to + * bytes so as to write in the watermark length register. + */ +static int8_t convert_frames_to_bytes(uint16_t *watermark_len, + const struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings) +{ + int8_t rslt = BMP3_OK; + + if ((fifo->req_frames > 0) && (fifo->req_frames <= BMP3_FIFO_MAX_FRAMES)) + { + if (fifo_settings->press_en && fifo_settings->temp_en) + { + /* Multiply with pressure and temperature header len */ + *watermark_len = fifo->req_frames * BMP3_LEN_P_AND_T_HEADER_DATA; + } + else if (fifo_settings->temp_en || fifo_settings->press_en) + { + /* Multiply with pressure or temperature header len */ + *watermark_len = fifo->req_frames * BMP3_LEN_P_OR_T_HEADER_DATA; + } + else + { + /* No sensor is enabled */ + rslt = BMP3_W_SENSOR_NOT_ENABLED; + } + } + else + { + /* Required frame count is zero, which is invalid */ + rslt = BMP3_W_INVALID_FIFO_REQ_FRAME_CNT; + } + + return rslt; +} + +/*! + * @brief This internal API resets the FIFO buffer, start index, + * parsed frame count, configuration change, configuration error and + * frame_not_available variables. + */ +static void reset_fifo_index(struct bmp3_fifo_data *fifo) +{ + /* Loop variable */ + uint16_t index; + + /* Variable for FIFO size */ + uint16_t fifo_size = 512; + + /* The size of FIFO in BMP3 is 512 bytes */ + for (index = 0; index < fifo_size; index++) + { + /* Initialize data buffer to zero */ + fifo->buffer[index] = 0; + } + + fifo->byte_count = 0; + fifo->start_idx = 0; + fifo->parsed_frames = 0; + fifo->config_change = 0; + fifo->config_err = 0; + fifo->frame_not_available = 0; +} + +/*! + * @brief This internal API parse the FIFO data frame from the fifo buffer and + * fills the byte count, uncompensated pressure and/or temperature data and no + * of parsed frames. + */ +static uint8_t parse_fifo_data_frame(uint8_t header, + struct bmp3_fifo_data *fifo, + uint16_t *byte_index, + struct bmp3_uncomp_data *uncomp_data, + uint8_t *parsed_frames) +{ + uint8_t t_p_frame = FALSE; + + switch (header) + { + case BMP3_FIFO_TEMP_PRESS_FRAME: + unpack_temp_press_frame(byte_index, fifo->buffer, uncomp_data); + *parsed_frames = *parsed_frames + 1; + t_p_frame = BMP3_PRESS_TEMP; + break; + case BMP3_FIFO_TEMP_FRAME: + unpack_temp_frame(byte_index, fifo->buffer, uncomp_data); + *parsed_frames = *parsed_frames + 1; + t_p_frame = BMP3_TEMP; + break; + case BMP3_FIFO_PRESS_FRAME: + unpack_press_frame(byte_index, fifo->buffer, uncomp_data); + *parsed_frames = *parsed_frames + 1; + t_p_frame = BMP3_PRESS; + break; + case BMP3_FIFO_TIME_FRAME: + unpack_time_frame(byte_index, fifo->buffer, &fifo->sensor_time); + break; + case BMP3_FIFO_CONFIG_CHANGE: + fifo->config_change = 1; + *byte_index = *byte_index + 1; + break; + case BMP3_FIFO_ERROR_FRAME: + fifo->config_err = 1; + *byte_index = *byte_index + 1; + break; + case BMP3_FIFO_EMPTY_FRAME: + *byte_index = fifo->byte_count; + break; + default: + fifo->config_err = 1; + *byte_index = *byte_index + 1; + break; + } + + return t_p_frame; +} + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count, uncompensated pressure and/or temperature data. + */ +static void unpack_temp_press_frame(uint16_t *byte_index, + const uint8_t *fifo_buffer, + struct bmp3_uncomp_data *uncomp_data) +{ + parse_fifo_sensor_data((BMP3_PRESS_TEMP), &fifo_buffer[*byte_index], uncomp_data); + *byte_index = *byte_index + BMP3_LEN_P_T_DATA; +} + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count and uncompensated temperature data. + */ +static void unpack_temp_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, struct bmp3_uncomp_data *uncomp_data) +{ + parse_fifo_sensor_data(BMP3_TEMP, &fifo_buffer[*byte_index], uncomp_data); + *byte_index = *byte_index + BMP3_LEN_T_DATA; +} + +/*! + * @brief This internal API unpacks the FIFO data frame from the fifo buffer and + * fills the byte count and uncompensated pressure data. + */ +static void unpack_press_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, struct bmp3_uncomp_data *uncomp_data) +{ + parse_fifo_sensor_data(BMP3_PRESS, &fifo_buffer[*byte_index], uncomp_data); + *byte_index = *byte_index + BMP3_LEN_P_DATA; +} + +/*! + * @brief This internal API unpacks the time frame from the fifo data buffer and + * fills the byte count and update the sensor time variable. + */ +static void unpack_time_frame(uint16_t *byte_index, const uint8_t *fifo_buffer, uint32_t *sensor_time) +{ + uint16_t index = *byte_index; + uint32_t xlsb = fifo_buffer[index]; + uint32_t lsb = ((uint32_t)fifo_buffer[index + 1]) << 8; + uint32_t msb = ((uint32_t)fifo_buffer[index + 2]) << 16; + + *sensor_time = msb | lsb | xlsb; + *byte_index = *byte_index + BMP3_LEN_SENSOR_TIME; +} + +/*! + * @brief This internal API parses the FIFO data frame from the fifo buffer and + * fills uncompensated temperature and/or pressure data. + */ +static void parse_fifo_sensor_data(uint8_t sensor_comp, const uint8_t *fifo_buffer, + struct bmp3_uncomp_data *uncomp_data) +{ + /* Temporary variables to store the sensor data */ + uint32_t data_xlsb; + uint32_t data_lsb; + uint32_t data_msb; + + /* Store the parsed register values for temperature data */ + data_xlsb = (uint32_t)fifo_buffer[0]; + data_lsb = (uint32_t)fifo_buffer[1] << 8; + data_msb = (uint32_t)fifo_buffer[2] << 16; + + if (sensor_comp == BMP3_TEMP) + { + /* Update uncompensated temperature */ + uncomp_data->temperature = data_msb | data_lsb | data_xlsb; + } + + if (sensor_comp == BMP3_PRESS) + { + /* Update uncompensated pressure */ + uncomp_data->pressure = data_msb | data_lsb | data_xlsb; + } + + if (sensor_comp == (BMP3_PRESS_TEMP)) + { + uncomp_data->temperature = data_msb | data_lsb | data_xlsb; + + /* Store the parsed register values for pressure data */ + data_xlsb = (uint32_t)fifo_buffer[3]; + data_lsb = (uint32_t)fifo_buffer[4] << 8; + data_msb = (uint32_t)fifo_buffer[5] << 16; + uncomp_data->pressure = data_msb | data_lsb | data_xlsb; + } +} + +/*! + * @brief This internal API parses the FIFO buffer and gets the header info. + */ +static void get_header_info(uint8_t *header, const uint8_t *fifo_buffer, uint16_t *byte_index) +{ + *header = fifo_buffer[*byte_index]; + *byte_index = *byte_index + 1; +} + +/*! + * @brief This internal API sets the normal mode in the sensor. + */ +static int8_t set_normal_mode(struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t conf_err_status; + + rslt = validate_normal_mode_settings(settings, dev); + + /* If OSR and ODR settings are proper then write the power mode */ + if (rslt == BMP3_OK) + { + rslt = write_power_mode(settings, dev); + + /* check for configuration error */ + if (rslt == BMP3_OK) + { + /* Read the configuration error status */ + rslt = bmp3_get_regs(BMP3_REG_ERR, &conf_err_status, 1, dev); + + /* Check if conf. error flag is set */ + if (rslt == BMP3_OK) + { + if (conf_err_status & BMP3_ERR_CONF) + { + /* OSR and ODR configuration is not proper */ + rslt = BMP3_E_CONFIGURATION_ERR; + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API writes the power mode in the sensor. + */ +static int8_t write_power_mode(const struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_PWR_CTRL; + uint8_t op_mode = settings->op_mode; + + /* Temporary variable to store the value read from op-mode register */ + uint8_t op_mode_reg_val; + + /* Read the power mode register */ + rslt = bmp3_get_regs(reg_addr, &op_mode_reg_val, 1, dev); + + /* Set the power mode */ + if (rslt == BMP3_OK) + { + op_mode_reg_val = BMP3_SET_BITS(op_mode_reg_val, BMP3_OP_MODE, op_mode); + + /* Write the power mode in the register */ + rslt = bmp3_set_regs(®_addr, &op_mode_reg_val, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API puts the device to sleep mode. + */ +static int8_t put_device_to_sleep(struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_PWR_CTRL; + + /* Temporary variable to store the value read from op-mode register */ + uint8_t op_mode_reg_val; + + rslt = bmp3_get_regs(BMP3_REG_PWR_CTRL, &op_mode_reg_val, 1, dev); + + if (rslt == BMP3_OK) + { + /* Set the power mode */ + op_mode_reg_val = op_mode_reg_val & (~(BMP3_OP_MODE_MSK)); + + /* Write the power mode in the register */ + rslt = bmp3_set_regs(®_addr, &op_mode_reg_val, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API validate the normal mode settings of the sensor. + */ +static int8_t validate_normal_mode_settings(struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + + rslt = get_odr_filter_settings(settings, dev); + + if (rslt == BMP3_OK) + { + rslt = validate_osr_and_odr_settings(settings); + } + + return rslt; +} + +/*! + * @brief This internal API reads the calibration data from the sensor, parse + * it then compensates it and store in the device structure. + */ +static int8_t get_calib_data(struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_CALIB_DATA; + + /* Array to store calibration data */ + uint8_t calib_data[BMP3_LEN_CALIB_DATA] = { 0 }; + + /* Read the calibration data from the sensor */ + rslt = bmp3_get_regs(reg_addr, calib_data, BMP3_LEN_CALIB_DATA, dev); + + /* Parse calibration data and store it in device structure */ + parse_calib_data(calib_data, dev); + + return rslt; +} + +/*! + * @brief This internal API interleaves the register address between the + * register data buffer for burst write operation. + */ +static void interleave_reg_addr(const uint8_t *reg_addr, uint8_t *temp_buff, const uint8_t *reg_data, uint32_t len) +{ + uint32_t index; + + for (index = 1; index < len; index++) + { + temp_buff[(index * 2) - 1] = reg_addr[index]; + temp_buff[index * 2] = reg_data[index]; + } +} + +/*! + * @brief This internal API parse the power control(power mode, pressure enable + * and temperature enable), over sampling, ODR, filter, interrupt control and + * advance settings and store in the device structure. + */ +static void parse_sett_data(const uint8_t *reg_data, struct bmp3_settings *settings) +{ + /* Parse interrupt control settings and store in device structure */ + parse_int_ctrl_settings(®_data[0], &settings->int_settings); + + /* Parse advance settings and store in device structure */ + parse_advance_settings(®_data[1], &settings->adv_settings); + + /* Parse power control settings and store in device structure */ + parse_pwr_ctrl_settings(®_data[2], settings); + + /* Parse ODR and filter settings and store in device structure */ + parse_odr_filter_settings(®_data[3], &settings->odr_filter); +} + +/*! + * @brief This internal API parse the interrupt control(output mode, level, + * latch and data ready) settings and store in the device structure. + */ +static void parse_int_ctrl_settings(const uint8_t *reg_data, struct bmp3_int_ctrl_settings *settings) +{ + settings->output_mode = BMP3_GET_BITS_POS_0(*reg_data, BMP3_INT_OUTPUT_MODE); + settings->level = BMP3_GET_BITS(*reg_data, BMP3_INT_LEVEL); + settings->latch = BMP3_GET_BITS(*reg_data, BMP3_INT_LATCH); + settings->drdy_en = BMP3_GET_BITS(*reg_data, BMP3_INT_DRDY_EN); +} +static void parse_advance_settings(const uint8_t *reg_data, struct bmp3_adv_settings *settings) +{ + settings->i2c_wdt_en = BMP3_GET_BITS(*reg_data, BMP3_I2C_WDT_EN); + settings->i2c_wdt_sel = BMP3_GET_BITS(*reg_data, BMP3_I2C_WDT_SEL); +} + +/*! + * @brief This internal API parse the power control(power mode, pressure enable + * and temperature enable) settings and store in the device structure. + */ +static void parse_pwr_ctrl_settings(const uint8_t *reg_data, struct bmp3_settings *settings) +{ + settings->op_mode = BMP3_GET_BITS(*reg_data, BMP3_OP_MODE); + settings->press_en = BMP3_GET_BITS_POS_0(*reg_data, BMP3_PRESS_EN); + settings->temp_en = BMP3_GET_BITS(*reg_data, BMP3_TEMP_EN); +} + +/*! + * @brief This internal API parse the over sampling, ODR and filter + * settings and store in the device structure. + */ +static void parse_odr_filter_settings(const uint8_t *reg_data, struct bmp3_odr_filter_settings *settings) +{ + uint8_t index = 0; + + /* ODR and filter settings index starts from one (0x1C register) */ + settings->press_os = BMP3_GET_BITS_POS_0(reg_data[index], BMP3_PRESS_OS); + settings->temp_os = BMP3_GET_BITS(reg_data[index], BMP3_TEMP_OS); + + /* Move index to 0x1D register */ + index++; + settings->odr = BMP3_GET_BITS_POS_0(reg_data[index], BMP3_ODR); + + /* Move index to 0x1F register */ + index = index + 2; + settings->iir_filter = BMP3_GET_BITS(reg_data[index], BMP3_IIR_FILTER); +} + +/*! + * @brief This API sets the pressure enable and temperature enable + * settings of the sensor. + */ +static int8_t set_pwr_ctrl_settings(uint32_t desired_settings, + const struct bmp3_settings *settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_PWR_CTRL; + uint8_t reg_data; + + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + if (desired_settings & BMP3_SEL_PRESS_EN) + { + /* Set the pressure enable settings in the + * register variable */ + reg_data = BMP3_SET_BITS_POS_0(reg_data, BMP3_PRESS_EN, settings->press_en); + } + + if (desired_settings & BMP3_SEL_TEMP_EN) + { + /* Set the temperature enable settings in the + * register variable */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_TEMP_EN, settings->temp_en); + } + + /* Write the power control settings in the register */ + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the over sampling, ODR and filter settings + * of the sensor based on the settings selected by the user. + */ +static int8_t set_odr_filter_settings(uint32_t desired_settings, struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + + /* No of registers to be configured is 3*/ + uint8_t reg_addr[3] = { 0 }; + + /* No of register data to be read is 4 */ + uint8_t reg_data[4]; + uint8_t len = 0; + + rslt = bmp3_get_regs(BMP3_REG_OSR, reg_data, 4, dev); + + if (rslt == BMP3_OK) + { + if (are_settings_changed((BMP3_SEL_PRESS_OS | BMP3_SEL_TEMP_OS), desired_settings)) + { + /* Fill the over sampling register address and + * register data to be written in the sensor */ + fill_osr_data(desired_settings, reg_addr, reg_data, &len, settings); + } + + if (are_settings_changed(BMP3_SEL_ODR, desired_settings)) + { + /* Fill the output data rate register address and + * register data to be written in the sensor */ + fill_odr_data(reg_addr, reg_data, &len, settings); + } + + if (are_settings_changed(BMP3_SEL_IIR_FILTER, desired_settings)) + { + /* Fill the iir filter register address and + * register data to be written in the sensor */ + fill_filter_data(reg_addr, reg_data, &len, settings); + } + + if (settings->op_mode == BMP3_MODE_NORMAL) + { + /* For normal mode, OSR and ODR settings should + * be proper */ + rslt = validate_osr_and_odr_settings(settings); + } + + if (rslt == BMP3_OK) + { + /* Burst write the over sampling, ODR and filter + * settings in the register */ + rslt = bmp3_set_regs(reg_addr, reg_data, len, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API sets the interrupt control (output mode, level, + * latch and data ready) settings of the sensor based on the settings + * selected by the user. + */ +static int8_t set_int_ctrl_settings(uint32_t desired_settings, + const struct bmp3_settings *settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + uint8_t reg_addr; + struct bmp3_int_ctrl_settings int_settings; + + reg_addr = BMP3_REG_INT_CTRL; + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + int_settings = settings->int_settings; + + if (desired_settings & BMP3_SEL_OUTPUT_MODE) + { + /* Set the interrupt output mode bits */ + reg_data = BMP3_SET_BITS_POS_0(reg_data, BMP3_INT_OUTPUT_MODE, int_settings.output_mode); + } + + if (desired_settings & BMP3_SEL_LEVEL) + { + /* Set the interrupt level bits */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_INT_LEVEL, int_settings.level); + } + + if (desired_settings & BMP3_SEL_LATCH) + { + /* Set the interrupt latch bits */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_INT_LATCH, int_settings.latch); + } + + if (desired_settings & BMP3_SEL_DRDY_EN) + { + /* Set the interrupt data ready bits */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_INT_DRDY_EN, int_settings.drdy_en); + } + + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the advance (i2c_wdt_en, i2c_wdt_sel) + * settings of the sensor based on the settings selected by the user. + */ +static int8_t set_advance_settings(uint32_t desired_settings, const struct bmp3_settings *settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr; + uint8_t reg_data; + struct bmp3_adv_settings adv_settings = settings->adv_settings; + + reg_addr = BMP3_REG_IF_CONF; + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + if (desired_settings & BMP3_SEL_I2C_WDT_EN) + { + /* Set the i2c watch dog enable bits */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_I2C_WDT_EN, adv_settings.i2c_wdt_en); + } + + if (desired_settings & BMP3_SEL_I2C_WDT) + { + /* Set the i2c watch dog select bits */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_I2C_WDT_SEL, adv_settings.i2c_wdt_sel); + } + + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the over sampling, ODR and filter settings + * of the sensor. + */ +static int8_t get_odr_filter_settings(struct bmp3_settings *settings, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data[4]; + + /* Read data beginning from 0x1C register */ + rslt = bmp3_get_regs(BMP3_REG_OSR, reg_data, 4, dev); + + /* Parse the read data and store it in dev structure */ + parse_odr_filter_settings(reg_data, &settings->odr_filter); + + return rslt; +} + +/*! + * @brief This internal API validate the over sampling, ODR settings of the + * sensor. + */ +static int8_t validate_osr_and_odr_settings(const struct bmp3_settings *settings) +{ + int8_t rslt; + + /* According to BMP388 datasheet at Section 3.9.2. "Measurement rate in + * forced mode and normal mode" there is also the constant of 234us also to + * be considered in the sum. */ + uint32_t meas_t = 234; + uint32_t meas_t_p = 0; + + /* Sampling period corresponding to ODR in microseconds */ + uint32_t odr[18] = { + 5000, 10000, 20000, 40000, 80000, 160000, 320000, 640000, 1280000, 2560000, 5120000, 10240000, 20480000, + 40960000, 81920000, 163840000, 327680000, 655360000 + }; + + if (settings->press_en) + { + /* Calculate the pressure measurement duration */ + meas_t_p += calculate_press_meas_time(settings); + } + + if (settings->temp_en) + { + /* Calculate the temperature measurement duration */ + meas_t_p += calculate_temp_meas_time(settings); + } + + /* Constant 234us added to the summation of temperature and pressure measurement duration */ + meas_t += meas_t_p; + + rslt = verify_meas_time_and_odr_duration(meas_t, odr[settings->odr_filter.odr]); + + return rslt; +} + +/*! + * @brief This internal API checks whether the measurement time and ODR duration + * of the sensor are proper. + */ +static int8_t verify_meas_time_and_odr_duration(uint32_t meas_t, uint32_t odr_duration) +{ + int8_t rslt; + + if (meas_t < odr_duration) + { + /* If measurement duration is less than ODR duration + * then OSR and ODR settings are fine */ + rslt = BMP3_OK; + } + else + { + /* OSR and ODR settings are not proper */ + rslt = BMP3_E_INVALID_ODR_OSR_SETTINGS; + } + + return rslt; +} + +/*! + * @brief This internal API calculates the pressure measurement duration of the + * sensor. + */ +static uint32_t calculate_press_meas_time(const struct bmp3_settings *settings) +{ + uint32_t press_meas_t; + struct bmp3_odr_filter_settings odr_filter = settings->odr_filter; + +#ifdef BMP3_FLOAT_COMPENSATION + double base = 2.0; + float partial_out; +#else + uint8_t base = 2; + uint32_t partial_out; +#endif /* BMP3_FLOAT_COMPENSATION */ + partial_out = pow_bmp3(base, odr_filter.press_os); + press_meas_t = (uint32_t)(BMP3_SETTLE_TIME_PRESS + partial_out * BMP3_ADC_CONV_TIME); + + /* Output in microseconds */ + return press_meas_t; +} + +/*! + * @brief This internal API calculates the temperature measurement duration of + * the sensor. + */ +static uint32_t calculate_temp_meas_time(const struct bmp3_settings *settings) +{ + uint32_t temp_meas_t; + struct bmp3_odr_filter_settings odr_filter = settings->odr_filter; + +#ifdef BMP3_FLOAT_COMPENSATION + double base = 2.0; + float partial_out; +#else + uint8_t base = 2; + uint32_t partial_out; +#endif /* BMP3_FLOAT_COMPENSATION */ + partial_out = pow_bmp3(base, odr_filter.temp_os); + temp_meas_t = (uint32_t)(BMP3_SETTLE_TIME_TEMP + partial_out * BMP3_ADC_CONV_TIME); + + /* Output in uint32_t */ + return temp_meas_t; +} + +/*! + * @brief This internal API fills the register address and register data of + * the over sampling settings for burst write operation. + */ +static void fill_osr_data(uint32_t desired_settings, + uint8_t *addr, + uint8_t *reg_data, + uint8_t *len, + const struct bmp3_settings *settings) +{ + struct bmp3_odr_filter_settings osr_settings = settings->odr_filter; + + if (desired_settings & (BMP3_SEL_PRESS_OS | BMP3_SEL_TEMP_OS)) + { + /* Pressure over sampling settings check */ + if (desired_settings & BMP3_SEL_PRESS_OS) + { + /* Set the pressure over sampling settings in the + * register variable */ + reg_data[*len] = BMP3_SET_BITS_POS_0(reg_data[0], BMP3_PRESS_OS, osr_settings.press_os); + } + + /* Temperature over sampling settings check */ + if (desired_settings & BMP3_SEL_TEMP_OS) + { + /* Set the temperature over sampling settings in the + * register variable */ + reg_data[*len] = BMP3_SET_BITS(reg_data[0], BMP3_TEMP_OS, osr_settings.temp_os); + } + + /* 0x1C is the register address of over sampling register */ + addr[*len] = BMP3_REG_OSR; + (*len)++; + } +} + +/*! + * @brief This internal API fills the register address and register data of + * the ODR settings for burst write operation. + */ +static void fill_odr_data(uint8_t *addr, uint8_t *reg_data, uint8_t *len, struct bmp3_settings *settings) +{ + struct bmp3_odr_filter_settings *osr_settings = &settings->odr_filter; + + /* Limit the ODR to 0.001525879 Hz*/ + if (osr_settings->odr > BMP3_ODR_0_001_HZ) + { + osr_settings->odr = BMP3_ODR_0_001_HZ; + } + + /* Set the ODR settings in the register variable */ + reg_data[*len] = BMP3_SET_BITS_POS_0(reg_data[1], BMP3_ODR, osr_settings->odr); + + /* 0x1D is the register address of output data rate register */ + addr[*len] = BMP3_REG_ODR; + (*len)++; +} + +/*! + * @brief This internal API fills the register address and register data of + * the filter settings for burst write operation. + */ +static void fill_filter_data(uint8_t *addr, uint8_t *reg_data, uint8_t *len, const struct bmp3_settings *settings) +{ + struct bmp3_odr_filter_settings osr_settings = settings->odr_filter; + + /* Set the iir settings in the register variable */ + reg_data[*len] = BMP3_SET_BITS(reg_data[3], BMP3_IIR_FILTER, osr_settings.iir_filter); + + /* 0x1F is the register address of iir filter register */ + addr[*len] = BMP3_REG_CONFIG; + (*len)++; +} + +/*! + * @brief This internal API is used to parse the pressure or temperature or + * both the data and store it in the bmp3_uncomp_data structure instance. + */ +static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data) +{ + /* Temporary variables to store the sensor data */ + uint32_t data_xlsb; + uint32_t data_lsb; + uint32_t data_msb; + + /* Store the parsed register values for pressure data */ + data_xlsb = (uint32_t)reg_data[0]; + data_lsb = (uint32_t)reg_data[1] << 8; + data_msb = (uint32_t)reg_data[2] << 16; + uncomp_data->pressure = data_msb | data_lsb | data_xlsb; + + /* Store the parsed register values for temperature data */ + data_xlsb = (uint32_t)reg_data[3]; + data_lsb = (uint32_t)reg_data[4] << 8; + data_msb = (uint32_t)reg_data[5] << 16; + uncomp_data->temperature = data_msb | data_lsb | data_xlsb; +} + +/*! + * @brief This internal API is used to compensate the pressure or temperature + * or both the data according to the component selected by the user. + */ +static int8_t compensate_data(uint8_t sensor_comp, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_data *comp_data, + struct bmp3_calib_data *calib_data) +{ + int8_t rslt = BMP3_OK; + + if ((uncomp_data != NULL) && (comp_data != NULL) && (calib_data != NULL)) + { + /* If pressure and temperature component is selected */ + if (sensor_comp == BMP3_PRESS_TEMP) + { + /* + * NOTE : Temperature compensation must be done first. + * Followed by pressure compensation + * Compensated temperature updated in calib structure, + * is needed for pressure calculation + */ + + /* Compensate pressure and temperature data */ + rslt = compensate_temperature(&comp_data->temperature, uncomp_data, calib_data); + + if (rslt == BMP3_OK) + { + rslt = compensate_pressure(&comp_data->pressure, uncomp_data, calib_data); + } + } + else if (sensor_comp == BMP3_PRESS) + { + /* + * NOTE : Temperature compensation must be done first. + * Followed by pressure compensation + * Compensated temperature updated in calib structure, + * is needed for pressure calculation. + * As only pressure is enabled in 'sensor_comp', after calculating + * compensated temperature, assign it to zero. + */ + (void)compensate_temperature(&comp_data->temperature, uncomp_data, calib_data); + comp_data->temperature = 0; + + /* Compensate the pressure data */ + rslt = compensate_pressure(&comp_data->pressure, uncomp_data, calib_data); + } + else if (sensor_comp == BMP3_TEMP) + { + /* Compensate the temperature data */ + rslt = compensate_temperature(&comp_data->temperature, uncomp_data, calib_data); + + /* + * As only temperature is enabled in 'sensor_comp' + * make compensated pressure as zero + */ + comp_data->pressure = 0; + } + else + { + comp_data->pressure = 0; + comp_data->temperature = 0; + } + } + else + { + rslt = BMP3_E_NULL_PTR; + } + + return rslt; +} + +#ifdef BMP3_FLOAT_COMPENSATION + +/*! + * @brief This internal API is used to parse the calibration data, compensates + * it and store it in device structure + */ +static void parse_calib_data(const uint8_t *reg_data, struct bmp3_dev *dev) +{ + /* Temporary variable to store the aligned trim data */ + struct bmp3_reg_calib_data *reg_calib_data = &dev->calib_data.reg_calib_data; + struct bmp3_quantized_calib_data *quantized_calib_data = &dev->calib_data.quantized_calib_data; + + /* Temporary variable */ + double temp_var; + + /* 1 / 2^8 */ + temp_var = 0.00390625f; + reg_calib_data->par_t1 = BMP3_CONCAT_BYTES(reg_data[1], reg_data[0]); + quantized_calib_data->par_t1 = ((double)reg_calib_data->par_t1 / temp_var); + reg_calib_data->par_t2 = BMP3_CONCAT_BYTES(reg_data[3], reg_data[2]); + temp_var = 1073741824.0f; + quantized_calib_data->par_t2 = ((double)reg_calib_data->par_t2 / temp_var); + reg_calib_data->par_t3 = (int8_t)reg_data[4]; + temp_var = 281474976710656.0f; + quantized_calib_data->par_t3 = ((double)reg_calib_data->par_t3 / temp_var); + reg_calib_data->par_p1 = (int16_t)BMP3_CONCAT_BYTES(reg_data[6], reg_data[5]); + temp_var = 1048576.0f; + quantized_calib_data->par_p1 = ((double)(reg_calib_data->par_p1 - (16384)) / temp_var); + reg_calib_data->par_p2 = (int16_t)BMP3_CONCAT_BYTES(reg_data[8], reg_data[7]); + temp_var = 536870912.0f; + quantized_calib_data->par_p2 = ((double)(reg_calib_data->par_p2 - (16384)) / temp_var); + reg_calib_data->par_p3 = (int8_t)reg_data[9]; + temp_var = 4294967296.0f; + quantized_calib_data->par_p3 = ((double)reg_calib_data->par_p3 / temp_var); + reg_calib_data->par_p4 = (int8_t)reg_data[10]; + temp_var = 137438953472.0f; + quantized_calib_data->par_p4 = ((double)reg_calib_data->par_p4 / temp_var); + reg_calib_data->par_p5 = BMP3_CONCAT_BYTES(reg_data[12], reg_data[11]); + + /* 1 / 2^3 */ + temp_var = 0.125f; + quantized_calib_data->par_p5 = ((double)reg_calib_data->par_p5 / temp_var); + reg_calib_data->par_p6 = BMP3_CONCAT_BYTES(reg_data[14], reg_data[13]); + temp_var = 64.0f; + quantized_calib_data->par_p6 = ((double)reg_calib_data->par_p6 / temp_var); + reg_calib_data->par_p7 = (int8_t)reg_data[15]; + temp_var = 256.0f; + quantized_calib_data->par_p7 = ((double)reg_calib_data->par_p7 / temp_var); + reg_calib_data->par_p8 = (int8_t)reg_data[16]; + temp_var = 32768.0f; + quantized_calib_data->par_p8 = ((double)reg_calib_data->par_p8 / temp_var); + reg_calib_data->par_p9 = (int16_t)BMP3_CONCAT_BYTES(reg_data[18], reg_data[17]); + temp_var = 281474976710656.0f; + quantized_calib_data->par_p9 = ((double)reg_calib_data->par_p9 / temp_var); + reg_calib_data->par_p10 = (int8_t)reg_data[19]; + temp_var = 281474976710656.0f; + quantized_calib_data->par_p10 = ((double)reg_calib_data->par_p10 / temp_var); + reg_calib_data->par_p11 = (int8_t)reg_data[20]; + temp_var = 36893488147419103232.0f; + quantized_calib_data->par_p11 = ((double)reg_calib_data->par_p11 / temp_var); +} + +/*! + * @brief This internal API is used to compensate the raw temperature data and + * return the compensated temperature data in double data type. + * Returns temperature (deg Celsius) in double. + * For e.g. Returns temperature 24.26 deg Celsius + */ +static int8_t compensate_temperature(double *temperature, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_calib_data *calib_data) +{ + int8_t rslt = BMP3_OK; + int64_t uncomp_temp = uncomp_data->temperature; + double partial_data1; + double partial_data2; + + partial_data1 = (double)(uncomp_temp - calib_data->quantized_calib_data.par_t1); + partial_data2 = (double)(partial_data1 * calib_data->quantized_calib_data.par_t2); + + /* Update the compensated temperature in calib structure since this is + * needed for pressure calculation */ + calib_data->quantized_calib_data.t_lin = partial_data2 + (partial_data1 * partial_data1) * + calib_data->quantized_calib_data.par_t3; + + /* Returns compensated temperature */ + if (calib_data->quantized_calib_data.t_lin < BMP3_MIN_TEMP_DOUBLE) + { + calib_data->quantized_calib_data.t_lin = BMP3_MIN_TEMP_DOUBLE; + rslt = BMP3_W_MIN_TEMP; + } + + if (calib_data->quantized_calib_data.t_lin > BMP3_MAX_TEMP_DOUBLE) + { + calib_data->quantized_calib_data.t_lin = BMP3_MAX_TEMP_DOUBLE; + rslt = BMP3_W_MAX_TEMP; + } + + (*temperature) = calib_data->quantized_calib_data.t_lin; + + return rslt; +} + +/*! + * @brief This internal API is used to compensate the raw pressure data and + * return the compensated pressure data in double data type. + * For e.g. returns pressure in Pascal p = 95305.295 + */ +static int8_t compensate_pressure(double *pressure, + const struct bmp3_uncomp_data *uncomp_data, + const struct bmp3_calib_data *calib_data) +{ + int8_t rslt = BMP3_OK; + const struct bmp3_quantized_calib_data *quantized_calib_data = &calib_data->quantized_calib_data; + + /* Variable to store the compensated pressure */ + double comp_press; + + /* Temporary variables used for compensation */ + double partial_data1; + double partial_data2; + double partial_data3; + double partial_data4; + double partial_out1; + double partial_out2; + + partial_data1 = quantized_calib_data->par_p6 * quantized_calib_data->t_lin; + partial_data2 = quantized_calib_data->par_p7 * pow_bmp3(quantized_calib_data->t_lin, 2); + partial_data3 = quantized_calib_data->par_p8 * pow_bmp3(quantized_calib_data->t_lin, 3); + partial_out1 = quantized_calib_data->par_p5 + partial_data1 + partial_data2 + partial_data3; + partial_data1 = quantized_calib_data->par_p2 * quantized_calib_data->t_lin; + partial_data2 = quantized_calib_data->par_p3 * pow_bmp3(quantized_calib_data->t_lin, 2); + partial_data3 = quantized_calib_data->par_p4 * pow_bmp3(quantized_calib_data->t_lin, 3); + partial_out2 = uncomp_data->pressure * + (quantized_calib_data->par_p1 + partial_data1 + partial_data2 + partial_data3); + partial_data1 = pow_bmp3((double)uncomp_data->pressure, 2); + partial_data2 = quantized_calib_data->par_p9 + quantized_calib_data->par_p10 * quantized_calib_data->t_lin; + partial_data3 = partial_data1 * partial_data2; + partial_data4 = partial_data3 + pow_bmp3((double)uncomp_data->pressure, 3) * quantized_calib_data->par_p11; + comp_press = partial_out1 + partial_out2 + partial_data4; + + if (comp_press < BMP3_MIN_PRES_DOUBLE) + { + comp_press = BMP3_MIN_PRES_DOUBLE; + rslt = BMP3_W_MIN_PRES; + } + + if (comp_press > BMP3_MAX_PRES_DOUBLE) + { + comp_press = BMP3_MAX_PRES_DOUBLE; + rslt = BMP3_W_MAX_PRES; + } + + (*pressure) = comp_press; + + return rslt; +} + +/*! + * @brief This internal API is used to calculate the power functionality for + * floating point values. + */ +static float pow_bmp3(double base, uint8_t power) +{ + float pow_output = 1; + + while (power != 0) + { + pow_output = (float) base * pow_output; + power--; + } + + return pow_output; +} +#else + +/*! + * @brief This internal API is used to parse the calibration data, compensates + * it and store it in device structure + */ +static void parse_calib_data(const uint8_t *reg_data, struct bmp3_dev *dev) +{ + /* Temporary variable to store the aligned trim data */ + struct bmp3_reg_calib_data *reg_calib_data = &dev->calib_data.reg_calib_data; + + reg_calib_data->par_t1 = BMP3_CONCAT_BYTES(reg_data[1], reg_data[0]); + reg_calib_data->par_t2 = BMP3_CONCAT_BYTES(reg_data[3], reg_data[2]); + reg_calib_data->par_t3 = (int8_t)reg_data[4]; + reg_calib_data->par_p1 = (int16_t)BMP3_CONCAT_BYTES(reg_data[6], reg_data[5]); + reg_calib_data->par_p2 = (int16_t)BMP3_CONCAT_BYTES(reg_data[8], reg_data[7]); + reg_calib_data->par_p3 = (int8_t)reg_data[9]; + reg_calib_data->par_p4 = (int8_t)reg_data[10]; + reg_calib_data->par_p5 = BMP3_CONCAT_BYTES(reg_data[12], reg_data[11]); + reg_calib_data->par_p6 = BMP3_CONCAT_BYTES(reg_data[14], reg_data[13]); + reg_calib_data->par_p7 = (int8_t)reg_data[15]; + reg_calib_data->par_p8 = (int8_t)reg_data[16]; + reg_calib_data->par_p9 = (int16_t)BMP3_CONCAT_BYTES(reg_data[18], reg_data[17]); + reg_calib_data->par_p10 = (int8_t)reg_data[19]; + reg_calib_data->par_p11 = (int8_t)reg_data[20]; +} + +/*! + * @brief This internal API is used to compensate the raw temperature data and + * return the compensated temperature data in integer data type. + * Returns temperature in integer. Actual temperature is obtained by dividing by 100 + * For eg : If returned temperature is 2426 then it is 2426/100 = 24 deg Celsius + */ +static int8_t compensate_temperature(int64_t *temperature, + const struct bmp3_uncomp_data *uncomp_data, + struct bmp3_calib_data *calib_data) +{ + int8_t rslt = BMP3_OK; + int64_t partial_data1; + int64_t partial_data2; + int64_t partial_data3; + int64_t partial_data4; + int64_t partial_data5; + int64_t partial_data6; + int64_t comp_temp; + + partial_data1 = (int64_t)(uncomp_data->temperature - ((int64_t)256 * calib_data->reg_calib_data.par_t1)); + partial_data2 = (int64_t)(calib_data->reg_calib_data.par_t2 * partial_data1); + partial_data3 = (int64_t)(partial_data1 * partial_data1); + partial_data4 = (int64_t)partial_data3 * calib_data->reg_calib_data.par_t3; + partial_data5 = (int64_t)((int64_t)(partial_data2 * 262144) + partial_data4); + partial_data6 = (int64_t)(partial_data5 / 4294967296); + + /* Store t_lin in dev. structure for pressure calculation */ + calib_data->reg_calib_data.t_lin = (int64_t)partial_data6; + comp_temp = (int64_t)((partial_data6 * 25) / 16384); + + if (comp_temp < BMP3_MIN_TEMP_INT) + { + comp_temp = BMP3_MIN_TEMP_INT; + rslt = BMP3_W_MIN_TEMP; + } + + if (comp_temp > BMP3_MAX_TEMP_INT) + { + comp_temp = BMP3_MAX_TEMP_INT; + rslt = BMP3_W_MAX_TEMP; + } + + (*temperature) = comp_temp; + + return rslt; +} + +/*! + * @brief This internal API is used to compensate the raw pressure data and + * return the compensated pressure data in integer data type. + * for eg return if pressure is 9528709 which is 9528709/100 = 95287.09 Pascal + */ +static int8_t compensate_pressure(uint64_t *pressure, + const struct bmp3_uncomp_data *uncomp_data, + const struct bmp3_calib_data *calib_data) +{ + int8_t rslt = BMP3_OK; + const struct bmp3_reg_calib_data *reg_calib_data = &calib_data->reg_calib_data; + int64_t partial_data1; + int64_t partial_data2; + int64_t partial_data3; + int64_t partial_data4; + int64_t partial_data5; + int64_t partial_data6; + int64_t offset; + int64_t sensitivity; + uint64_t comp_press; + + partial_data1 = (int64_t)(reg_calib_data->t_lin * reg_calib_data->t_lin); + partial_data2 = (int64_t)(partial_data1 / 64); + partial_data3 = (int64_t)((partial_data2 * reg_calib_data->t_lin) / 256); + partial_data4 = (int64_t)((reg_calib_data->par_p8 * partial_data3) / 32); + partial_data5 = (int64_t)((reg_calib_data->par_p7 * partial_data1) * 16); + partial_data6 = (int64_t)((reg_calib_data->par_p6 * reg_calib_data->t_lin) * 4194304); + offset = (int64_t)((reg_calib_data->par_p5 * 140737488355328) + partial_data4 + partial_data5 + partial_data6); + partial_data2 = (int64_t)((reg_calib_data->par_p4 * partial_data3) / 32); + partial_data4 = (int64_t)((reg_calib_data->par_p3 * partial_data1) * 4); + partial_data5 = (int64_t)((reg_calib_data->par_p2 - (int32_t)16384) * reg_calib_data->t_lin * 2097152); + sensitivity = + (int64_t)(((reg_calib_data->par_p1 - (int32_t)16384) * 70368744177664) + partial_data2 + partial_data4 + + partial_data5); + partial_data1 = (int64_t)((sensitivity / 16777216) * uncomp_data->pressure); + partial_data2 = (int64_t)(reg_calib_data->par_p10 * reg_calib_data->t_lin); + partial_data3 = (int64_t)(partial_data2 + ((int32_t)65536 * reg_calib_data->par_p9)); + partial_data4 = (int64_t)((partial_data3 * uncomp_data->pressure) / (int32_t)8192); + + /* dividing by 10 followed by multiplying by 10 + * To avoid overflow caused by (uncomp_data->pressure * partial_data4) + */ + partial_data5 = (int64_t)((uncomp_data->pressure * (partial_data4 / 10)) / (int32_t)512); + partial_data5 = (int64_t)(partial_data5 * 10); + partial_data6 = (int64_t)(uncomp_data->pressure * uncomp_data->pressure); + partial_data2 = (int64_t)((reg_calib_data->par_p11 * partial_data6) / (int32_t)65536); + partial_data3 = (int64_t)((int64_t)(partial_data2 * uncomp_data->pressure) / 128); + partial_data4 = (int64_t)((offset / 4) + partial_data1 + partial_data5 + partial_data3); + comp_press = (((uint64_t)partial_data4 * 25) / (uint64_t)1099511627776); + + if (comp_press < BMP3_MIN_PRES_INT) + { + comp_press = BMP3_MIN_PRES_INT; + rslt = BMP3_W_MIN_PRES; + } + + if (comp_press > BMP3_MAX_PRES_INT) + { + comp_press = BMP3_MAX_PRES_INT; + rslt = BMP3_W_MAX_PRES; + } + + (*pressure) = comp_press; + + return rslt; +} + +/*! + * @brief This internal API is used to calculate the power functionality. + */ +static uint32_t pow_bmp3(uint8_t base, uint8_t power) +{ + uint32_t pow_output = 1; + + while (power != 0) + { + pow_output = base * pow_output; + power--; + } + + return pow_output; +} + +#endif + +/*! + * @brief This internal API is used to identify the settings which the user + * wants to modify in the sensor. + */ +static uint8_t are_settings_changed(uint32_t sub_settings, uint32_t desired_settings) +{ + uint8_t settings_changed = FALSE; + + if (sub_settings & desired_settings) + { + /* User wants to modify this particular settings */ + settings_changed = TRUE; + } + else + { + /* User don't want to modify this particular settings */ + settings_changed = FALSE; + } + + return settings_changed; +} + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmp3_dev *dev) +{ + int8_t rslt; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL) || + (dev->intf_ptr == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMP3_E_NULL_PTR; + } + else + { + /* Device structure is fine */ + rslt = BMP3_OK; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings and store it in device structure + */ +static void parse_fifo_settings(const uint8_t *reg_data, struct bmp3_fifo_settings *fifo_settings) +{ + uint8_t fifo_config_1_data = reg_data[0]; + uint8_t fifo_config_2_data = reg_data[1]; + uint8_t fifo_int_ctrl_data = reg_data[2]; + + /* Parse fifo config 1 register data */ + fifo_settings->mode = BMP3_GET_BITS_POS_0(fifo_config_1_data, BMP3_FIFO_MODE); + fifo_settings->stop_on_full_en = BMP3_GET_BITS(fifo_config_1_data, BMP3_FIFO_STOP_ON_FULL); + fifo_settings->time_en = BMP3_GET_BITS(fifo_config_1_data, BMP3_FIFO_TIME_EN); + fifo_settings->press_en = BMP3_GET_BITS(fifo_config_1_data, BMP3_FIFO_PRESS_EN); + fifo_settings->temp_en = BMP3_GET_BITS(fifo_config_1_data, BMP3_FIFO_TEMP_EN); + + /* Parse fifo config 2 register data */ + fifo_settings->down_sampling = BMP3_GET_BITS_POS_0(fifo_config_2_data, BMP3_FIFO_DOWN_SAMPLING); + fifo_settings->filter_en = BMP3_GET_BITS(fifo_config_2_data, BMP3_FIFO_FILTER_EN); + + /* Parse fifo related interrupt control data */ + fifo_settings->ffull_en = BMP3_GET_BITS(fifo_int_ctrl_data, BMP3_FIFO_FULL_EN); + fifo_settings->fwtm_en = BMP3_GET_BITS(fifo_int_ctrl_data, BMP3_FIFO_FWTM_EN); +} + +/*! + * @brief This internal API fills the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en) settings in the + * reg_data variable so as to burst write in the sensor. + */ +static int8_t fill_fifo_config_1(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_FIFO_CONFIG_1; + uint8_t reg_data; + + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + if (desired_settings & BMP3_SEL_FIFO_MODE) + { + /* Fill the FIFO mode register data */ + reg_data = BMP3_SET_BITS_POS_0(reg_data, BMP3_FIFO_MODE, fifo_settings->mode); + } + + if (desired_settings & BMP3_SEL_FIFO_STOP_ON_FULL_EN) + { + /* Fill the stop on full data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_STOP_ON_FULL, fifo_settings->stop_on_full_en); + } + + if (desired_settings & BMP3_SEL_FIFO_TIME_EN) + { + /* Fill the time enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_TIME_EN, fifo_settings->time_en); + } + + if (desired_settings & (BMP3_SEL_FIFO_PRESS_EN | BMP3_SEL_FIFO_TEMP_EN)) + { + /* Fill the pressure enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_PRESS_EN, fifo_settings->press_en); + + /* Fill the temperature enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_TEMP_EN, fifo_settings->temp_en); + } + + /* Write the power control settings in the register */ + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API fills the fifo_config_2(fifo_subsampling, + * data_select) settings in the reg_data variable so as to burst write + * in the sensor. + */ +static int8_t fill_fifo_config_2(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_FIFO_CONFIG_2; + uint8_t reg_data; + + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + if (desired_settings & BMP3_SEL_FIFO_DOWN_SAMPLING) + { + /* To do check Normal mode */ + /* Fill the down-sampling data */ + reg_data = BMP3_SET_BITS_POS_0(reg_data, BMP3_FIFO_DOWN_SAMPLING, fifo_settings->down_sampling); + } + + if (desired_settings & BMP3_SEL_FIFO_FILTER_EN) + { + /* Fill the filter enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_FILTER_EN, fifo_settings->filter_en); + } + + /* Write the power control settings in the register */ + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API fills the fifo interrupt control(fwtm_en, ffull_en) + * settings in the reg_data variable so as to burst write in the sensor. + */ +static int8_t fill_fifo_int_ctrl(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr = BMP3_REG_INT_CTRL; + uint8_t reg_data; + + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + if (desired_settings & BMP3_SEL_FIFO_FWTM_EN) + { + /* Fill the FIFO watermark interrupt enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_FWTM_EN, fifo_settings->fwtm_en); + } + + if (desired_settings & BMP3_SEL_FIFO_FULL_EN) + { + /* Fill the FIFO full interrupt enable data */ + reg_data = BMP3_SET_BITS(reg_data, BMP3_FIFO_FULL_EN, fifo_settings->ffull_en); + } + + /* Write the power control settings in the register */ + rslt = bmp3_set_regs(®_addr, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This API gets the command ready, data ready for pressure and + * temperature, power on reset status from the sensor. + */ +static int8_t get_sensor_status(struct bmp3_status *status, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_addr; + uint8_t reg_data; + + reg_addr = BMP3_REG_SENS_STATUS; + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + status->sensor.cmd_rdy = BMP3_GET_BITS(reg_data, BMP3_STATUS_CMD_RDY); + status->sensor.drdy_press = BMP3_GET_BITS(reg_data, BMP3_STATUS_DRDY_PRESS); + status->sensor.drdy_temp = BMP3_GET_BITS(reg_data, BMP3_STATUS_DRDY_TEMP); + reg_addr = BMP3_REG_EVENT; + rslt = bmp3_get_regs(reg_addr, ®_data, 1, dev); + status->pwr_on_rst = reg_data & 0x01; + } + + return rslt; +} + +/*! + * @brief This API gets the interrupt (fifo watermark, fifo full, data ready) + * status from the sensor. + */ +static int8_t get_int_status(struct bmp3_status *status, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmp3_get_regs(BMP3_REG_INT_STATUS, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + status->intr.fifo_wm = BMP3_GET_BITS_POS_0(reg_data, BMP3_INT_STATUS_FWTM); + status->intr.fifo_full = BMP3_GET_BITS(reg_data, BMP3_INT_STATUS_FFULL); + status->intr.drdy = BMP3_GET_BITS(reg_data, BMP3_INT_STATUS_DRDY); + } + + return rslt; +} + +/*! + * @brief This API gets the fatal, command and configuration error + * from the sensor. + */ +static int8_t get_err_status(struct bmp3_status *status, struct bmp3_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmp3_get_regs(BMP3_REG_ERR, ®_data, 1, dev); + + if (rslt == BMP3_OK) + { + status->err.fatal = BMP3_GET_BITS_POS_0(reg_data, BMP3_ERR_FATAL); + status->err.cmd = BMP3_GET_BITS(reg_data, BMP3_ERR_CMD); + status->err.conf = BMP3_GET_BITS(reg_data, BMP3_ERR_CONF); + } + + return rslt; +} diff --git a/src/madflight/baro/bmp3/bst/bmp3.h b/src/madflight/baro/bmp3/bst/bmp3.h new file mode 100644 index 0000000..886bd51 --- /dev/null +++ b/src/madflight/baro/bmp3/bst/bmp3.h @@ -0,0 +1,552 @@ +/** +* Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmp3.h +* @date 2022-04-01 +* @version v2.0.6 +* +*/ + +/*! + * @defgroup bmp3 BMP3 + */ + +#ifndef _BMP3_H +#define _BMP3_H + +/* Header includes */ +#include "bmp3_defs.h" + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmp3ApiInit + * \page bmp3_api_bmp3_init bmp3_init + * \code + * int8_t bmp3_init(struct bmp3_dev *dev); + * \endcode + * @details This API is the entry point. + * Performs the selection of I2C/SPI read mechanism according to the + * selected interface and reads the chip-id and calibration data of the sensor. + * + * @param[in,out] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_init(struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiSoftReset Soft reset + * @brief Perform soft reset of the sensor + */ + +/*! + * \ingroup bmp3ApiSoftReset + * \page bmp3_api_bmp3_soft_reset bmp3_soft_reset + * \code + * int8_t bmp3_soft_reset(const struct bmp3_dev *dev); + * \endcode + * @details This API performs the soft reset of the sensor. + * + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_soft_reset(struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiSensorS Sensor settings + * @brief Get / Set sensor settings + */ + +/*! + * \ingroup bmp3ApiSensorS + * \page bmp3_api_bmp3_set_sensor_settings bmp3_set_sensor_settings + * \code + * int8_t bmp3_set_sensor_settings(uint32_t desired_settings, struct bmp3_settings *settings, struct bmp3_dev *dev); + * \endcode + * @details This API sets the power control(pressure enable and + * temperature enable), over sampling, odr and filter + * settings in the sensor. + * + * @param[in] desired_settings : Variable used to select the settings which + * are to be set in the sensor. + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @note : Below are the macros to be used by the user for selecting the + * desired settings. User can do OR operation of these macros for configuring + * multiple settings. + * + *@verbatim + * ---------------------|---------------------------------------------- + * desired_settings | Functionality + * ---------------------|---------------------------------------------- + * BMP3_SEL_PRESS_EN | Enable/Disable pressure. + * BMP3_SEL_TEMP_EN | Enable/Disable temperature. + * BMP3_SEL_PRESS_OS | Set pressure oversampling. + * BMP3_SEL_TEMP_OS | Set temperature oversampling. + * BMP3_SEL_IIR_FILTER | Set IIR filter. + * BMP3_SEL_ODR | Set ODR. + * BMP3_SEL_OUTPUT_MODE | Set either open drain or push pull + * BMP3_SEL_LEVEL | Set interrupt pad to be active high or low + * BMP3_SEL_LATCH | Set interrupt pad to be latched or nonlatched. + * BMP3_SEL_DRDY_EN | Map/Unmap the drdy interrupt to interrupt pad. + * BMP3_SEL_I2C_WDT_EN | Enable/Disable I2C internal watch dog. + * BMP3_SEL_I2C_WDT | Set I2C watch dog timeout delay. + * ---------------------|---------------------------------------------- + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_set_sensor_settings(uint32_t desired_settings, struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiSensorS + * \page bmp3_api_bmp3_get_sensor_settings bmp3_get_sensor_settings + * \code + * int8_t bmp3_get_sensor_settings(struct bmp3_settings *settings, struct bmp3_dev *dev); + * \endcode + * @details This API gets the power control(power mode, pressure enable and + * temperature enable), over sampling, odr, filter, interrupt control and + * advance settings from the sensor. + * + * @param[out] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_sensor_settings(struct bmp3_settings *settings, struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiPowermode Power mode + * @brief Set / Get power mode of the sensor + */ + +/*! + * \ingroup bmp3ApiPowermode + * \page bmp3_api_bmp3_set_op_mode bmp3_set_op_mode + * \code + * int8_t bmp3_set_op_mode(struct bmp3_settings *settings, struct bmp3_dev *dev); + * \endcode + * @details This API sets the power mode of the sensor. + * + * @param[in] settings : Structure instance of bmp3_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + *@verbatim + * ----------------------|------------------- + * dev->settings.op_mode | Macros + * ----------------------|------------------- + * 0 | BMP3_MODE_SLEEP + * 1 | BMP3_MODE_FORCED + * 3 | BMP3_MODE_NORMAL + * ----------------------|------------------- + *@endverbatim + * + * @note : Before setting normal mode, valid odr and osr settings should be set + * in the sensor by using 'bmp3_set_sensor_settings' function. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_set_op_mode(struct bmp3_settings *settings, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiPowermode + * \page bmp3_api_bmp3_get_op_mode bmp3_get_op_mode + * \code + * int8_t bmp3_get_op_mode(uint8_t *op_mode, const struct bmp3_dev *dev); + * \endcode + * @details This API gets the power mode of the sensor. + * + * @param[in] dev : Structure instance of bmp3_dev. + * @param[out] op_mode : Pointer variable to store the op-mode. + * + *@verbatim + * ------------------------------------------ + * op_mode | Macros + * ----------------------|------------------- + * 0 | BMP3_MODE_SLEEP + * 1 | BMP3_MODE_FORCED + * 3 | BMP3_MODE_NORMAL + * ------------------------------------------ + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_op_mode(uint8_t *op_mode, struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiData Sensor Data + * @brief Get Sensor data + */ + +/*! + * \ingroup bmp3ApiData + * \page bmp3_api_bmp3_get_sensor_data bmp3_get_sensor_data + * \code + * int8_t bmp3_get_sensor_data(uint8_t sensor_comp, struct bmp3_data *data, struct bmp3_dev *dev); + * \endcode + * @details This API reads the pressure, temperature or both data from the + * sensor, compensates the data and store it in the bmp3_data structure + * instance passed by the user. + * + * @param[in] sensor_comp : Variable which selects which data to be read from + * the sensor. + * + *@verbatim + * sensor_comp | Macros + * ------------|------------------- + * 1 | BMP3_PRESS + * 2 | BMP3_TEMP + * 3 | BMP3_ALL + *@endverbatim + * + * @param[out] data : Structure instance of bmp3_data. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @note : For fixed point the compensated temperature and pressure has a multiplication factor of 100. + * Units are degree celsius and Pascal respectively. + * ie. If temperature is 2426 then temperature is 24.26 deg C + * If press is 9528709, then pressure is 95287.09 Pascal. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_sensor_data(uint8_t sensor_comp, struct bmp3_data *data, struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiRegs Registers + * @brief Read / Write data to the given register address + */ + +/*! + * \ingroup bmp3ApiRegs + * \page bmp3_api_bmp3_set_regs bmp3_set_regs + * \code + * int8_t bmp3_set_regs(uint8_t *reg_addr, const uint8_t *reg_data, uint8_t len, const struct bmp3_dev *dev); + * \endcode + * @details This API writes the given data to the register address + * of the sensor. + * + * @param[in] reg_addr : Register address to where the data to be written. + * @param[in] reg_data : Pointer to data buffer which is to be written + * in the sensor. + * @param[in] len : No. of bytes of data to write. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_set_regs(uint8_t *reg_addr, const uint8_t *reg_data, uint32_t len, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiRegs + * \page bmp3_api_bmp3_get_regs bmp3_get_regs + * \code + * int8_t bmp3_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint16_t length, const struct bmp3_dev *dev); + * \endcode + * @details This API reads the data from the given register address of the sensor. + * + * @param[in] reg_addr : Register address from where the data to be read + * @param[out] reg_data : Pointer to data buffer to store the read data. + * @param[in] len : No. of bytes of data to be read. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiFIFO FIFO + * @brief FIFO operations of the sensor + */ + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_set_fifo_settings bmp3_set_fifo_settings + * \code + * int8_t bmp3_set_fifo_settings(uint16_t desired_settings, struct bmp3_fifo_settings *fifo_settings, + * struct bmp3_dev *dev); + * \endcode + * @details This API sets the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings in the sensor. + * + * @param[in] desired_settings : Variable used to select the FIFO settings which + * are to be set in the sensor. + * + * @note : Below are the macros to be used by the user for selecting the + * desired settings. User can do OR operation of these macros for configuring + * multiple settings. + * + *@verbatim + * --------------------------------|--------------------------------------------- + * desired_settings | Functionality + * --------------------------------|--------------------------------------------- + * BMP3_SEL_FIFO_MODE | Enable/Disable FIFO + * BMP3_SEL_FIFO_STOP_ON_FULL_EN | Set FIFO stop on full interrupt + * BMP3_SEL_FIFO_TIME_EN | Enable/Disable FIFO time + * BMP3_SEL_FIFO_PRESS_EN | Enable/Disable pressure + * BMP3_SEL_FIFO_TEMP_EN | Enable/Disable temperature + * BMP3_SEL_FIFO_DOWN_SAMPLING | Set FIFO downsampling + * BMP3_SEL_FIFO_FILTER_EN | Enable/Disable FIFO filter + * BMP3_SEL_FIFO_FWTM_EN | Enable/Disable FIFO watermark interrupt + * BMP3_SEL_FIFO_FFULL_EN | Enable/Disable FIFO full interrupt + * --------------------------------|--------------------------------------------- + *@endverbatim + * + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_set_fifo_settings(uint16_t desired_settings, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_get_fifo_settings bmp3_get_fifo_settings + * \code + * int8_t bmp3_get_fifo_settings(struct bmp3_fifo_settings *fifo_settings, struct bmp3_dev *dev); + * \endcode + * @details This API gets the fifo_config_1(fifo_mode, + * fifo_stop_on_full, fifo_time_en, fifo_press_en, fifo_temp_en), + * fifo_config_2(fifo_subsampling, data_select) and int_ctrl(fwtm_en, ffull_en) + * settings from the sensor. + * + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_fifo_settings(struct bmp3_fifo_settings *fifo_settings, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_get_fifo_data bmp3_get_fifo_data + * \code + * int8_t bmp3_get_fifo_data(struct bmp3_fifo_data *fifo, + * const struct bmp3_fifo_settings *fifo_settings, + * struct bmp3_dev *dev); + * \endcode + * @details This API gets the fifo data from the sensor. + * + * @param[in,out] fifo : Structure instance of bmp3_fifo_data + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_fifo_data(struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_get_fifo_length bmp3_get_fifo_length + * \code + * int8_t bmp3_get_fifo_length(uint16_t *fifo_length, struct bmp3_dev *dev); + * \endcode + * @details This API gets the fifo length from the sensor. + * + * @param[out] fifo_length : Variable used to store the fifo length. + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_get_fifo_length(uint16_t *fifo_length, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_extract_fifo_data bmp3_extract_fifo_data + * \code + * int8_t bmp3_extract_fifo_data(struct bmp3_data *data, struct bmp3_fifo_data *fifo, struct bmp3_dev *dev); + * \endcode + * @details This API extracts the temperature and/or pressure data from the FIFO + * data which is already read from the fifo. + * + * @param[out] data : Array of bmp3_data structures where the temperature + * and pressure frames will be stored. + * @param[in] fifo : Structure instance of bmp3_fifo_data + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Error + */ +int8_t bmp3_extract_fifo_data(struct bmp3_data *data, struct bmp3_fifo_data *fifo, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_set_fifo_watermark bmp3_set_fifo_watermark + * \code + * int8_t bmp3_set_fifo_watermark(const struct bmp3_fifo_data *fifo, + * const struct bmp3_fifo_settings *fifo_settings, + * struct bmp3_dev *dev); + * \endcode + * @details This API sets the fifo watermark length according to the frames count + * set by the user in the device structure. Refer below for usage. + * + * @note: fifo.req_frames = 50; + * + * @param[in] fifo : Structure instance of bmp3_fifo_data + * @param[in] fifo_settings : Structure instance of bmp3_fifo_settings + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Error + */ +int8_t bmp3_set_fifo_watermark(const struct bmp3_fifo_data *fifo, + const struct bmp3_fifo_settings *fifo_settings, + struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_get_fifo_watermark bmp3_get_fifo_watermark + * \code + * int8_t bmp3_get_fifo_watermark(uint16_t *watermark_len, struct bmp3_dev *dev); + * \endcode + * @details This API gets the fifo watermark length according to the frames count + * set by the user in the device structure + * + * @param[in] watermark_len : Watermark level value + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Error + */ +int8_t bmp3_get_fifo_watermark(uint16_t *watermark_len, struct bmp3_dev *dev); + +/*! + * \ingroup bmp3ApiFIFO + * \page bmp3_api_bmp3_fifo_flush bmp3_fifo_flush + * \code + * int8_t bmp3_fifo_flush(const struct bmp3_dev *dev); + * \endcode + * @details This API performs fifo flush + * + * @param[in] dev : Structure instance of bmp3_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval >0 -> Warning + * @retval <0 -> Error + */ +int8_t bmp3_fifo_flush(struct bmp3_dev *dev); + +/** + * \ingroup bmp3 + * \defgroup bmp3ApiStatus Sensor Status + * @brief Read status of the sensor + */ + +/*! + * \ingroup bmp3ApiStatus + * \page bmp3_api_bmp3_get_status bmp3_get_status + * \code + * int8_t bmp3_get_status(struct bmp3_status *status, struct bmp3_dev *dev); + * \endcode + * @details This API gets the command ready, data ready for pressure and + * temperature and interrupt (fifo watermark, fifo full, data ready) and + * error status from the sensor. + * + * @param[out] status : Structure instance of bmp3_status + * @param[in] dev : Structure instance of bmp3_dev + * + * @return Result of API execution status. + * @retval 0 -> Success + * @retval <0 -> Error + */ +int8_t bmp3_get_status(struct bmp3_status *status, struct bmp3_dev *dev); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _BMP3_H */ diff --git a/src/madflight/baro/bmp3/bst/bmp3_defs.h b/src/madflight/baro/bmp3/bst/bmp3_defs.h new file mode 100644 index 0000000..9eb83a9 --- /dev/null +++ b/src/madflight/baro/bmp3/bst/bmp3_defs.h @@ -0,0 +1,920 @@ +/** +* Copyright (c) 2022 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmp3_defs.h +* @date 2022-04-01 +* @version v2.0.6 +* +*/ + +/*! @file bmp3_defs.h + * @brief Sensor driver for BMP3 sensor */ + +#ifndef BMP3_DEFS_H_ +#define BMP3_DEFS_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/********************************************************/ +/* header includes */ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/*************************** Common macros *****************************/ + +#if !defined(UINT8_C) && !defined(INT8_C) +#define INT8_C(x) S8_C(x) +#define UINT8_C(x) U8_C(x) +#endif + +#if !defined(UINT16_C) && !defined(INT16_C) +#define INT16_C(x) S16_C(x) +#define UINT16_C(x) U16_C(x) +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#define INT32_C(x) S32_C(x) +#define UINT32_C(x) U32_C(x) +#endif + +#if !defined(INT64_C) && !defined(UINT64_C) +#define INT64_C(x) S64_C(x) +#define UINT64_C(x) U64_C(x) +#endif + +/**@}*/ +/**\name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +#ifndef TRUE +#define TRUE UINT8_C(1) +#endif + +#ifndef FALSE +#define FALSE UINT8_C(0) +#endif + +/********************************************************/ +/**\name Compiler switch macros */ +#define BMP3_FLOAT_COMPENSATION +#ifndef BMP3_64BIT_COMPENSATION /*< Check if 64bit (using BMP3_64BIT_COMPENSATION) is enabled */ +#ifndef BMP3_FLOAT_COMPENSATION /*< If 64 bit integer data type is not enabled then enable + * BMP3_FLOAT_COMPENSATION */ +#define BMP3_FLOAT_COMPENSATION +#endif +#endif + +/********************************************************/ +/**\name Macro definitions */ + +/** + * BMP3_INTF_RET_TYPE is the read/write interface return type which can be overwritten by the build system. + */ +#ifndef BMP3_INTF_RET_TYPE +#define BMP3_INTF_RET_TYPE int8_t +#endif + +/** + * The last error code from read/write interface is stored in the device structure as intf_rslt. + */ +#ifndef BMP3_INTF_RET_SUCCESS +#define BMP3_INTF_RET_SUCCESS INT8_C(0) +#endif + +/**\name I2C addresses */ +#define BMP3_ADDR_I2C_PRIM UINT8_C(0x76) +#define BMP3_ADDR_I2C_SEC UINT8_C(0x77) + +/**\name BMP3 chip identifier */ +#define BMP3_CHIP_ID UINT8_C(0x50) +#define BMP390_CHIP_ID UINT8_C(0x60) + +/**\name BMP3 pressure settling time (micro secs)*/ +#define BMP3_SETTLE_TIME_PRESS UINT16_C(392) + +/**\name BMP3 temperature settling time (micro secs) */ +#define BMP3_SETTLE_TIME_TEMP UINT16_C(313) + +/**\name BMP3 adc conversion time (micro secs) */ +#define BMP3_ADC_CONV_TIME UINT16_C(2000) + +/**\name Register Address */ +#define BMP3_REG_CHIP_ID UINT8_C(0x00) +#define BMP3_REG_ERR UINT8_C(0x02) +#define BMP3_REG_SENS_STATUS UINT8_C(0x03) +#define BMP3_REG_DATA UINT8_C(0x04) +#define BMP3_REG_EVENT UINT8_C(0x10) +#define BMP3_REG_INT_STATUS UINT8_C(0x11) +#define BMP3_REG_FIFO_LENGTH UINT8_C(0x12) +#define BMP3_REG_FIFO_DATA UINT8_C(0x14) +#define BMP3_REG_FIFO_WM UINT8_C(0x15) +#define BMP3_REG_FIFO_CONFIG_1 UINT8_C(0x17) +#define BMP3_REG_FIFO_CONFIG_2 UINT8_C(0x18) +#define BMP3_REG_INT_CTRL UINT8_C(0x19) +#define BMP3_REG_IF_CONF UINT8_C(0x1A) +#define BMP3_REG_PWR_CTRL UINT8_C(0x1B) +#define BMP3_REG_OSR UINT8_C(0X1C) +#define BMP3_REG_ODR UINT8_C(0x1D) +#define BMP3_REG_CONFIG UINT8_C(0x1F) +#define BMP3_REG_CALIB_DATA UINT8_C(0x31) +#define BMP3_REG_CMD UINT8_C(0x7E) + +/**\name Error status macros */ +#define BMP3_ERR_FATAL UINT8_C(0x01) +#define BMP3_ERR_CMD UINT8_C(0x02) +#define BMP3_ERR_CONF UINT8_C(0x04) + +/**\name Status macros */ +#define BMP3_CMD_RDY UINT8_C(0x10) +#define BMP3_DRDY_PRESS UINT8_C(0x20) +#define BMP3_DRDY_TEMP UINT8_C(0x40) + +/**\name Power mode macros */ +#define BMP3_MODE_SLEEP UINT8_C(0x00) +#define BMP3_MODE_FORCED UINT8_C(0x01) +#define BMP3_MODE_NORMAL UINT8_C(0x03) + +/**\name FIFO related macros */ +/**\name FIFO enable */ +#define BMP3_ENABLE UINT8_C(0x01) +#define BMP3_DISABLE UINT8_C(0x00) + +/**\name Interrupt pin configuration macros */ +/**\name Open drain */ +#define BMP3_INT_PIN_OPEN_DRAIN UINT8_C(0x01) +#define BMP3_INT_PIN_PUSH_PULL UINT8_C(0x00) + +/**\name Level */ +#define BMP3_INT_PIN_ACTIVE_HIGH UINT8_C(0x01) +#define BMP3_INT_PIN_ACTIVE_LOW UINT8_C(0x00) + +/**\name Latch */ +#define BMP3_INT_PIN_LATCH UINT8_C(0x01) +#define BMP3_INT_PIN_NON_LATCH UINT8_C(0x00) + +/**\name Advance settings */ +/**\name I2c watch dog timer period selection */ +#define BMP3_I2C_WDT_SHORT_1_25_MS UINT8_C(0x00) +#define BMP3_I2C_WDT_LONG_40_MS UINT8_C(0x01) + +/**\name FIFO Sub-sampling macros */ +#define BMP3_FIFO_NO_SUBSAMPLING UINT8_C(0x00) +#define BMP3_FIFO_SUBSAMPLING_2X UINT8_C(0x01) +#define BMP3_FIFO_SUBSAMPLING_4X UINT8_C(0x02) +#define BMP3_FIFO_SUBSAMPLING_8X UINT8_C(0x03) +#define BMP3_FIFO_SUBSAMPLING_16X UINT8_C(0x04) +#define BMP3_FIFO_SUBSAMPLING_32X UINT8_C(0x05) +#define BMP3_FIFO_SUBSAMPLING_64X UINT8_C(0x06) +#define BMP3_FIFO_SUBSAMPLING_128X UINT8_C(0x07) + +/**\name Over sampling macros */ +#define BMP3_NO_OVERSAMPLING UINT8_C(0x00) +#define BMP3_OVERSAMPLING_2X UINT8_C(0x01) +#define BMP3_OVERSAMPLING_4X UINT8_C(0x02) +#define BMP3_OVERSAMPLING_8X UINT8_C(0x03) +#define BMP3_OVERSAMPLING_16X UINT8_C(0x04) +#define BMP3_OVERSAMPLING_32X UINT8_C(0x05) + +/**\name Filter setting macros */ +#define BMP3_IIR_FILTER_DISABLE UINT8_C(0x00) +#define BMP3_IIR_FILTER_COEFF_1 UINT8_C(0x01) +#define BMP3_IIR_FILTER_COEFF_3 UINT8_C(0x02) +#define BMP3_IIR_FILTER_COEFF_7 UINT8_C(0x03) +#define BMP3_IIR_FILTER_COEFF_15 UINT8_C(0x04) +#define BMP3_IIR_FILTER_COEFF_31 UINT8_C(0x05) +#define BMP3_IIR_FILTER_COEFF_63 UINT8_C(0x06) +#define BMP3_IIR_FILTER_COEFF_127 UINT8_C(0x07) + +/**\name Odr setting macros */ +#define BMP3_ODR_200_HZ UINT8_C(0x00) +#define BMP3_ODR_100_HZ UINT8_C(0x01) +#define BMP3_ODR_50_HZ UINT8_C(0x02) +#define BMP3_ODR_25_HZ UINT8_C(0x03) +#define BMP3_ODR_12_5_HZ UINT8_C(0x04) +#define BMP3_ODR_6_25_HZ UINT8_C(0x05) +#define BMP3_ODR_3_1_HZ UINT8_C(0x06) +#define BMP3_ODR_1_5_HZ UINT8_C(0x07) +#define BMP3_ODR_0_78_HZ UINT8_C(0x08) +#define BMP3_ODR_0_39_HZ UINT8_C(0x09) +#define BMP3_ODR_0_2_HZ UINT8_C(0x0A) +#define BMP3_ODR_0_1_HZ UINT8_C(0x0B) +#define BMP3_ODR_0_05_HZ UINT8_C(0x0C) +#define BMP3_ODR_0_02_HZ UINT8_C(0x0D) +#define BMP3_ODR_0_01_HZ UINT8_C(0x0E) +#define BMP3_ODR_0_006_HZ UINT8_C(0x0F) +#define BMP3_ODR_0_003_HZ UINT8_C(0x10) +#define BMP3_ODR_0_001_HZ UINT8_C(0x11) + +/**\name Soft reset command */ +#define BMP3_SOFT_RESET UINT8_C(0xB6) + +/**\name FIFO flush command */ +#define BMP3_FIFO_FLUSH UINT8_C(0xB0) + +/**\name API success code */ +#define BMP3_OK INT8_C(0) + +/**\name API error codes */ +#define BMP3_E_NULL_PTR INT8_C(-1) +#define BMP3_E_COMM_FAIL INT8_C(-2) +#define BMP3_E_INVALID_ODR_OSR_SETTINGS INT8_C(-3) +#define BMP3_E_CMD_EXEC_FAILED INT8_C(-4) +#define BMP3_E_CONFIGURATION_ERR INT8_C(-5) +#define BMP3_E_INVALID_LEN INT8_C(-6) +#define BMP3_E_DEV_NOT_FOUND INT8_C(-7) +#define BMP3_E_FIFO_WATERMARK_NOT_REACHED INT8_C(-8) + +/**\name API warning codes */ +#define BMP3_W_SENSOR_NOT_ENABLED INT8_C(1) +#define BMP3_W_INVALID_FIFO_REQ_FRAME_CNT INT8_C(2) +#define BMP3_W_MIN_TEMP INT8_C(3) +#define BMP3_W_MAX_TEMP INT8_C(4) +#define BMP3_W_MIN_PRES INT8_C(5) +#define BMP3_W_MAX_PRES INT8_C(6) + +/**\name Macros to select the which sensor settings are to be set by the user. + * These values are internal for API implementation. Don't relate this to + * data sheet. */ +#define BMP3_SEL_PRESS_EN UINT16_C(1 << 1) +#define BMP3_SEL_TEMP_EN UINT16_C(1 << 2) +#define BMP3_SEL_DRDY_EN UINT16_C(1 << 3) +#define BMP3_SEL_PRESS_OS UINT16_C(1 << 4) +#define BMP3_SEL_TEMP_OS UINT16_C(1 << 5) +#define BMP3_SEL_IIR_FILTER UINT16_C(1 << 6) +#define BMP3_SEL_ODR UINT16_C(1 << 7) +#define BMP3_SEL_OUTPUT_MODE UINT16_C(1 << 8) +#define BMP3_SEL_LEVEL UINT16_C(1 << 9) +#define BMP3_SEL_LATCH UINT16_C(1 << 10) +#define BMP3_SEL_I2C_WDT_EN UINT16_C(1 << 11) +#define BMP3_SEL_I2C_WDT UINT16_C(1 << 12) +#define BMP3_SEL_ALL UINT16_C(0x7FF) + +/**\name Macros to select the which FIFO settings are to be set by the user + * These values are internal for API implementation. Don't relate this to + * data sheet.*/ +#define BMP3_SEL_FIFO_MODE UINT16_C(1 << 1) +#define BMP3_SEL_FIFO_STOP_ON_FULL_EN UINT16_C(1 << 2) +#define BMP3_SEL_FIFO_TIME_EN UINT16_C(1 << 3) +#define BMP3_SEL_FIFO_PRESS_EN UINT16_C(1 << 4) +#define BMP3_SEL_FIFO_TEMP_EN UINT16_C(1 << 5) +#define BMP3_SEL_FIFO_DOWN_SAMPLING UINT16_C(1 << 6) +#define BMP3_SEL_FIFO_FILTER_EN UINT16_C(1 << 7) +#define BMP3_SEL_FIFO_FWTM_EN UINT16_C(1 << 8) +#define BMP3_SEL_FIFO_FULL_EN UINT16_C(1 << 9) + +/**\name Sensor component selection macros + * These values are internal for API implementation. Don't relate this to + * data sheet.*/ +#define BMP3_PRESS UINT8_C(1) +#define BMP3_TEMP UINT8_C(2) +#define BMP3_PRESS_TEMP UINT8_C(3) + +/**\name Temperature range values in integer and float */ +#define BMP3_MIN_TEMP_INT INT64_C(-4000) +#define BMP3_MAX_TEMP_INT INT64_C(8500) +#define BMP3_MIN_TEMP_DOUBLE -40.0f +#define BMP3_MAX_TEMP_DOUBLE 85.0f + +/**\name Pressure range values in integer and float */ +#define BMP3_MIN_PRES_INT UINT64_C(3000000) +#define BMP3_MAX_PRES_INT UINT64_C(12500000) +#define BMP3_MIN_PRES_DOUBLE 30000.0f +#define BMP3_MAX_PRES_DOUBLE 125000.0f + +/**\name Macros for bit masking */ +#define BMP3_ERR_FATAL_MSK UINT8_C(0x01) + +#define BMP3_ERR_CMD_MSK UINT8_C(0x02) +#define BMP3_ERR_CMD_POS UINT8_C(0x01) + +#define BMP3_ERR_CONF_MSK UINT8_C(0x04) +#define BMP3_ERR_CONF_POS UINT8_C(0x02) + +#define BMP3_STATUS_CMD_RDY_MSK UINT8_C(0x10) +#define BMP3_STATUS_CMD_RDY_POS UINT8_C(0x04) + +#define BMP3_STATUS_DRDY_PRESS_MSK UINT8_C(0x20) +#define BMP3_STATUS_DRDY_PRESS_POS UINT8_C(0x05) + +#define BMP3_STATUS_DRDY_TEMP_MSK UINT8_C(0x40) +#define BMP3_STATUS_DRDY_TEMP_POS UINT8_C(0x06) + +#define BMP3_OP_MODE_MSK UINT8_C(0x30) +#define BMP3_OP_MODE_POS UINT8_C(0x04) + +#define BMP3_PRESS_EN_MSK UINT8_C(0x01) + +#define BMP3_TEMP_EN_MSK UINT8_C(0x02) +#define BMP3_TEMP_EN_POS UINT8_C(0x01) + +#define BMP3_IIR_FILTER_MSK UINT8_C(0x0E) +#define BMP3_IIR_FILTER_POS UINT8_C(0x01) + +#define BMP3_ODR_MSK UINT8_C(0x1F) + +#define BMP3_PRESS_OS_MSK UINT8_C(0x07) + +#define BMP3_TEMP_OS_MSK UINT8_C(0x38) +#define BMP3_TEMP_OS_POS UINT8_C(0x03) + +#define BMP3_FIFO_MODE_MSK UINT8_C(0x01) + +#define BMP3_FIFO_STOP_ON_FULL_MSK UINT8_C(0x02) +#define BMP3_FIFO_STOP_ON_FULL_POS UINT8_C(0x01) + +#define BMP3_FIFO_TIME_EN_MSK UINT8_C(0x04) +#define BMP3_FIFO_TIME_EN_POS UINT8_C(0x02) + +#define BMP3_FIFO_PRESS_EN_MSK UINT8_C(0x08) +#define BMP3_FIFO_PRESS_EN_POS UINT8_C(0x03) + +#define BMP3_FIFO_TEMP_EN_MSK UINT8_C(0x10) +#define BMP3_FIFO_TEMP_EN_POS UINT8_C(0x04) + +#define BMP3_FIFO_FILTER_EN_MSK UINT8_C(0x18) +#define BMP3_FIFO_FILTER_EN_POS UINT8_C(0x03) + +#define BMP3_FIFO_DOWN_SAMPLING_MSK UINT8_C(0x07) + +#define BMP3_FIFO_FWTM_EN_MSK UINT8_C(0x08) +#define BMP3_FIFO_FWTM_EN_POS UINT8_C(0x03) + +#define BMP3_FIFO_FULL_EN_MSK UINT8_C(0x10) +#define BMP3_FIFO_FULL_EN_POS UINT8_C(0x04) + +#define BMP3_INT_OUTPUT_MODE_MSK UINT8_C(0x01) + +#define BMP3_INT_LEVEL_MSK UINT8_C(0x02) +#define BMP3_INT_LEVEL_POS UINT8_C(0x01) + +#define BMP3_INT_LATCH_MSK UINT8_C(0x04) +#define BMP3_INT_LATCH_POS UINT8_C(0x02) + +#define BMP3_INT_DRDY_EN_MSK UINT8_C(0x40) +#define BMP3_INT_DRDY_EN_POS UINT8_C(0x06) + +#define BMP3_I2C_WDT_EN_MSK UINT8_C(0x02) +#define BMP3_I2C_WDT_EN_POS UINT8_C(0x01) + +#define BMP3_I2C_WDT_SEL_MSK UINT8_C(0x04) +#define BMP3_I2C_WDT_SEL_POS UINT8_C(0x02) + +#define BMP3_INT_STATUS_FWTM_MSK UINT8_C(0x01) + +#define BMP3_INT_STATUS_FFULL_MSK UINT8_C(0x02) +#define BMP3_INT_STATUS_FFULL_POS UINT8_C(0x01) + +#define BMP3_INT_STATUS_DRDY_MSK UINT8_C(0x08) +#define BMP3_INT_STATUS_DRDY_POS UINT8_C(0x03) + +/**\name UTILITY MACROS */ +#define BMP3_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMP3_SET_HIGH_BYTE UINT16_C(0xFF00) + +/**\name Macro to combine two 8 bit data's to form a 16 bit data */ +#define BMP3_CONCAT_BYTES(msb, lsb) (((uint16_t)msb << 8) | (uint16_t)lsb) + +#define BMP3_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + ((data << bitname##_POS) & bitname##_MSK)) + +/* Macro variant to handle the bitname position if it is zero */ +#define BMP3_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMP3_GET_BITS(reg_data, bitname) ((reg_data & (bitname##_MSK)) >> \ + (bitname##_POS)) + +/* Macro variant to handle the bitname position if it is zero */ +#define BMP3_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +#define BMP3_GET_LSB(var) (uint8_t)(var & BMP3_SET_LOW_BYTE) +#define BMP3_GET_MSB(var) (uint8_t)((var & BMP3_SET_HIGH_BYTE) >> 8) + +/**\name Macros related to size */ +#define BMP3_LEN_CALIB_DATA UINT8_C(21) +#define BMP3_LEN_P_AND_T_HEADER_DATA UINT8_C(7) +#define BMP3_LEN_P_OR_T_HEADER_DATA UINT8_C(4) +#define BMP3_LEN_P_T_DATA UINT8_C(6) +#define BMP3_LEN_GEN_SETT UINT8_C(7) +#define BMP3_LEN_P_DATA UINT8_C(3) +#define BMP3_LEN_T_DATA UINT8_C(3) +#define BMP3_LEN_SENSOR_TIME UINT8_C(3) +#define BMP3_FIFO_MAX_FRAMES UINT8_C(73) + +/*! Power control settings */ +#define BMP3_POWER_CNTL UINT16_C(0x0006) + +/*! Odr and filter settings */ +#define BMP3_ODR_FILTER UINT16_C(0x00F0) + +/*! Interrupt control settings */ +#define BMP3_INT_CTRL UINT16_C(0x0708) + +/*! Advance settings */ +#define BMP3_ADV_SETT UINT16_C(0x1800) + +/*! FIFO settings */ + +/*! Mask for fifo_mode, fifo_stop_on_full, fifo_time_en, fifo_press_en and + * fifo_temp_en settings */ +#define BMP3_FIFO_CONFIG_1 UINT16_C(0x003E) + +/*! Mask for fifo_sub_sampling and data_select settings */ +#define BMP3_FIFO_CONFIG_2 UINT16_C(0x00C0) + +/*! Mask for fwtm_en and ffull_en settings */ +#define BMP3_FIFO_INT_CTRL UINT16_C(0x0300) + +/*! FIFO Header */ +/*! FIFO temperature pressure header frame */ +#define BMP3_FIFO_TEMP_PRESS_FRAME UINT8_C(0x94) + +/*! FIFO temperature header frame */ +#define BMP3_FIFO_TEMP_FRAME UINT8_C(0x90) + +/*! FIFO pressure header frame */ +#define BMP3_FIFO_PRESS_FRAME UINT8_C(0x84) + +/*! FIFO time header frame */ +#define BMP3_FIFO_TIME_FRAME UINT8_C(0xA0) + +/*! FIFO error header frame */ +#define BMP3_FIFO_ERROR_FRAME UINT8_C(0x44) + +/*! FIFO configuration change header frame */ +#define BMP3_FIFO_CONFIG_CHANGE UINT8_C(0x48) + +/*! FIFO empty frame */ +#define BMP3_FIFO_EMPTY_FRAME UINT8_C(0x80) + +/*! FIFO sensortime overhead byte count */ +#define BMP3_SENSORTIME_OVERHEAD_BYTES UINT8_C(20) + +/********************************************************/ + +/*! + * @brief Interface selection Enums + */ +enum bmp3_intf { + /*! SPI interface */ + BMP3_SPI_INTF, + /*! I2C interface */ + BMP3_I2C_INTF +}; + +/********************************************************/ + +/*! + * @brief Type definitions + */ + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : 8bit register address of the sensor + * @param[out] reg_data : Data from the specified address + * @param[in] length : Length of the reg_data array + * @param[in,out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related callbacks + * @retval 0 for Success + * @retval Non-zero for Failure + */ +typedef BMP3_INTF_RET_TYPE (*bmp3_read_fptr_t)(uint8_t reg_addr, uint8_t *read_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : 8bit register address of the sensor + * @param[out] reg_data : Data to the specified address + * @param[in] length : Length of the reg_data array + * @param[in,out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related callbacks + * @retval 0 for Success + * @retval Non-zero for Failure + * + */ +typedef BMP3_INTF_RET_TYPE (*bmp3_write_fptr_t)(uint8_t reg_addr, const uint8_t *read_data, uint32_t len, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param[in] period : Delay in microseconds. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + */ +typedef void (*bmp3_delay_us_fptr_t)(uint32_t period, void *intf_ptr); + +/********************************************************/ + +/*! + * @brief Register Trim Variables + */ +struct bmp3_reg_calib_data +{ + /*! Trim Variables */ + + uint16_t par_t1; + uint16_t par_t2; + int8_t par_t3; + int16_t par_p1; + int16_t par_p2; + int8_t par_p3; + int8_t par_p4; + uint16_t par_p5; + uint16_t par_p6; + int8_t par_p7; + int8_t par_p8; + int16_t par_p9; + int8_t par_p10; + int8_t par_p11; + int64_t t_lin; +}; + +/*! + * @brief bmp3 advance settings + */ +struct bmp3_adv_settings +{ + /*! I2C watchdog enable */ + uint8_t i2c_wdt_en; + + /*! I2C watchdog select */ + uint8_t i2c_wdt_sel; +}; + +/*! + * @brief bmp3 odr and filter settings + */ +struct bmp3_odr_filter_settings +{ + /*! Pressure oversampling */ + uint8_t press_os; + + /*! Temperature oversampling */ + uint8_t temp_os; + + /*! IIR filter */ + uint8_t iir_filter; + + /*! Output data rate */ + uint8_t odr; +}; + +/*! + * @brief bmp3 sensor status flags + */ +struct bmp3_sens_status +{ + /*! Command ready status */ + uint8_t cmd_rdy; + + /*! Data ready for pressure */ + uint8_t drdy_press; + + /*! Data ready for temperature */ + uint8_t drdy_temp; +}; + +/*! + * @brief bmp3 interrupt status flags + */ +struct bmp3_int_status +{ + /*! Fifo watermark interrupt */ + uint8_t fifo_wm; + + /*! Fifo full interrupt */ + uint8_t fifo_full; + + /*! Data ready interrupt */ + uint8_t drdy; +}; + +/*! + * @brief bmp3 error status flags + */ +struct bmp3_err_status +{ + /*! Fatal error */ + uint8_t fatal; + + /*! Command error */ + uint8_t cmd; + + /*! Configuration error */ + uint8_t conf; +}; + +/*! + * @brief bmp3 status flags + */ +struct bmp3_status +{ + /*! Interrupt status */ + struct bmp3_int_status intr; + + /*! Sensor status */ + struct bmp3_sens_status sensor; + + /*! Error status */ + struct bmp3_err_status err; + + /*! Power on reset status */ + uint8_t pwr_on_rst; +}; + +/*! + * @brief bmp3 interrupt pin settings + */ +struct bmp3_int_ctrl_settings +{ + /*! Output mode */ + uint8_t output_mode; + + /*! Active high/low */ + uint8_t level; + + /*! Latched or Non-latched */ + uint8_t latch; + + /*! Data ready interrupt */ + uint8_t drdy_en; +}; + +/*! + * @brief bmp3 device settings + */ +struct bmp3_settings +{ + /*! Power mode which user wants to set */ + uint8_t op_mode; + + /*! Enable/Disable pressure sensor */ + uint8_t press_en; + + /*! Enable/Disable temperature sensor */ + uint8_t temp_en; + + /*! ODR and filter configuration */ + struct bmp3_odr_filter_settings odr_filter; + + /*! Interrupt configuration */ + struct bmp3_int_ctrl_settings int_settings; + + /*! Advance settings */ + struct bmp3_adv_settings adv_settings; +}; + +/*! + * @brief bmp3 fifo frame + */ +struct bmp3_fifo_data +{ + /*! Data buffer of user defined length is to be mapped here + * 512 + 4 */ + uint8_t *buffer; + + /*! Number of bytes of data read from the fifo */ + uint16_t byte_count; + + /*! Number of frames to be read as specified by the user */ + uint8_t req_frames; + + /*! Will be equal to length when no more frames are there to parse */ + uint16_t start_idx; + + /*! Will contain the no of parsed data frames from fifo */ + uint8_t parsed_frames; + + /*! Configuration error */ + uint8_t config_err; + + /*! Sensor time */ + uint32_t sensor_time; + + /*! FIFO input configuration change */ + uint8_t config_change; + + /*! All available frames are parsed */ + uint8_t frame_not_available; +}; + +/*! + * @brief bmp3 fifo configuration + */ +struct bmp3_fifo_settings +{ + /*! enable/disable */ + uint8_t mode; + + /*! stop on full enable/disable */ + uint8_t stop_on_full_en; + + /*! time enable/disable */ + uint8_t time_en; + + /*! pressure enable/disable */ + uint8_t press_en; + + /*! temperature enable/disable */ + uint8_t temp_en; + + /*! down sampling rate */ + uint8_t down_sampling; + + /*! filter enable/disable */ + uint8_t filter_en; + + /*! FIFO watermark enable/disable */ + uint8_t fwtm_en; + + /*! FIFO full enable/disable */ + uint8_t ffull_en; +}; + +#ifdef BMP3_FLOAT_COMPENSATION + +/*! + * @brief Quantized Trim Variables + */ +struct bmp3_quantized_calib_data +{ + /*! Quantized Trim Variables */ + + double par_t1; + double par_t2; + double par_t3; + double par_p1; + double par_p2; + double par_p3; + double par_p4; + double par_p5; + double par_p6; + double par_p7; + double par_p8; + double par_p9; + double par_p10; + double par_p11; + double t_lin; +}; + +/*! + * @brief Calibration data + */ +struct bmp3_calib_data +{ + /*! Quantized data */ + struct bmp3_quantized_calib_data quantized_calib_data; + + /*! Register data */ + struct bmp3_reg_calib_data reg_calib_data; +}; + +/*! + * @brief bmp3 sensor structure which comprises of temperature and pressure + * data. + */ +struct bmp3_data +{ + /*! Compensated temperature */ + double temperature; + + /*! Compensated pressure */ + double pressure; +}; + +#else + +/*! + * @brief bmp3 sensor structure which comprises of temperature and pressure + * data. + */ +struct bmp3_data +{ + /*! Compensated temperature */ + int64_t temperature; + + /*! Compensated pressure */ + uint64_t pressure; +}; + +/*! + * @brief Calibration data + */ +struct bmp3_calib_data +{ + /*! Register data */ + struct bmp3_reg_calib_data reg_calib_data; +}; + +#endif /* BMP3_FLOAT_COMPENSATION */ + +/*! + * @brief bmp3 sensor structure which comprises of un-compensated temperature + * and pressure data. + */ +struct bmp3_uncomp_data +{ + /*! un-compensated pressure */ + uint64_t pressure; + + /*! un-compensated temperature */ + int64_t temperature; +}; + +/*! + * @brief bmp3 device structure + */ +struct bmp3_dev +{ + /*! Chip Id */ + uint8_t chip_id; + + /*! + * The interface pointer is used to enable the user + * to link their interface descriptors for reference during the + * implementation of the read and write interfaces to the + * hardware. + */ + void *intf_ptr; + + /*! Interface Selection + * For SPI, interface = BMP3_SPI_INTF + * For I2C, interface = BMP3_I2C_INTF + **/ + enum bmp3_intf intf; + + /*! To store interface pointer error */ + BMP3_INTF_RET_TYPE intf_rslt; + + /*! Decide SPI or I2C read mechanism */ + uint8_t dummy_byte; + + /*! Read function pointer */ + bmp3_read_fptr_t read; + + /*! Write function pointer */ + bmp3_write_fptr_t write; + + /*! Delay function pointer */ + bmp3_delay_us_fptr_t delay_us; + + /*! Trim data */ + struct bmp3_calib_data calib_data; +}; + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMP3_DEFS_H_ */ diff --git a/src/madflight/bat/BatteryADC.h b/src/madflight/bat/BatteryADC.h index 53293cb..22a6d61 100644 --- a/src/madflight/bat/BatteryADC.h +++ b/src/madflight/bat/BatteryADC.h @@ -4,8 +4,8 @@ ADC Battery Monitor Need to have at least HW_PIN_BAT_V or HW_PIN_BAT_I defined before including this header Also needs cfg for: -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 ) -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 ) +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 ) +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 ) =================================================================================================*/ @@ -46,10 +46,10 @@ class BatteryADC: public Battery { float dt_h = dt / 3600e6; ts = now; #ifdef HW_PIN_BAT_V - v = cfg.bat_cal_v * analogRead(HW_PIN_BAT_V); + v = cfg.BAT_CAL_V * analogRead(HW_PIN_BAT_V); #endif #ifdef HW_PIN_BAT_I - i = cfg.bat_cal_v * analogRead(HW_PIN_BAT_I); + i = cfg.BAT_CAL_V * analogRead(HW_PIN_BAT_I); #endif w = v * i; mah += i * dt_h * 1000; diff --git a/src/madflight/bat/bat.h b/src/madflight/bat/bat.h index 26fac3f..73627fc 100644 --- a/src/madflight/bat/bat.h +++ b/src/madflight/bat/bat.h @@ -60,7 +60,7 @@ INA226 bat_ina226; class BatteryINA226: public Battery { public: void setup() { - float Rshunt = cfg.bat_cal_i; //ohm + float Rshunt = cfg.BAT_CAL_I; //ohm float iMaxExpected = 0.080 / Rshunt; // ampere (max 80mv shunt voltage) // Default INA226 address is 0x40 diff --git a/src/madflight/bb/bb_sdcard/bb_sdcard.h b/src/madflight/bb/bb_sdcard/bb_sdcard.h index c47097c..168010a 100644 --- a/src/madflight/bb/bb_sdcard/bb_sdcard.h +++ b/src/madflight/bb/bb_sdcard/bb_sdcard.h @@ -36,7 +36,7 @@ SOFTWARE. //#define FREERTOS_DEFAULT_STACK_SIZE - defined in hw_xxx.h #define LOG_TYPE_LEN 64 // max number of message types -#define QUEUE_LENGTH 50 // max number of messages in the queue +#define QUEUE_LENGTH 100 // max number of messages in the queue #define MAX_MSG_LEN 89 // max message len is FMT with 89 (0x59) bytes #define HEAD_BYTE1 0xA3 @@ -233,6 +233,8 @@ class BinLog { msg_end(); } + uint8_t keepFree = 5; //number of queue spots to keep free when sending messages + private: uint8_t buf[128]; uint8_t buflen = 0; @@ -354,7 +356,7 @@ class BinLog { } //queue message - queueSend(buf,buflen); + queueSend(buf,buflen, keepFree); //keep a few queue spots free for more important messages (such as FMT) inhibit = true; } @@ -490,10 +492,11 @@ class BinLog { enum State_t { READY, STARTING, STARTED }; static State_t state; static uint32_t startMicros; + static uint32_t missCnt; static void setup() { queue = xQueueCreateStatic(QUEUE_LENGTH, sizeof(msg_t), ucQueueStorageArea, &xStaticQueue); - if(xTaskCreate(bb_task, "BB", FREERTOS_DEFAULT_STACK_SIZE, NULL, uxTaskPriorityGet(NULL)+1, &xHandle) != pdPASS ){ + if(xTaskCreate(bb_task, "BB", FREERTOS_DEFAULT_STACK_SIZE, NULL, uxTaskPriorityGet(NULL), &xHandle) != pdPASS ){ Serial.println("BB: Task creation failed"); } } @@ -514,6 +517,7 @@ class BinLog { //log file time start now startMicros = micros(); + missCnt = 0; //write headers (flush often) FMT_sendFMT(); //send FMT for FMT @@ -537,9 +541,9 @@ class BinLog { //write parameters (plot.ardupilot.org needs at least one) String name; - float value; + float value = 0; int i = 0; - while(cfg.getNameValue(i,&name,&value)) { + while(cfg.getNameAndValue(i,&name,&value)) { log_header_parm(name.c_str(), value, 0); queueFlush(); i++; @@ -562,14 +566,14 @@ class BinLog { public: static void stop() { command = STOP; - //send zero length message to wake up BB task + //send zero length message to wake up BB task to process command uint8_t msg = 0; xQueueSend(queue, (void*)&msg, 0); } static void start() { command = START; - //send zero length message to wake up BB task + //send zero length message to wake up BB task to process command uint8_t msg = 0; xQueueSend(queue, (void*)&msg, 0); } @@ -589,10 +593,13 @@ class BinLog { //append message to queue - static bool queueSend(uint8_t *buf, uint8_t len) { + static bool queueSend(uint8_t *buf, uint8_t len, uint8_t keepfree = 0) { + if(keepfree && uxQueueSpacesAvailable(queue) < keepfree) return false; //NOTE: random data will pad the message - improve this? buf[0] = len; //overwrite header1 with message length - return ( xQueueSend(queue, (void*)buf, 0) == pdPASS ); + bool ok = ( xQueueSend(queue, (void*)buf, 0) == pdPASS ); + if(!ok) missCnt++; + return ok; } private: @@ -766,6 +773,7 @@ StaticQueue_t BinLog::xStaticQueue = {}; uint8_t BinLog::ucQueueStorageArea[QUEUE_LENGTH * sizeof(msg_t)] = {}; BinLog::State_t BinLog::state = READY; BinLog::Command_t BinLog::command = NONE; +uint32_t BinLog::missCnt = 0; @@ -876,7 +884,7 @@ class BlackBox_SD : public BlackBox { void log_att() override { BinLog bl("ATT"); - bl.TimeUS(millis()); + bl.TimeUS(); bl.i16("DesRoll",0, 1e-2, "deg"); bl.i16("Roll",ahrs.roll*100, 1e-2, "deg"); bl.i16("DesPitch",0, 1e-2, "deg"); @@ -890,25 +898,26 @@ class BlackBox_SD : public BlackBox { //raw (unfiltered but corrected) IMU data void log_imu() override { - BinLog bl("IMU"); + BinLog bl("IMU"); + bl.keepFree = QUEUE_LENGTH/4; //keep 25% of queue free for other messages bl.TimeUS(imu.ts); - bl.i16("ax",(imu.ax - cfg.imu_cal_ax)*100, 1e-2, "G"); //G - bl.i16("ay",(imu.ay - cfg.imu_cal_ay)*100, 1e-2, "G"); //G - bl.i16("az",(imu.az - cfg.imu_cal_az)*100, 1e-2, "G"); //G - bl.i16("gx",(imu.gx - cfg.imu_cal_gx)*10, 1e-1, "deg/s"); //dps - bl.i16("gy",(imu.gy - cfg.imu_cal_gy)*10, 1e-1, "deg/s"); //dps - bl.i16("gz",(imu.gz - cfg.imu_cal_gz)*10, 1e-1, "deg/s"); //dps + bl.i16("ax",(imu.ax - cfg.IMU_CAL_AX)*100, 1e-2, "G"); //G + bl.i16("ay",(imu.ay - cfg.IMU_CAL_AY)*100, 1e-2, "G"); //G + bl.i16("az",(imu.az - cfg.IMU_CAL_AZ)*100, 1e-2, "G"); //G + bl.i16("gx",(imu.gx - cfg.IMU_CAL_GX)*10, 1e-1, "deg/s"); //dps + bl.i16("gy",(imu.gy - cfg.IMU_CAL_GY)*10, 1e-1, "deg/s"); //dps + bl.i16("gz",(imu.gz - cfg.IMU_CAL_GZ)*10, 1e-1, "deg/s"); //dps #if MAG_USE != MAG_USE_NONE //get from magnetometer - bl.i16("mx",((mag.x - cfg.mag_cal_x) * cfg.mag_cal_sx)*100, 1e-2, "uT"); //uT - bl.i16("my",((mag.y - cfg.mag_cal_y) * cfg.mag_cal_sy)*100, 1e-2, "uT"); //uT - bl.i16("mz",((mag.z - cfg.mag_cal_z) * cfg.mag_cal_sz)*100, 1e-2, "uT"); //uT + bl.i16("mx",((mag.x - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX)*100, 1e-2, "uT"); //uT + bl.i16("my",((mag.y - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY)*100, 1e-2, "uT"); //uT + bl.i16("mz",((mag.z - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ)*100, 1e-2, "uT"); //uT #else //get from imu if(imu.hasMag()) { - bl.i16("mx",((imu.mx - cfg.mag_cal_x) * cfg.mag_cal_sx)*100, 1e-2, "uT"); //uT - bl.i16("my",((imu.my - cfg.mag_cal_y) * cfg.mag_cal_sy)*100, 1e-2, "uT"); //uT - bl.i16("mz",((imu.mz - cfg.mag_cal_z) * cfg.mag_cal_sz)*100, 1e-2, "uT"); //uT + bl.i16("mx",((imu.mx - cfg.MAG_CAL_X) * cfg.MAG_CAL_SX)*100, 1e-2, "uT"); //uT + bl.i16("my",((imu.my - cfg.MAG_CAL_Y) * cfg.MAG_CAL_SY)*100, 1e-2, "uT"); //uT + bl.i16("mz",((imu.mz - cfg.MAG_CAL_Z) * cfg.MAG_CAL_SZ)*100, 1e-2, "uT"); //uT } #endif bl.i16("roll",ahrs.roll*100, 1e-2, "deg"); //deg -180 to 180 @@ -933,6 +942,15 @@ class BlackBox_SD : public BlackBox { BinLog::log_parm(name, value, default_value); } + //system status + void log_sys() override { + BinLog bl("SYS"); + bl.TimeUS(); + bl.u32("BBm",BinLog::missCnt); + bl.u32("IMi",imu.interrupt_cnt); + bl.i32("IMm",imu.interrupt_cnt - imu.update_cnt); + } + //------------------------------- // Blackbox interface //------------------------------- diff --git a/src/madflight/cfg/cfg.h b/src/madflight/cfg/cfg.h index 5a8ae8c..5cd3cc8 100644 --- a/src/madflight/cfg/cfg.h +++ b/src/madflight/cfg/cfg.h @@ -1,22 +1,108 @@ +/*========================================================================================== +cfg.h - madflight configuration from eeprom/flash + +MIT License + +Copyright (c) 2023-2024 https://madflight.com + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +===========================================================================================*/ + + //this header needs hw_eeprom_begin(), hw_eeprom_read(int adr), hw_eeprom_write(int adr, uint8_t val), and hw_eeprom_commit() -const String _cfg_names[] = { - "imu_cal_ax", - "imu_cal_ay", - "imu_cal_az", - "imu_cal_gx", - "imu_cal_gy", - "imu_cal_gz", - - "mag_cal_x", - "mag_cal_y", - "mag_cal_z", - "mag_cal_sx", - "mag_cal_sy", - "mag_cal_sz", - - "bat_cal_v", - "bat_cal_i", +#pragma once + +/*strange.... + +on RP2040: + +const char* _cfg_names[] +Sketch uses 158796 bytes (7%) of program storage space. Maximum is 2093056 bytes. +Global variables use 35972 bytes (13%) of dynamic memory, leaving 226172 bytes for local variables. Maximum is 262144 bytes. + +static const char* _cfg_names[] +Sketch uses 159004 bytes (7%) of program storage space. Maximum is 2093056 bytes. +Global variables use 35764 bytes (13%) of dynamic memory, leaving 226380 bytes for local variables. Maximum is 262144 bytes. + +-->static has +208 flash, -208 ram, 208=52*4 for 53 entries + +on ESP32S3: + +const char* _cfg_names[] +Sketch uses 398221 bytes (30%) of program storage space. Maximum is 1310720 bytes. +Global variables use 22000 bytes (6%) of dynamic memory, leaving 305680 bytes for local variables. Maximum is 327680 bytes. + +static const char* _cfg_names[] +Sketch uses 398221 bytes (30%) of program storage space. Maximum is 1310720 bytes. +Global variables use 21792 bytes (6%) of dynamic memory, leaving 305888 bytes for local variables. Maximum is 327680 bytes. + +-->static has same flash, -208 ram +*/ + + +//list of parameter names +static const char* _cfg_names[] = { + "IMU_CAL_AX", //accel X zero offset calibration + "IMU_CAL_AY", //accel Y zero offset calibration + "IMU_CAL_AZ", //accel Z zero offset calibration + "IMU_CAL_GX", //gyro X zero offset calibration + "IMU_CAL_GY", //gyro Y zero offset calibration + "IMU_CAL_GZ", //gyro Z zero offset calibration + + "MAG_CAL_X", //magnetometer X zero offset calibration + "MAG_CAL_Y", //magnetometer Y zero offset calibration + "MAG_CAL_Z", //magnetometer Z zero offset calibration + "MAG_CAL_SX", //magnetometer X scale calibration + "MAG_CAL_SY", //magnetometer Y scale calibration + "MAG_CAL_SZ", //magnetometer Z scale calibration + + "BAT_CAL_V", //battery ADC voltage scale calibration, value is actual_voltage_in_V / adc_reading + "BAT_CAL_I", //battery ADC current scale calibration, value is actual_current_in_A / adc_reading, INA226: Rshunt value in Ohm + + "RCIN_THR_CH", //1-based channel number + "RCIN_THR_PULL", //pmw when stick pulled toward you + "RCIN_THR_MID", //pmw mid stick + "RCIN_THR_PUSH", //pwm when stick pushed away from you + + "RCIN_ROL_CH", + "RCIN_ROL_LEFT", //pmw when stick left + "RCIN_ROL_MID", + "RCIN_ROL_RIGHT",//pmw when stick right + + "RCIN_PIT_CH", + "RCIN_PIT_PULL", + "RCIN_PIT_MID", + "RCIN_PIT_PUSH", + + "RCIN_YAW_CH", + "RCIN_YAW_LEFT", + "RCIN_YAW_MID", + "RCIN_YAW_RIGHT", + + "RCIN_ARM_CH", + "RCIN_ARM_MIN", //armed pwm range min + "RCIN_ARM_MAX", //armed pwm range max + + "RCIN_FLT_CH", + "RCIN_FLT_MIN", //6-pos switch lowest pwm (flight mode 0) + "RCIN_FLT_MAX", //6-pos switch lowest pwm (flight mode 5) }; #define CFG_LEN_OFFSET 6 //offset in bytes to _len @@ -34,22 +120,56 @@ class Config { uint16_t _crc = 0; //crc starting from _len uint16_t _len = 0; //sizeof(Config) public: - float imu_cal_ax = 0; //accel X zero offset calibration - float imu_cal_ay = 0; //accel Y zero offset calibration - float imu_cal_az = 0; //accel Z zero offset calibration - float imu_cal_gx = 0; //gyro X zero offset calibration - float imu_cal_gy = 0; //gyro Y zero offset calibration - float imu_cal_gz = 0; //gyro Z zero offset calibration + float IMU_CAL_AX = 0; //accel X zero offset calibration + float IMU_CAL_AY = 0; //accel Y zero offset calibration + float IMU_CAL_AZ = 0; //accel Z zero offset calibration + float IMU_CAL_GX = 0; //gyro X zero offset calibration + float IMU_CAL_GY = 0; //gyro Y zero offset calibration + float IMU_CAL_GZ = 0; //gyro Z zero offset calibration - float mag_cal_x = 0; //magnetometer X zero offset calibration - float mag_cal_y = 0; //magnetometer Y zero offset calibration - float mag_cal_z = 0; //magnetometer Z zero offset calibration - float mag_cal_sx = 1; //magnetometer X scale calibration - float mag_cal_sy = 1; //magnetometer Y scale calibration - float mag_cal_sz = 1; //magnetometer Z scale calibration + float MAG_CAL_X = 0; //magnetometer X zero offset calibration + float MAG_CAL_Y = 0; //magnetometer Y zero offset calibration + float MAG_CAL_Z = 0; //magnetometer Z zero offset calibration + float MAG_CAL_SX = 1; //magnetometer X scale calibration + float MAG_CAL_SY = 1; //magnetometer Y scale calibration + float MAG_CAL_SZ = 1; //magnetometer Z scale calibration - float bat_cal_v = 1; //battery ADC voltage scale calibration, value is actual_voltage_in_V / adc_reading - float bat_cal_i = 1; //battery ADC current scale calibration, value is actual_current_in_A / adc_reading, INA226: Rshunt value in Ohm + float BAT_CAL_V = 1; //battery ADC voltage scale calibration, value is actual_voltage_in_V / adc_reading + float BAT_CAL_I = 1; //battery ADC current scale calibration, value is actual_current_in_A / adc_reading, INA226: Rshunt value in Ohm + + float RCIN_THR_CH = 1; //1-based channel number + float RCIN_THR_PULL = 1100; + float RCIN_THR_MID = 1500; + float RCIN_THR_PUSH = 1900; + + float RCIN_ROL_CH = 2; + float RCIN_ROL_LEFT = 1100; + float RCIN_ROL_MID = 1500; + float RCIN_ROL_RIGHT = 1900; + + float RCIN_PIT_CH = 3; + float RCIN_PIT_PULL = 1100; + float RCIN_PIT_MID = 1500; + float RCIN_PIT_PUSH = 1900; + + float RCIN_YAW_CH = 4; + float RCIN_YAW_LEFT = 1100; + float RCIN_YAW_MID = 1500; + float RCIN_YAW_RIGHT = 1900; + + float RCIN_ARM_CH = 5; + float RCIN_ARM_MIN = 1600; //armed pwm range min + float RCIN_ARM_MAX = 2500; //armed pwm range max + + //flightmode 6 position switch - Ardupilot switch pwm: 1165,1295,1425,1555,1685,1815 (spacing 130) + //EdgeTx 3-pos SA + 2-pos SB setup: + // Source:SA Weight:52 Offset:0 + Source:SB Weight:13 Offset:-1 Multiplex: add + //-OR- Source:SA Weight:26 Offset:-40 Switch:SBdown + Source:SA Weight:26 Offset:36 Switch:SBup Multiplex:Replace + + float RCIN_FLT_CH = 6; + float RCIN_FLT_MIN = 1165; //6-pos switch lowest pwm (flight mode 0) + float RCIN_FLT_MAX = 1815; //6-pos switch lowest pwm (flight mode 5) + Config() { _len = sizeof(Config); @@ -65,37 +185,54 @@ class Config { return (sizeof(Config) - CFG_PARAM_OFFSET) / sizeof(float); } + union value_t { + float f; + uint32_t u; + int32_t i; + }; + //get parameter name and value for index - bool getNameValue(uint16_t index, String* name, float* value) { + bool getNameAndValue(uint16_t index, String* name, float* value) { if(index>=valueCount()) return false; *name = _cfg_names[index]; - *value = (&imu_cal_ax)[index]; + *value = (&IMU_CAL_AX)[index]; return true; } - //print all config values +private: + void printValue(int i) { + const char *name = _cfg_names[i]; + float v = (&IMU_CAL_AX)[i]; + int vi = (int)v; + if(v==vi) { + Serial.printf("set %s %d\n", name, vi); + }else{ + Serial.printf("set %s %f\n", name, v); + } + } + +public: + //CLI print all config values void list() { - String name; - float value; - uint16_t i = 0; - while(getNameValue(i,&name,&value)) { - Serial.printf("set %s %f\n", name.c_str(), value); - i++; + for(int i=0;iimu_cal_ax)); + + //CLI set a parameter value + void set(String namestr, String val) { + namestr.toUpperCase(); + const char *name = namestr.c_str(); + float *fvalues = ((float*)&(this->IMU_CAL_AX)); for(uint16_t i=0;ipin = pin; - this->led_on_value = led_on_value; + void setup() { + this->pin = HW_PIN_LED; + this->led_on_value = HW_LED_ON; if(pin<0) return; pinMode(pin, OUTPUT); enabled = true; @@ -75,4 +96,7 @@ class LedSingle : public Led { LedSingle led_instance; +//================================================================================================= +#endif + Led &led = led_instance; \ No newline at end of file diff --git a/src/madflight/out/out.h b/src/madflight/out/out.h new file mode 100644 index 0000000..be145de --- /dev/null +++ b/src/madflight/out/out.h @@ -0,0 +1,59 @@ +/*========================================================================================== +out.h - madflight motor and servo output driver + +MIT License + +Copyright (c) 2023-2024 https://madflight.com + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +===========================================================================================*/ + +#include "../interface.h" + +void Out::setup() { +} + +bool Out::_setupOutput(char typ, uint8_t i, int pin, int freq_hz, int pwm_min_us, int pwm_max_us){ + if(i >= HW_OUT_COUNT) return false; + type[i] = typ; + pwm[i].begin(pin, freq_hz, pwm_min_us, pwm_max_us); + command[i] = 0; + pwm[i].writeFactor(command[i]); + return true; +} + +bool Out::setupMotor(uint8_t i, int pin, int freq_hz, int pwm_min_us, int pwm_max_us) { + return _setupOutput('M', i, pin, freq_hz, pwm_min_us, pwm_max_us); +} +bool Out::setupServo(uint8_t i, int pin, int freq_hz, int pwm_min_us, int pwm_max_us) { + return _setupOutput('S', i, pin, freq_hz, pwm_min_us, pwm_max_us); +} + +void Out::set(uint8_t i, float value) { + if(i >= HW_OUT_COUNT) return; + command[i] = value; + if(armed) { + pwm[i].writeFactor(value); + }else{ + if(type[i] == 'M') pwm[i].writeFactor(0); + } +} + +Out out; + diff --git a/src/madflight/rcin/rcin.h b/src/madflight/rcin/rcin.h index 49d2837..f06b974 100644 --- a/src/madflight/rcin/rcin.h +++ b/src/madflight/rcin/rcin.h @@ -1,50 +1,210 @@ -/*================================================================================================= -Each RCIN_USE_xxx section in this file defines: -rcin_Setup() -> init -rcin_GetPWM(int *pwm) -> fills pwm[0..RCIN_NUM_CHANNELS-1] received PWM values, returns true if new data was received - -=================================================================================================*/ - -//#define RCIN_USE_NONE 0 //always need a radio -#define RCIN_USE_CRSF 1 -#define RCIN_USE_SBUS 2 -#define RCIN_USE_DSM 3 -#define RCIN_USE_PPM 4 -#define RCIN_USE_PWM 5 -#define RCIN_USE_DEBUG 6 +/*========================================================================================== +rcin.h - madflight RC radio receiver + +MIT License + +Copyright (c) 2023-2024 https://madflight.com + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +===========================================================================================*/ + +#pragma once + +#include "../interface.h" //RCIN interface definition +#include "../cfg/cfg.h" +#include "rcin_calibrate.h" + +#ifndef RCIN_NUM_CHANNELS + #define RCIN_NUM_CHANNELS 8 //number of receiver channels (minimal 6) +#endif + +#ifndef RCIN_TIMEOUT + #define RCIN_TIMEOUT 3000 // lost connection timeout in milliseconds +#endif + +#ifndef RCIN_STICK_DEADBAND + #define RCIN_STICK_DEADBAND 0 //pwm deadband around stick center +#endif +#ifndef RCIN_THROTTLE_DEADBAND + #define RCIN_THROTTLE_DEADBAND 60 //pwm deadband for zero throttle +#endif -#include "../interface.h" +#define RCIN_USE_NONE 0 +#define RCIN_USE_CRSF 1 +#define RCIN_USE_SBUS 2 +#define RCIN_USE_DSM 3 +#define RCIN_USE_PPM 4 +#define RCIN_USE_PWM 5 +#define RCIN_USE_DEBUG 6 -/* INTERFACE -class Rcin { +//Rcin implements public interface, and is base for specific rcin radio classes +class Rcin : public Rcin_interface { public: - uint16_t *pwm; //pwm channel data. values: 988-2012 - virtual void setup() = 0; - bool update() { //returns true if channel pwm data was updated - bool rv = _update(); - if(rv) { - update_time = millis(); - } - return rv; - } - bool connected() { - return ((uint32_t)millis() - update_time <= (RCIN_TIMEOUT) ); - } + void setup() override; + bool update() override; //returns true if channel pwm data was updated + bool connected() override; + void calibrate() override; //interactive calibration + private: + virtual bool _update() = 0; //update the specific radio + virtual void _setup() = 0; //setup the specific radio + float _ChannelNormalize(int val, int min, int center, int max, int deadband); + void _setupStick(int stickno, int ch, int left_pull, int mid, int right_push); + uint32_t update_time = 0; - virtual bool _update() = 0; //returns true if channel pwm data was updated + + enum stick_enum {THR,ROL,PIT,YAW,ARM,FLT}; + + struct stick_t{ + uint8_t ch; //stick/switch channel - 0 based + uint16_t min; //stick/switch min pwm + uint16_t mid; //stick center pwm + uint16_t max; //stick/switch max pwm + int8_t inv; //stick inverted: -1 inverted, 1 normal + } st[6]; + + uint16_t st_flt_spacing; + uint16_t dummy_pwm = 0; }; -extern Rcin &rcin; -*/ +void Rcin::_setupStick(int stickno, int ch, int left_pull, int mid, int right_push) { + st[stickno].ch = ch-1; + st[stickno].mid = mid; + if(left_pull < right_push) { + st[stickno].min = left_pull; + st[stickno].max = right_push; + st[stickno].inv = 1; + }else{ + st[stickno].min = right_push; + st[stickno].max = left_pull; + st[stickno].inv = -1; + } +} +void Rcin::setup() { + throttle = 0; + roll = 0; + pitch = 0; + yaw = 0; + vspeed = 0; + arm = false; + + //setup stick/switch parameters from config values + Serial.printf("RCIN: setup channels thr=%d rol=%d pit=%d yaw=%d arm=%d flt=%d\n", (int)cfg.RCIN_THR_CH, (int)cfg.RCIN_ROL_CH, (int)cfg.RCIN_PIT_CH, (int)cfg.RCIN_YAW_CH, (int)cfg.RCIN_ARM_CH, (int)cfg.RCIN_FLT_CH); + _setupStick(THR, cfg.RCIN_THR_CH, cfg.RCIN_THR_PULL, cfg.RCIN_THR_MID, cfg.RCIN_THR_PUSH); + _setupStick(ROL, cfg.RCIN_ROL_CH, cfg.RCIN_ROL_LEFT, cfg.RCIN_ROL_MID, cfg.RCIN_ROL_RIGHT); + _setupStick(PIT, cfg.RCIN_PIT_CH, cfg.RCIN_PIT_PULL, cfg.RCIN_PIT_MID, cfg.RCIN_PIT_PUSH); + _setupStick(YAW, cfg.RCIN_YAW_CH, cfg.RCIN_YAW_LEFT, cfg.RCIN_YAW_MID, cfg.RCIN_YAW_RIGHT); + st[ARM].ch = cfg.RCIN_ARM_CH-1; + st[ARM].min = cfg.RCIN_ARM_MIN; + st[ARM].mid = 0; //NOT USED + st[ARM].max = cfg.RCIN_ARM_MAX; + st[ARM].inv = 0; //NOT USED + st[FLT].ch = cfg.RCIN_FLT_CH-1; + st[FLT].min = cfg.RCIN_FLT_MIN; + st[FLT].mid = 0; //NOT USED + st[FLT].max = 0; //NOT USED + st[FLT].inv = 0; //NOT USED + st_flt_spacing = (cfg.RCIN_FLT_MAX - cfg.RCIN_FLT_MIN) / 5; + + //check st parameters + for(int i=0;i<6;i++) { + if(st[i].ch >= RCIN_NUM_CHANNELS) { + Serial.println("RCIN: invalid channel in config, re-calibrate radio"); + st[i].ch = 0; + st[i].min = 1; + st[i].mid = 2; + st[i].max = 3; + st[i].inv = 1; + } + } + + //setup specific radio + _setup(); +} + +bool Rcin::update() { //returns true if channel pwm data was updated + bool rv = _update(); + if(rv) { + //throttle: 0.0 in range from stick full back to rcin_cfg_thro_low, 1.0 on full throttle + float tlow = st[THR].min + RCIN_THROTTLE_DEADBAND; + throttle = constrain( ((float)(pwm[st[THR].ch] - tlow)) / (st[THR].max - tlow), 0.0, 1.0); + + //roll,pitch,yaw + vspeed = st[THR].inv * _ChannelNormalize(pwm[st[THR].ch], st[THR].min, st[THR].mid, st[THR].max, RCIN_STICK_DEADBAND); // output: -1 (descent, st back) to 1 (ascent, st forward) + roll = st[ROL].inv * _ChannelNormalize(pwm[st[ROL].ch], st[ROL].min, st[ROL].mid, st[ROL].max, RCIN_STICK_DEADBAND); // output: -1 (roll left, st left) to 1 (roll right, st right) + pitch = -st[PIT].inv * _ChannelNormalize(pwm[st[PIT].ch], st[PIT].min, st[PIT].mid, st[PIT].max, RCIN_STICK_DEADBAND); // output: -1 (pitch down, st back) to 1 (pitch up, st forward) + yaw = st[YAW].inv * _ChannelNormalize(pwm[st[YAW].ch], st[YAW].min, st[YAW].mid, st[YAW].max, RCIN_STICK_DEADBAND); // output: -1 (yaw left, st left) to 1 (yaw right, st right) + + //arm switch + arm = (st[ARM].min <= pwm[st[ARM].ch] && pwm[st[ARM].ch] < st[ARM].max); + + //flightmode 6 position switch + flightmode = constrain( ( pwm[st[FLT].ch] - st[FLT].min + st_flt_spacing/2) / st_flt_spacing, 0, 5); //output 0..5 + + //set update timestamp + update_time = millis(); + } + return rv; +} + +bool Rcin::connected() { + return ((uint32_t)millis() - update_time <= (RCIN_TIMEOUT) ); +} + +//helper to nomalize a channel based on min,center,max calibration +float Rcin::_ChannelNormalize(int val, int min, int center, int max, int deadband) { + int rev = 1; //1=normal, -1=reverse channel + //needs: min < center < max + if(valbegin(CRSF_BAUD); } - bool _update() { + bool _update() override { bool rv = false; while(rcin_Serial->available()) { int c = rcin_Serial->read(); @@ -95,13 +255,13 @@ bool sbusLostFrame; class RcinSBUS : public Rcin { public: - void setup() { + void _setup() override { Serial.println("RCIN_USE_SBUS"); pwm = pwm_instance; sbus.begin(); } - bool _update() { + bool _update() override { if (sbus.read(sbusChannels, &sbusFailSafe, &sbusLostFrame)) { //sBus scaling below is for Taranis-Plus and X4R-SB @@ -141,13 +301,13 @@ void serialEvent3(void) class RcinDSM : public Rcin { public: - void setup() { + void _setup() override { Serial.println("RCIN_USE_DSM"); pwm = pwm_instance; Serial3.begin(115000); } - bool _update() { + bool _update() override { if (DSM.timedOut(micros())) { //Serial.println("*** DSM RX TIMED OUT ***"); } @@ -172,6 +332,11 @@ RcinDSM rcin_instance; //PPM Receiver //================================================================================================= #elif RCIN_USE == RCIN_USE_PPM + +#if RCIN_NUM_CHANNELS > 6 + #error RCIN_USE_PPM has max 6 channels, change RCIN_NUM_CHANNELS +#endif + volatile uint32_t channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; volatile bool rcin_updated = false; @@ -222,7 +387,7 @@ void getPPM() { class RcinPPM : public Rcin { public: - void setup() { + void _setup() override { Serial.printf("RCIN_USE_PPM pin=%d\n",HW_PIN_RCIN_RX); pwm = pwm_instance; //Declare interrupt pin @@ -232,7 +397,7 @@ class RcinPPM : public Rcin { attachInterrupt(digitalPinToInterrupt(HW_PIN_RCIN_RX), getPPM, CHANGE); } - bool _update() { + bool _update() override { pwm[0] = channel_1_raw; pwm[1] = channel_2_raw; pwm[2] = channel_3_raw; @@ -254,6 +419,11 @@ RcinPPM rcin_instance; // PWM Receiver //================================================================================================= #elif RCIN_USE == RCIN_USE_PWM + +#if RCIN_NUM_CHANNELS > 6 + #error RCIN_USE_PWM has max 6 channels, change RCIN_NUM_CHANNELS +#endif + #warning "RCIN_USE_PWM not ported/tested - see src/rcin/rcin.h" //TODO uint32_t channel_1_raw, channel_2_raw, channel_3_raw, channel_4_raw, channel_5_raw, channel_6_raw; uint32_t rising_edge_start_1, rising_edge_start_2, rising_edge_start_3, rising_edge_start_4, rising_edge_start_5, rising_edge_start_6; @@ -323,7 +493,7 @@ void getCh6() { class RcinPWM : public Rcin { public: - void setup() { + void _setup() override { Serial.println("RCIN_USE_PWM"); pwm = pwm_instance; //Declare interrupt pins @@ -344,7 +514,7 @@ class RcinPWM : public Rcin { delay(20); } - bool _update() { + bool _update() override { pwm[0] = channel_1_raw; pwm[1] = channel_2_raw; pwm[2] = channel_3_raw; @@ -362,27 +532,6 @@ class RcinPWM : public Rcin { RcinPWM rcin_instance; -//================================================================================================= -// DEBUG Receiver -//================================================================================================= -#elif RCIN_USE == RCIN_USE_DEBUG -class RcinDebug : public Rcin { - public: - void setup() { - Serial.printf("RCIN_USE_DEBUG\n"); - pwm = pwm_instance; - }; - private: - bool _update() { //returns true if channel pwm data was updated - for(int i=0;iwrite(buf, len); -} - -void rcin_telemetry_flight_mode(const char *flight_mode) { - uint8_t buf[65]; - int len = CRSF_Telemetry::telemetry_flight_mode(buf, flight_mode); - rcin_Serial->write(buf, len); - //Serial.printf("\nFM(len=%d) ",len); - //for(int i=0;iwrite(buf, len); - //Serial.printf("\natt(len=%d) ",len); - //for(int i=0;iwrite(buf, len); -} - -//======================================================================================================================== -// OTHERS -//======================================================================================================================== -#else - - - -void rcin_telemetry_gps(int32_t latitude, int32_t longitude, uint16_t groundspeed, uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites) { -} - -void rcin_telemetry_flight_mode(const char *flight_mode) { -} - -void rcin_telemetry_attitude(float pitch, float roll, float yaw) { -} - -void rcin_telemetry_battery(float voltage_V, float current_A, int fuel_mAh, uint8_t remaining) { -} - -#endif +Rcin_interface &rcin = rcin_instance; diff --git a/src/madflight/rcin/rcin_calibrate.h b/src/madflight/rcin/rcin_calibrate.h new file mode 100644 index 0000000..2b8fbc6 --- /dev/null +++ b/src/madflight/rcin/rcin_calibrate.h @@ -0,0 +1,227 @@ + +//this class assumes rcin.update() is called in the background... (i.e. in imu_loop()) + +void rcin_cal_imu_loop() { + rcin.update(); +} + +class RcinCalibrate { + +public: + static void calibrate() { + imu.onUpdate = rcin_cal_imu_loop; + RcinCalibrate *cal = new RcinCalibrate(); + cal->_calibrate(); + } + + struct stick_t{ + uint8_t ch; //stick channel + uint16_t left_pull; //spwm at left/pull position + uint16_t mid; //pwm at center position + uint16_t right_push; //pwm at right/push position + uint16_t min; + uint16_t max; + }; + +private: + stick_t thr; //throttle stick + stick_t yaw; //yaw stick + stick_t pit; //pitch stick + stick_t rol; //roll stick + stick_t arm; //arm switch + stick_t flt; //flightmode switch + + bool ignore[RCIN_NUM_CHANNELS]={}; //channels to ignore + int16_t pc[RCIN_NUM_CHANNELS] = {0}; //temp value + int16_t arm_last; //temp value + +void prompt(const char *msg) { + Serial.println(msg); + + //empty buffer + while(Serial.available()) Serial.read(); + + //wait for enter + for(;;) { + if(Serial.available()) { + char c = Serial.read(); + if ( c=='\r' || c=='\n' ) return; + } + } +} + +void printpwm() { + for(int i=0;i 500); +} + +void armToggleWait() { + armToggleInit(); + for(;;) { + if(armToggled()) return; + taskYIELD(); + } +} + +void findStick(stick_t &stk) { + int16_t pmin[RCIN_NUM_CHANNELS]={}; + setVal(pmin,9999); + uint32_t tmin[RCIN_NUM_CHANNELS]={}; + int16_t pmax[RCIN_NUM_CHANNELS]={}; + uint32_t tmax[RCIN_NUM_CHANNELS]={}; + armToggleInit(); + + //for each channel: record pwm_min and pwm_max, and record timestamps + for(int c = 0; ; c++) { + if(ignore[c]) c++; //skip known channels + if(c>=RCIN_NUM_CHANNELS) c=0; + int16_t p = rcin.pwm[c]; + if(pmin[c] > p) {pmin[c] = p; tmin[c] = micros();} + if(pmax[c] < p) {pmax[c] = p; tmax[c] = micros();} + if(armToggled()) break; + taskYIELD(); + } + + //find channel with max difference between pwm_min and pwm_max + int16_t diffmax = 0; + int16_t diffc = 0; + for(int i=0;i= 0 ) { //use unsigned math... + stk.left_pull = pmin[stk.ch]; + stk.right_push = pmax[stk.ch]; + }else{ + stk.left_pull = pmax[stk.ch]; + stk.right_push = pmin[stk.ch]; + } + ignore[stk.ch] = true; //ignore this channel from now on +} + +protected: +void _calibrate() { + int c; + Serial.println("\n==== Radio Calibration ===="); + while(!rcin.connected()) { + Serial.println("Connect radio to continue..."); + delay(500); + } + + Serial.println("Switch to ARMED (if already armed, toggle until calibration done, then restart 'calradio' DISARMED)"); + setPc(); + for(c=0;;c++) { + if(c>=RCIN_NUM_CHANNELS) c=0; + if(abs((int16_t)rcin.pwm[c] - pc[c]) > 500) break; + taskYIELD(); + } + arm.ch = c; + int16_t arm_v1 = pc[c]; + int16_t arm_v2 = rcin.pwm[c]; + + int16_t arm_armed = arm_v2; + int16_t arm_disarmed = arm_v1; + if(abs(arm_disarmed - arm_armed) < 100) arm_disarmed = arm_v2; + if(arm_disarmed < arm_armed) { + arm.min = (arm_disarmed+arm_armed)/2; + arm.max = 2500; + }else{ + arm.min = 500; + arm.max = (arm_disarmed+arm_armed)/2; + } + Serial.printf("==> ARMED: ch=%d from=%d to=%d\n", arm.ch+1, arm.min, arm.max); + ignore[arm.ch]=true; + + Serial.println("THROTTLE: First pull throttle idle, then full throttle, then back to center, then toggle arm switch"); + findStick(thr); + Serial.printf("==> THROTTLE: ch=%d pull=%d mid=%d push=%d\n", rol.ch+1, rol.left_pull, rol.mid, rol.right_push); + + Serial.println("PITCH: First pull stick back, then push forward, then back to center, then toggle arm switch"); + findStick(pit); + Serial.printf("==> PITCH: ch=%d pull=%d mid=%d push=%d\n", pit.ch+1, pit.left_pull, pit.mid, pit.right_push); + + Serial.println("ROLL: First move stick left, then right, then back to center, then toggle arm switch"); + findStick(rol); + Serial.printf("==> ROLL: ch=%d left=%d mid=%d right=%d\n", rol.ch+1, rol.left_pull, rol.mid, rol.right_push); + + Serial.println("YAW: First move stick left, then right, then back to center, then toggle arm switch"); + findStick(yaw); + Serial.printf("==> YAW: ch=%d left=%d mid=%d right=%d\n", yaw.ch+1, yaw.left_pull, yaw.mid, yaw.right_push); + + Serial.println("Select min and max flight modes, then toggle arm switch"); + findStick(flt); + Serial.printf("==> FLIGHTMODE: ch=%d from=%d to=%d\n", flt.ch+1, flt.min, flt.max); + + //set config + cfg.RCIN_THR_CH = thr.ch+1; + cfg.RCIN_THR_PULL = thr.left_pull; + cfg.RCIN_THR_MID = thr.mid; + cfg.RCIN_THR_PUSH = thr.right_push; + + cfg.RCIN_ROL_CH = rol.ch+1; + cfg.RCIN_ROL_LEFT = rol.left_pull; + cfg.RCIN_ROL_MID = rol.mid; + cfg.RCIN_ROL_RIGHT = rol.right_push; + + cfg.RCIN_PIT_CH = pit.ch+1; + cfg.RCIN_PIT_PULL = pit.left_pull; + cfg.RCIN_PIT_MID = pit.mid; + cfg.RCIN_PIT_PUSH = pit.right_push; + + cfg.RCIN_YAW_CH = yaw.ch+1; + cfg.RCIN_YAW_LEFT = yaw.left_pull; + cfg.RCIN_YAW_MID = yaw.mid; + cfg.RCIN_YAW_RIGHT = yaw.right_push; + + cfg.RCIN_ARM_CH = arm.ch+1; + cfg.RCIN_ARM_MIN = arm.min; + cfg.RCIN_ARM_MAX = arm.max; + + cfg.RCIN_FLT_CH = flt.ch+1; + cfg.RCIN_FLT_MIN = flt.min; + cfg.RCIN_FLT_MAX = flt.max; + + rcin.setup(); //restart rcin to reload config + + Serial.printf("Radio calibration completed! Now use 'pradio' to check, 'cwrite' to save config, or reboot to restart with old config.\n"); +} + +}; diff --git a/src/madflight/rcin/telem.h b/src/madflight/rcin/telem.h new file mode 100644 index 0000000..4bf6770 --- /dev/null +++ b/src/madflight/rcin/telem.h @@ -0,0 +1,95 @@ +/*========================================================================================== +telem.h - madflight RC radio receiver telemetry + +MIT License + +Copyright (c) 2023-2024 https://madflight.com + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +===========================================================================================*/ + +#include "../interface.h" + +/* INTERFACE + +#define RCIN_USE_NONE 0 +#define RCIN_USE_CRSF 1 +#define RCIN_USE_SBUS 2 +#define RCIN_USE_DSM 3 +#define RCIN_USE_PPM 4 +#define RCIN_USE_PWM 5 +#define RCIN_USE_DEBUG 6 + +void rcin_telemetry_gps(int32_t latitude, int32_t longitude, uint16_t groundspeed, uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites); +void rcin_telemetry_flight_mode(const char *flight_mode); +void rcin_telemetry_attitude(float pitch, float roll, float yaw); +void rcin_telemetry_battery(float voltage_V, float current_A, int fuel_mAh, uint8_t remaining); +*/ +//======================================================================================================================== +// CRSF +//======================================================================================================================== +#if RCIN_USE == RCIN_USE_CRSF + +#include "crsf/crsf_telemetry.h" +void rcin_telemetry_gps(int32_t lat, int32_t lon, uint16_t sog_kmh, uint16_t cog_deg, uint16_t alt_m, uint8_t sats) { + uint8_t buf[65]; + int len = CRSF_Telemetry::telemetry_gps(buf, lat, lon, sog_kmh, cog_deg, alt_m, sats); + rcin_Serial->write(buf, len); +} + +void rcin_telemetry_flight_mode(const char *flight_mode) { + uint8_t buf[65]; + int len = CRSF_Telemetry::telemetry_flight_mode(buf, flight_mode); + rcin_Serial->write(buf, len); + //Serial.printf("\nFM(len=%d) ",len); + //for(int i=0;iwrite(buf, len); + //Serial.printf("\natt(len=%d) ",len); + //for(int i=0;iwrite(buf, len); +} + +//======================================================================================================================== +// OTHERS +//======================================================================================================================== +#else + +void rcin_telemetry_gps(int32_t latitude, int32_t longitude, uint16_t groundspeed, uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites) { +} + +void rcin_telemetry_flight_mode(const char *flight_mode) { +} + +void rcin_telemetry_attitude(float pitch, float roll, float yaw) { +} + +void rcin_telemetry_battery(float voltage_V, float current_A, int fuel_mAh, uint8_t remaining) { +} + +#endif diff --git a/src/madflight_board_custom_DYST-DYSF4PRO_V2.h b/src/madflight_board_custom_DYST-DYSF4PRO_V2.h index 00b2c5e..413d054 100644 --- a/src/madflight_board_custom_DYST-DYSF4PRO_V2.h +++ b/src/madflight_board_custom_DYST-DYSF4PRO_V2.h @@ -34,8 +34,8 @@ PB11 RX3 - TX3 PB10 GND - GND -set bat_cal_v 0.00057630 -set bat_cal_i 0.0011806 +set BAT_CAL_V 0.00057630 +set BAT_CAL_I 0.0011806 cwrite ==============================================================================*/