From 2a03120871b05b4d43cf9ebf4e72f56451b4e0ab Mon Sep 17 00:00:00 2001 From: YohanHadji Date: Fri, 25 Mar 2022 19:03:31 +0100 Subject: [PATCH] Adding new code! --- .../R2Home_OBC_V1.03.ino} | 335 ++++++++---------- .../R2Home_OBC_V1.04/main/barometer.hpp | 121 +++++++ .../R2Home_OBC_V1.04/main/battery.hpp | 23 ++ .../R2Home_OBC_V1.04/main/camera.hpp | 52 +++ .../R2Home_OBC_V1.04/main/config.h | 65 ++++ .../R2Home_OBC_V1.04/main/control.hpp | 18 + .../R2Home_OBC_V1.04/main/data.hpp | 163 +++++++++ .../R2Home_OBC_V1.04/main/debug.hpp | 13 + .../R2Home_OBC_V1.04/main/feedback.hpp | 199 +++++++++++ .../R2Home_OBC_V1.04/main/flight_state.hpp | 287 +++++++++++++++ .../R2Home_OBC_V1.04/main/gps.hpp | 175 +++++++++ .../R2Home_OBC_V1.04/main/main.ino | 97 +++++ .../R2Home_OBC_V1.04/main/navigation.hpp | 67 ++++ .../R2Home_OBC_V1.04/main/position.hpp | 130 +++++++ .../R2Home_OBC_V1.04/main/rc.hpp | 31 ++ .../R2Home_OBC_V1.04/main/servo.hpp | 188 ++++++++++ 16 files changed, 1771 insertions(+), 193 deletions(-) rename RLS_V1.0/R2Home_SOFTWARE_V1.01/{R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino => R2Home_OBC_V1.03/R2Home_OBC_V1.03.ino} (89%) create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/debug.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/navigation.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp create mode 100644 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/servo.hpp diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.03/R2Home_OBC_V1.03.ino similarity index 89% rename from RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino rename to RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.03/R2Home_OBC_V1.03.ino index 891fb29..1f0c737 100644 --- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.03/R2Home_OBC_V1.03.ino @@ -23,8 +23,8 @@ #define drop true // R2Home's version, drop or motorised #define record_home false // only record autopilot #define dep_alt 100 // m above ground -#define vup 3 // m/s -#define vdown -3 // m/s +#define vup 1 // m/s +#define vdown -1 // m/s #define gps_freq 5 // Hz #define nav_waypoint false // Doing the waypoint sequence before reaching home? #define nav_home true // Should it go home after the waypoint sequence? @@ -35,7 +35,8 @@ #define safe_trigger false // For HAB flight, will use a safer, but slower methode to detect apogee and transision to descent mode #define cog_brake false // Will reduce command if CoG is turning faster than a threshold -#define descent_timer 5 // Code will check for x times every second that the vertical speed is lower than threshold to trigger descent mode +//#define descent_timer 5 Code will check for x times every second that the vertical speed is lower than threshold to trigger descent mode +#define gliding_timer 3000 #define time_out 300 @@ -52,7 +53,6 @@ #define left_offset 100 #define right_offset 100 -#define gliding_timer 3000 #define waypoint_threshold 10 // Distance to waypoint before going to the next waypoint int dep_altitude = dep_alt; @@ -60,36 +60,29 @@ int cog_count = 2; double Setpoint, Input, Output; - // NAV PIDs // float NKp = 1; float NKi = 0.05; float NKd = 0.1; PID myPID(&Input, &Output, &Setpoint,NKp, NKi, NKd, DIRECT); - - -double long sim_cmd_time = 0; -float sim_cmd = 0; + // ----------------------------------- GLOBAL VARIABLES ----------------------------------- // // BARO // Adafruit_BMP280 bmp(&Wire); -int baro_adress = 0x00; +int baro_adress = 0x00; +movingAvg baro_avg(20); float alt_baro = 0; -float prev_alt = 0; -float vspeed = 0; float baro_set = 1000; -float baro_count = 0; -int vspeed_count = 0; -bool new_baro = false; -float dt = 0; -bool stable_descent = false; -int descent_count = 0; -movingAvg al(5); -movingAvg vs(5); - +bool new_baro_alt = false; +float baro_vspeed = 0; +float prev_alt_baro = 0; +unsigned long prev_time_baro = 0; +unsigned long stable_baro_timer = 0; +int stable_baro_alt_since = 0; +int baro_count = 0; // BATTERY // movingAvg voltage(50); @@ -102,15 +95,26 @@ boolean batt_low = false; TinyGPSPlus gps; TinyGPSCustom fix_type(gps, "GNGSA", 2); movingAvg rs(1); +movingAvg gps_vs_time(10); unsigned char serial2bufferRead[1000]; -float prev_cog = 0; -int gps_count = 0; -int valid_count = 0; -float prev_gps = 0; +float prev_cog = 0; boolean new_gps = false; boolean cog_ok = 0; boolean new_cog = false; float ground_altitude = 0; +int valid_count = 0; + +bool new_gps_alt = false; +float gps_vspeed = 0; +float prev_alt_gps = 0; +unsigned long prev_time_gps = 0; +unsigned long stable_gps_timer = 0; +int stable_gps_alt_since = 0; + +bool new_merged_alt = false; +float merged_vspeed = 0; +float prev_alt_merged = 0; +unsigned long prev_time_merged = 0; // LED // #define LED_PIN 3 @@ -143,7 +147,6 @@ int cells = 0; bool armed = false; bool camIsOn = false; bool camReady = false; -bool flight_started = false; int onDuration = 0; int offDuration = 0; @@ -161,6 +164,7 @@ float lat_B = 0; float lon_B = 0; float cmd = 0; boolean spiral = false; +float sim_cmd = 0; float cmdHome = 1500; float next_cog = 0; float ratecog = 0; @@ -272,7 +276,6 @@ int loop_time_max_glob = 999; double loop_time_mean = 999; int loop_time_count = 0; - int delaySD = 100; // Datalog int delayTLM = 1000; // Tlm @@ -302,9 +305,8 @@ void setup() { EasyBuzzer.setPin(buzzer); - voltage.begin(); - vs.begin(); - al.begin(); + baro_avg.begin(); + voltage.begin(); roll_steer.begin(); pitch_steer.begin(); rs.begin(); @@ -328,12 +330,8 @@ void setup() { watchdog.enable(Watchdog::TIMEOUT_1S); getconfig(); bmp.begin(baro_adress); - - bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, - Adafruit_BMP280::SAMPLING_X1, - Adafruit_BMP280::SAMPLING_X2, - Adafruit_BMP280::FILTER_X16, - Adafruit_BMP280::STANDBY_MS_1); + + baro_begin(); strip.begin(); strip.show(); @@ -368,7 +366,6 @@ void setup() { void loop() { tloop = micros(); - sim(); getdata(); @@ -382,7 +379,6 @@ void loop() { userinter(); loop_time = micros()-tloop; - loop_time_cmpt(); } @@ -391,34 +387,32 @@ void getdata() { // -------------------------- Get GPS -------------------------- // - while (Serial7.available()) { gps.encode(Serial7.read()); } + while (Serial7.available()) { + gps.encode(Serial7.read()); + if (gps.altitude.isUpdated()) { + new_gps_alt = true; + } + } // -------------------------- Get BARO & COMPASS -------------------------- // - if ((millis()-baroA)>=10) { + if ((millis()-baroA)>=5) { + baroA = millis(); unsigned waitd = millis(); - alt_baro = al.reading(bmp.readAltitude(baro_set)*100); + alt_baro = bmp.readAltitude(baro_set); + alt_baro = baro_avg.reading(alt_baro*100.0); + alt_baro = alt_baro/100.0; + if (((millis() - waitd) > 100)) { EasyBuzzer.singleBeep(3000,50); bmp.begin(baro_adress); - bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, - Adafruit_BMP280::SAMPLING_X1, - Adafruit_BMP280::SAMPLING_X2, - Adafruit_BMP280::FILTER_X16, - Adafruit_BMP280::STANDBY_MS_1); - } - alt_baro = (alt_baro/100); - baro_count = (baro_count + 1);; - - if (baro_count >= 10) { - baro_count = 0; - new_baro = true; - dt = baroA-baroB; - baroB = millis(); - } + baro_begin(); + } + } + // -------------------------- Get RC -------------------------- // rx.read(&channels[0], &failSafe, &lostFrame); @@ -498,12 +492,9 @@ void datacmpt() { //cmdHome = cmd_mult*PIDsum cmdHome = cmd_mult*Output; - - - - - if (vspeedvdown-3) { spiral = false; } + + if (baro_vspeedvdown-3) { spiral = false; } if (spiral == true) { cmdHome = 0; } if ((TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B))<10) { @@ -526,70 +517,54 @@ void datacmpt() { steer_auto = map(sim_cmd, -180, +180, 1000, 2000); } +// -------------------- Alt Fusion -------------------- // + + baro_weight = (1+(abs(baro_vspeed)/10)); + + float hdo = gps.hdop.value(); + float gpsw = ((50/hdo)+(gps.altitude.meters()/10000)); + gps_weight = (gpsw*((abs(alt_baro-gps.altitude.meters())/10))); + + float total_weight = gps_weight+baro_weight; + + merged_alt = ((alt_baro*baro_weight)+(gps.altitude.meters()*gps_weight))/(total_weight); + // -------------------- Vertical Speed -------------------- // - if (new_baro == true) { - - new_baro = false; - - if ((initialised == false) and (reboot_state != 1)) { baro_set = (baro_set + ((0-alt_baro)/100)); } - if (millis()<=5000) { alt_baro = 0; } - - float da = (alt_baro - prev_alt); - - if (abs(da) < 50) { - float vps = (da/(dt/1000)); - vspeed = vs.reading(vps*100); - vspeed = (vspeed/100); - } - - prev_alt = alt_baro; + if ((initialised == false) and (reboot_state != 1)) { + baro_set = (baro_set + ((0-alt_baro)/100)); + } + if (millis()<=7500) { alt_baro = 0; } + + if (millis()-prev_time_baro>=200) { + new_baro_alt = false; + float baro_da = (alt_baro - prev_alt_baro); + float baro_dt = (millis() - prev_time_baro); + baro_vspeed = (baro_da/(200.0/1000.0)); + prev_alt_baro = alt_baro; + prev_time_baro = millis(); } -// -------------------- Alt Fusion -------------------- // + if (new_gps_alt) { + new_gps_alt = false; + float gps_da = (gps.altitude.meters() - prev_alt_gps); + float gps_dt = (millis() - prev_time_gps); + gps_vspeed = (gps_da/(gps_dt/1000)); + prev_alt_gps = gps.altitude.meters(); + prev_time_gps = millis(); + } - baro_weight = (1+(abs(vspeed)/10)); - - float hdo = gps.hdop.value(); - float gpsw = ((50/hdo)+(gps.altitude.meters()/10000)); - gps_weight = (gpsw*((abs(alt_baro-gps.altitude.meters())/10))); - - merged_alt = ((alt_baro*baro_weight)+(gps.altitude.meters()*gps_weight))/(baro_weight+gps_weight); + if (abs(baro_vspeed) > 1) { + stable_baro_timer = millis(); + } + if (abs(gps_vspeed) > 1) { + stable_gps_timer = millis(); + } -// -------------------- Stationary -------------------- // - - if ((millis()-tstab) >= 1000) { - tstab = millis(); - - if (abs(vspeed) < 0.2) { vspeed_count = (vspeed_count + 1); } - else { vspeed_count = 0; } - - if (abs(vspeed)>(-vdown)) { descent_count = (descent_count + 1); } - else { descent_count = 0; } - - if (abs(prev_gps-gps.altitude.meters())<1 and (gps.altitude.meters() != 0)) { gps_count = (gps_count + 1); } - else { gps_count = 0; } - - if (gps_count >= 10) { gps_stab = true; } - else if(millis()<(time_out*1000)) { gps_stab = false; } - else { gps_stab = true; } - - - if (vspeed_count >= 10) { baro_stab = true; } - else { baro_stab = false; } - - if (descent_count >= descent_timer) { stable_descent = true; } - else { stable_descent = false; } - - if (gps_count >= 10) { gps_stab = true; } - else { gps_stab = false; } - - prev_gps = gps.altitude.meters(); - - - } + stable_gps_alt_since = millis()-stable_gps_timer; + stable_baro_alt_since = millis()-stable_baro_timer; // -------------------------- String -------------------------- // @@ -621,7 +596,7 @@ void datacmpt() { String gps_text = date_time+","+lat_A_text+","+lon_A_text+","+alt_gps_text+","+cog_text+","+speed_text+","+sat_text+","+hdop_text+","+pos_age_text+","+fix_type_text; String alt_baro_text = String(alt_baro, 3); - String vspeed_text = String(vspeed, 3); + String vspeed_text = String(baro_vspeed, 3); String baro_text = alt_baro_text+","+vspeed_text; @@ -674,8 +649,8 @@ void datacmpt() { String gps_ok_text = String(gps_ok) ; String cog_ok_text = String(cog_ok); String failSafe_text = String(failSafe); - String gps_stab_text = String(gps_stab); - String baro_stab_text = String(baro_stab); + String gps_stab_text = String(stable_gps_alt_since); + String baro_stab_text = String(stable_baro_alt_since); String deployed_text = String(deployed); String wing_opened_text = String(wing_opened); String vbatt_text = String(vbatt, 3); @@ -719,7 +694,7 @@ void datacmpt() { tlm = millis(); packet_count = (packet_count +1); Serial5.println(mainTLM); - Serial.println(mainTLM); + //Serial.println(mainTLM); if (sd_ok == true and sd_writing == true) { dataFile = SD.open(namebuff, FILE_WRITE); @@ -837,11 +812,6 @@ void flight_state() { motorised_failSafe(); setled(0, 255, 255, 25, 500); break; - - case 11: - motorised_cruise(); - setled(0, 255, 255, 25, 500); - break; } } @@ -851,7 +821,7 @@ void flight_init() { if (reboot_state !=1) { - if ((gps.satellites.value()>=6 and gps_ok and (gps_stab or (gps.satellites.value()>=6 and millis()>300000)) and millis()>5000) or no_init) { + if ((gps.satellites.value()>=6 and gps_ok and millis()>5000) or no_init) { EasyBuzzer.beep(3000,100,50,10,500,1); @@ -876,9 +846,6 @@ void flight_init() { EEPROM.put(50, lon_B); baroset(); - - vs.reset(); - al.reset(); if (no_init == false) { dep_altitude = (dep_altitude+gps.altitude.meters()); @@ -907,9 +874,6 @@ void flight_init() { EasyBuzzer.beep(3000,100,50,10,500,1); initialised = true; - - vs.reset(); - al.reset(); strip.setBrightness(50); setcam(1, 60, 60); @@ -938,55 +902,48 @@ void flight_init() { //------------------- 1 -------------------// void ready_steady() { - if (millis()-init_time>=1000) { - - if (vspeed>vup) { + if (baro_vspeed>vup) { flight_mode = 2; EasyBuzzer.beep(1000,100,50,2,500,1); strip.setBrightness(255); setcam(1, 20, 600); } - - if (safe_trigger and stable_descent) { + else if (baro_vspeed10) { - flight_started = true; - } + if (baro_vspeed<0.5) { + flight_mode = 1; strip.setBrightness(255); + } } //------------------- 3 -------------------// -void flight_descent() { +void flight_descent() { + + if (baro_vspeed>1) { + flight_mode = 1; + strip.setBrightness(255); + } - if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(255);} - if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); setcam(1, 60, 600); } - - } + if (merged_alt < dep_altitude) { + flight_mode = 4; + EasyBuzzer.beep(3000,100,50,5,500,1); + strip.setBrightness(255); + deployed = true; + init_time = millis(); + setcam(1, 60, 600); + } +} //------------------- 4 -------------------// @@ -1019,7 +976,10 @@ void flight_gliding_auto() { myPID.SetMode(MANUAL); } - if (i_want_to_fly == true) { spiral = false; flight_mode = 5; } + if (i_want_to_fly == true) { + spiral = false; + flight_mode = 5; + } navigation(); } @@ -1051,12 +1011,6 @@ void motorised_man() { myPID.SetMode(AUTOMATIC); } - else if (channels[5] > 1000) { - flight_mode = 11; - cruise_cmd = gps.course.deg(); - if (record_home == true) {newfile();} - } - if (failSafe == true) { flight_mode = 10; if (record_home == true) {newfile();} @@ -1068,8 +1022,11 @@ void motorised_man() { void motorised_auto() { - navigation(); - if (channels[6] < 1000) { flight_mode = 8; } + navigation(); + + if (channels[6] < 1000) { + flight_mode = 8; + } } //------------------- 10 -------------------// @@ -1084,23 +1041,6 @@ void motorised_failSafe() { } } -//------------------- 11 -------------------// - -void motorised_cruise() { - if (channels[5] < 1000) { - flight_mode = 8; - myPID.SetMode(MANUAL); - } - - float incr = map(channels[0], 67, 1982, -1000, 1000); - if (abs(incr)<50) { incr = 0; } - incr = incr/10000.0; - - cruise_cmd = cruise_cmd + incr; - cruise_cmd = (int(cruise_cmd*1000.0)%360000)/1000.0; - Serial.println(cruise_cmd); -} - // ----------------------------------------------------- Apply command ----------------------------------------------------- // void applycmd() { @@ -1248,6 +1188,7 @@ if (drop == true) { } } + else { @@ -1259,7 +1200,8 @@ else { esc.write(map(servo_esc, 1000, 2000, 0, 180)); } - } + } + } // ----------------------------------------------------- User Interface ----------------------------------------------------- // @@ -1487,11 +1429,10 @@ void baroset() { baro_blk = millis(); alt_baro = (bmp.readAltitude(baro_set)); baro_set = (baro_set + ((gps.altitude.meters()-alt_baro)/8)); - prev_alt = alt_baro; watchdog.reset(); } } - + baro_avg.reset(); EEPROM.put(70, baro_set); } @@ -1599,8 +1540,8 @@ void newfile() { } void sim() { - sim_cmd_time = (millis()*(PI/10000)); - sim_cmd = sin(sim_cmd_time); + double long sim_cmd_time = (millis()*(PI/10000)); + float sim_cmd = sin(sim_cmd_time); sim_cmd = map(sim_cmd, -1, 1, -180, 180); if (test_dir_rc) { sim_cmd = map(roll_man, 1000, 2000, -180, 180); @@ -1730,3 +1671,11 @@ if (loop_time_count >= 100) { } } } + +void baro_begin() { + bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, + Adafruit_BMP280::SAMPLING_X1, + Adafruit_BMP280::SAMPLING_X16, + Adafruit_BMP280::FILTER_X16, + Adafruit_BMP280::STANDBY_MS_1); +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp new file mode 100644 index 0000000..cd0b02d --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp @@ -0,0 +1,121 @@ +#include "config.h" +#include +#include + +Adafruit_BMP280 bmp(&Wire); +int baro_adress = 0x00; +double baro_set = 1000.0; + +float alt_baro = 0; +float pressure_baro = 0; +float prev_alt_baro = 0; +float baro_vspeed = 0; +float baro_count = 0; + +bool new_baro = false; +bool stable_descent = false; + +movingAvg b_al(BARO_AL_AVG); +movingAvg b_vs(BARO_VS_AVG); +movingAvg ps(BARO_PS_AVG); + +unsigned long baroA = 0; +unsigned long baroB = 0; +unsigned long baro_blk = 0; +unsigned long baro_clb = 0; + +void baroset(float alt_set, int factor) { + do { + if ((micros()-baro_blk)>=10) { + baro_blk = millis(); + alt_baro = bmp.readAltitude(baro_set); + baro_set = (baro_set + ((alt_set-alt_baro)/(8*factor))); + prev_alt_baro = alt_baro; + } + } while (abs(alt_baro-alt_set)>0.01); +} + +void baro_adjust(float alt_set, int factor) { + if ((millis()-baro_clb)>1000) { + baro_clb = millis(); + alt_baro = bmp.readAltitude(baro_set); + baro_set = (baro_set + ((alt_set-alt_baro)/(8*factor))); + } +} + +void barometer_setup() { + b_vs.begin(); + b_al.begin(); + ps.begin(); + bmp.begin(baro_adress); + bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, + Adafruit_BMP280::SAMPLING_X1, + Adafruit_BMP280::SAMPLING_X2, + Adafruit_BMP280::FILTER_X16, + Adafruit_BMP280::STANDBY_MS_1); + baroset(0, 1); +} + +void cmpt_vertical_speed_baro(float da, int dt) { + baro_vspeed = (da/(dt/1000.0)); + baro_vspeed = b_vs.reading(baro_vspeed*100); + baro_vspeed = (baro_vspeed/100); +} + +void get_baro(int mode) { + if ((millis()-baroA)>=10 and mode == 0) { + + baroA = millis(); + unsigned waitd = millis(); + + alt_baro = bmp.readAltitude(baro_set); + pressure_baro = bmp.readPressure(); + + alt_baro = (b_al.reading(alt_baro*100.0)/100.0); + pressure_baro = (ps.reading(pressure_baro*100.0)/100.0); + + if (((millis() - waitd) >= 100)) { + barometer_setup(); + } + baro_count = (baro_count + 1);; + + if (baro_count >= BARO_VS_SAMPLE) { + baro_count = 0; + cmpt_vertical_speed_baro(alt_baro-prev_alt_baro, baroA-baroB); + baroB = millis(); + prev_alt_baro = alt_baro; + new_baro = true; + } + } + + else { + baroA = millis(); + unsigned waitd = millis(); + + alt_baro = bmp.readAltitude(baro_set); + pressure_baro = bmp.readPressure(); + + alt_baro = (b_al.reading(alt_baro*100.0)/100.0); + pressure_baro = (ps.reading(pressure_baro*100.0)/100.0); + + if (((millis() - waitd) >= 100)) { + barometer_setup(); + } + baro_count = (baro_count + 1);; + + if (baro_count >= BARO_VS_SAMPLE) { + baro_count = 0; + cmpt_vertical_speed_baro(alt_baro-prev_alt_baro, baroA-baroB); + baroB = millis(); + prev_alt_baro = alt_baro; + new_baro = true; + } + } +} + + +String baro_text() { + String alt_baro_text = String(alt_baro, 3); + String baro_vspeed_text = String(baro_vspeed, 3); + return alt_baro_text+","+baro_vspeed_text; +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp new file mode 100644 index 0000000..33ae5d7 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp @@ -0,0 +1,23 @@ +#include "config.h" + +#include +movingAvg voltage(50); + +float vpin = A17; + +float vbatt = 0; +boolean batt_critical = false; +boolean batt_low = false; +int cells = 0; + +void battery_setup() { + pinMode(vpin, INPUT); + voltage.begin(); +} + +void get_vbatt() { + analogReadResolution(12); + vbatt = analogRead(A17); + vbatt = (voltage.reading(vbatt)); + vbatt = vbatt/223.39; +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp new file mode 100644 index 0000000..233432a --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp @@ -0,0 +1,52 @@ +#include "config.h" + +int cam = 23; +bool camIsOn = false; +bool camReady = false; +int onDuration = 0; +int offDuration = 0; + +unsigned long camOn = 0; +unsigned long camOff = 0; + +void camera_setup() { + pinMode(cam, OUTPUT); + digitalWrite(cam, LOW); +} + +void setcam(int a, int b, int c) { + switch(a) { + + case 1 : + digitalWrite(cam, HIGH); + camOn = millis(); + onDuration = (b+4)*1000; + offDuration = c*1000; + camIsOn = true; + break; + + case 0 : + digitalWrite(cam, LOW); + camOff = millis(); + onDuration = (b+4)*1000; + offDuration = c*1000; + camIsOn = false; + break; + } + camReady = true; +} + +void updatecam() { + if (CAM_CYCLE and camReady) { + if(camIsOn) { + if ((millis()-camOn)>=onDuration) { + setcam(0, (onDuration/1000)-4, offDuration/1000); + } + else { + if ((millis()-camOff)>offDuration) { + setcam(1, (onDuration/1000)-4, offDuration/1000); + } + } + } + } +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h new file mode 100644 index 0000000..4d926f8 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h @@ -0,0 +1,65 @@ +//---------- CONFIG ----------// + +#define I_WANT_TO_FLY false // Simulated servo movement to test the servo movement :)) +#define TEST_DIR_RC false // Use channels 0 on the radio to test the direction of the autopilot and the servos, I_WANT_TO_FLY should be set true too. +#define BUZZER_TURN false // Buzzer sounds as function of the turn command +#define BUZZER_SWEEP false // Buzzer turn on steroïds, should be easier to understand his tricky language ^^ +#define NO_INIT false // Skip init, for testing only purposes +#define NAV_WAYPOINT true // Doing the waypoint sequence before reaching home? +#define NAV_HOME false // Should it go home after the waypoint sequence? +#define SD_WRITING true // Should it write on the SD card or not? +#define LOW_RATE false // Dataloging at low HZ (if true) instead of 20Hz, for balloon flight +#define LED_MODEL 0 // Big led = 1, small led = 0. +#define CAM_CYCLE false // If true, camera will only be powered on for 20s every 10min, used to save battery on long HAB flight +#define SAFE_TRIGGER false // For HAB flight, will use a safer, but slower methode to detect apogee and transision to descent mode +#define COG_BRAKE false // Will reduce command if CoG is turning faster than a threshold +#define GPS_FREQ 5 // Hz +#define SERVO_RATE 50 // Hz +#define TLM_MONITOR true // Show telemetry on serial monitor +#define GPS_PORT Serial7 +#define TLM_PORT Serial5 +#define RX_PORT Serial1 +#define TIME_OUT 300 + +//---------- FLIGHT SETTINGS ----------// + +#define RC_MODE 1 // only roll (0), pitch and roll mixed (1), pitch and roll separated (2) +#define CONTROL_MODE 1 // neutral position at the center (0) or with "hands up" (1) +#define LINEAR_MODE 0 // command is linear (0), or linear but with a large deadband set using servo_start etc (1) +#define DROP true // R2Home's version, drop or motorised + +#define DESCENT_TIMER 1000 +#define GLIDING_TIMER 3000 +#define DEP_ALT 100 // m above ground +#define VUP 1 // m/s +#define VDOWN -1.5 // m/s + +#define NKP 1 +#define NKI 0.05 +#define NKD 0.1 + +#define BCRITICAL 3.4 +#define BLOW 3.5 +#define NO_BATT 4.0 + +#define SERVO_MAX_M 2000 // m for map +#define SERVO_MAX_C 2000 // c for constrain + +#define TRIG 20 +#define LEFT_OFFSET 100 +#define RIGHT_OFFSET 100 + +#define HOME_WAYPOINT_THRESHOLD 10 // Distance to waypoint before going to the next waypoint + +#define BARO_VS_AVG 1 +#define BARO_VS_SAMPLE 50 + +#define BARO_AL_AVG 50 +#define BARO_PS_AVG 1 + +#define GPS_VS_AVG 1 + +#define GPS_SAFE_AVG 50 +#define BARO_SAFE_AVG 3 + +#define PRE_PE_AVG 50 diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp new file mode 100644 index 0000000..95006ed --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp @@ -0,0 +1,18 @@ +#include +#include "config.h" + +double Setpoint, Input, Output; +PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT); + +void navigation_setup() { + myPID.SetTunings(NKP, NKI, NKD); + myPID.SetOutputLimits(-180, 180); + myPID.SetMode(MANUAL); +} + +float cmpt_cmd(float err) { + Input = -err; + Setpoint = 0; + myPID.Compute(); + return map(Output, -180, 180, 1000, 2000); +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp new file mode 100644 index 0000000..071c3c2 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp @@ -0,0 +1,163 @@ +#include +#include +#include "feedback.hpp" + +Watchdog watchdog; +bool sd_ok = false; +int delaySD = 100; // Datalog +int delayTLM = 1000; // Tlm + +char sdnamebuff[20]; +String mainSD; +String mainTLM; +String minSD; + +File dataFile; +File configFile; +const int chipSelect = BUILTIN_SDCARD; +char namebuff[20]; +unsigned int addr = 0; +String filename; +int datatest = 0; +int count = 0; +int packet_count = 0; + +unsigned long tlm = 0; +unsigned long sd = 0; + +int time_number = 0; + + +void cmpt_data_rate(int flight_mode) { + switch(flight_mode) { + + case 0: + delaySD = 200; + delayTLM = 5000; + break; + + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 8: + case 9: + if (LOW_RATE) { delaySD = 200; } + else { delaySD = 50; } + delayTLM = 1000; + break; + } +} + +void send_data() { + packet_count = (packet_count +1); + TLM_PORT.println(mainTLM); + if (TLM_MONITOR) { + Serial.println(mainTLM); + } +} + +void save_data(bool initialised) { + if (initialised == true) { + if (sd_ok == true and SD_WRITING == true) { + dataFile = SD.open(namebuff, FILE_WRITE); + dataFile.println(mainSD); + dataFile.close(); + } + } +} + +void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool wing_opened) { + + time_number = ((gps.date.day()*1000000) + (gps.time.hour()*10000) + (gps.time.minute()*100) + gps.time.second()); + + int actual_time = millis(); + String time_text = String(actual_time); + String flight_mode_text = String(flight_mode); + String initialised_text = String(initialised); + String deployed_text = String(deployed); + String wing_opened_text = String(wing_opened); + String gps_ok_text = String(gps_ok) ; + String cog_ok_text = String(cog_ok); + String failsafe_text = String(failsafe); + String vbatt_text = String(vbatt,2); + String loop_rate_text = String(loop_rate); + String packet_count_text = String(packet_count); + + String status_text = time_text+","+packet_count_text+","+flight_mode_text+","+initialised_text+","+deployed_text+","+wing_opened_text+","+gps_ok_text+","+cog_ok_text+","+failsafe_text+","+vbatt_text+","+loop_rate_text; + + mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text(); + mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*"; +} + +void newfile() { + dtostrf(time_number, 1, 0, sdnamebuff); + sprintf(namebuff, "%s.txt", sdnamebuff); + + if (!SD.begin(chipSelect)) { sd_ok = false; } + else { + sd_ok = true; + SdFile::dateTimeCallback(dateTime); + dataFile = SD.open(namebuff, FILE_WRITE); + delay(10); + if (dataFile) { + dataFile.println("time (ms), Packet_Count (text), Mode (text), Initialised (text), Deployed (text), Wing_Opened (text), GPS_Ok (text), COG_Ok (text), FailSafe (text), Vbatt (V), Loop_rate (Hz), GPS-date, GPS-time, lat (deg), lon (deg), alt (m), CoG (deg), Speed (m/s), Sat_in_use (text), HDOP (text), Position_Age (text), Fix_type (text), Baro_Alt (m), Baro_Vspeed (m/s), Altitude (m), Baro_Weight, GPS_Weight, Baro_Vspeed_AVG (m/s), GPS_Vspeed_AVG (m/s), VDOWN (m/s), SetPoint_Home (deg), Err_Home (deg), LatB (deg), LonB (deg), WaypointNumber (text), Distance (m), Ch 0 (us), Ch 1 (us), Ch 2 (us), Ch 3 (us), Ch 4 (us), Ch 5 (us), Ch 6 (us), PWM_L (us), PWM_R (us), PWM_D (us)"); + dataFile.close(); + } + } +} + + +void getconfig() { + watchdog.enable(Watchdog::TIMEOUT_1S); + + if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); } + else { + sd_ok = true; + File configFile = SD.open("config.txt", FILE_READ); + if (configFile) { + + // Reading the baro adress + String memory = ""; + while (configFile.available()) { + char a = configFile.read(); + if (a != 44) { memory = memory + a; } + else { break; } + } + + baro_adress = memory.toFloat(); + + char a = '0'; + unsigned int i(0); + + // Reading the Waypoints + while (configFile.available() and (i<16)) { + String waypoint_str = ""; + do { + a = configFile.read(); + if (a != 44) { waypoint_str = waypoint_str + a; } + else { waypoint[i].latitude = (waypoint_str.toFloat()); break; } + } while (a != 44); + waypoint_str = ""; + do { + a = configFile.read(); + if (a != 44) { waypoint_str = waypoint_str + a; } + else { waypoint[i].longitude = (waypoint_str.toFloat()); break; } + } while (a != 44); + waypoint_str = ""; + do { + a = configFile.read(); + if (a != 44) { waypoint_str = waypoint_str + a; } + else { waypoint[i].radius = (waypoint_str.toFloat()); i++; break; } + } while (a != 44); + } + last_waypoint_number = i+1; + configFile.close(); + } + else { + delay(1500) ; + } + } +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/debug.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/debug.hpp new file mode 100644 index 0000000..1318ba1 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/debug.hpp @@ -0,0 +1,13 @@ +#include "config.h" + +double long sim_cmd_time = 0; +float sim_cmd = 0; +int loop_count = 0; +int loop_rate = 999; +unsigned long long tloop = 0; + +float sim() { + sim_cmd_time = (millis()*(PI/10000)); + sim_cmd = sin(sim_cmd_time); + return map(sim_cmd, -1, 1, 1000, 2000); +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp new file mode 100644 index 0000000..3dad246 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp @@ -0,0 +1,199 @@ +#include +#include + +#include "config.h" +#include "battery.hpp" +#include "camera.hpp" +#include "debug.hpp" +#include "navigation.hpp" +#include "rc.hpp" +#include "servo.hpp" + +#define LED_PIN 3 +#define LED_COUNT 1 +#define BRIGHTNESS 15 + +Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRBW + NEO_KHZ800); +int duration = 0; +int timeled = 0; +int lastled = 0; + +int BUZZER_PIN = 2; +int mid_freq = 0; +float time_sweep = 0; +int direc = 0; +unsigned long sweep_start = 0; +unsigned long tbeep = 0; +bool new_sweep = false; +bool current_sweep = false; +int current_freq = 0; +float sweep_step = 1; +float time_step = 1; +unsigned long last_waypoint_time = 0; +unsigned long sat_buzz = 0; +unsigned long batt_buzz = 0; +unsigned long t_turn = 0; + +int beep_start = 0; +int beep_stop = 0; + + + +void buzzer_setup() { + pinMode(BUZZER_PIN, OUTPUT); + EasyBuzzer.setPin(BUZZER_PIN); + tone(BUZZER_PIN, 523); + delay(200); + tone(BUZZER_PIN, 582); + delay(200); + tone(BUZZER_PIN, 762); + delay(200); + noTone(BUZZER_PIN); +} + +void buzzer_end_setup() { + tone(BUZZER_PIN, 582); + delay(200); + tone(BUZZER_PIN, 830); + delay(300); + noTone(BUZZER_PIN); +} + +void sweep_beep_set(int freq, int dur, int dir) { + mid_freq = freq; + time_sweep = dur; + new_sweep = true; + direc = dir; + beep_start = millis(); +} + +void buzzer_turn(int flight_mode, int turn_cmd) { + if (BUZZER_TURN == true and (flight_mode == 5 or flight_mode == 9 or flight_mode == 10)) { + + if (millis()-last_waypoint_time >= 2000) { + + int tone_turn = map(turn_cmd, 1000, 2000, 1000, 2000); + + if (BUZZER_SWEEP == true) { + double t_down = abs(map(turn_cmd, 1000, 2000, -3, 3))+1; + t_down = (1/t_down)*1500; + if ((millis()-t_turn)>=t_down) { + t_turn = millis(); + int dir = 0; + if (tone_turn >= 1500) { dir = 0; } + else { dir = 1; } + float force = 0; + if (tone_turn >= 1500) { force = tone_turn - 1500; } + else { force = 1500 - tone_turn; } + force = constrain(force, 10, 200); + sweep_beep_set(tone_turn, force, dir); + } + } + else { + double t_down = abs(map(turn_cmd, 1000, 2000, -5, 5))+1; + t_down = (1/t_down)*500; + if ((millis()-t_turn)>=t_down) { + t_turn = millis(); + EasyBuzzer.singleBeep(tone_turn, 20); + } + } + } + } +} + +void sweep_beep_update() { + if (direc == 0) { + if (new_sweep == true) { + sweep_start = millis(); + new_sweep = false; + current_sweep = true; + current_freq = mid_freq-((sweep_step*time_sweep)/(2*time_step)); + EasyBuzzer.singleBeep((current_freq),time_step); + } + + if ((current_sweep == true) and ((millis()-sweep_start)>=(time_step)) and (current_freq=(time_step)) and (current_freq>mid_freq-((sweep_step*time_sweep)/(2*time_step)))) { + sweep_start = millis(); + current_freq = current_freq - sweep_step; + EasyBuzzer.singleBeep((current_freq),time_step); + beep_stop = millis(); + } + } +} + +void buzzer_batt() { + cells = int(vbatt/3.65); + + if ((vbatt < (cells*BCRITICAL)) and (vbatt > NO_BATT)) { + batt_critical = true; + if (millis()-batt_buzz>=100) { + batt_buzz = millis(); + EasyBuzzer.singleBeep(3000,25); + } + } + + else { batt_critical = false; } + + if ((vbatt < (cells*BLOW)) and (vbatt > (cells*BCRITICAL))) { + batt_low = true; + if (millis()-batt_buzz>=200) { + batt_buzz = millis(); + EasyBuzzer.singleBeep(3000,50); + } + } + + else { batt_low = false; } +} + +void update_buzzer() { + sweep_beep_update(); + EasyBuzzer.update(); +} + +void led_setup() { + strip.begin(); + strip.show(); + strip.setBrightness(255); +} + +void colorWipe(uint32_t color, int wait) { + for(int i=0; i=e) { + lastled = millis(); + if (LED_MODEL == 1) { colorWipe(strip.Color(b,a,c), 0); } + else { colorWipe(strip.Color(a,b,c), 0); } + duration = d; + timeled = millis(); + } +} + +void updateled() { + if ((millis()-timeled)>=duration) { + colorWipe(strip.Color(0,0,0), 0); + } +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp new file mode 100644 index 0000000..9003050 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp @@ -0,0 +1,287 @@ +#include "config.h" +#include "data.hpp" + +int flight_mode = 0; +int dep_altitude = 0; +bool initialised = false; +bool flight_started = false; +bool deployed = false; +bool wing_opened = false; +unsigned long init_time = 0; + + +//------------------- 0 -------------------// + +void flight_init() { + + if ((gps.satellites.value()>=6 and gps_ok and gps.hdop.value()<200) or NO_INIT) { + + EasyBuzzer.beep(3000,100,50,10,500,1); + + if ((NAV_WAYPOINT == true) and (NAV_HOME == true)) { + waypoint[last_waypoint_number].latitude = gps.location.lat(); + waypoint[last_waypoint_number].longitude = gps.location.lng(); + waypoint[last_waypoint_number].radius = HOME_WAYPOINT_THRESHOLD; + current_waypoint = waypoint[waypoint_number]; + } + + else if ((NAV_WAYPOINT == false) and (NAV_HOME == true)) { + current_waypoint.latitude = gps.location.lat(); + current_waypoint.longitude = gps.location.lng(); + current_waypoint.radius = HOME_WAYPOINT_THRESHOLD; + } + + else if ((NAV_WAYPOINT == true) and (NAV_HOME == false)) { + current_waypoint = waypoint[waypoint_number]; + } + + baroset(gps.altitude.meters(), 1); + + get_baro(1); + + b_vs.reset(); + b_al.reset(); + g_vs.reset(); + + ground_altitude = gps.altitude.meters(); + dep_altitude = (DEP_ALT+ground_altitude); + + setcam(1, 60, 60); + newfile(); + + if (DROP == true) { flight_mode = 1;} + else { flight_mode = 8; } + + init_time = millis(); + initialised = true; + } + + if (millis()-sat_buzz>5000) { + sat_buzz = millis(); + EasyBuzzer.beep(2000,50,25,gps.satellites.value(),100,1); + } + +} + +//------------------- 1 -------------------// + +void ready_steady() { + + if (millis()-init_time>=5000) { + + if (is_ascent(VUP, 0)) { + flight_mode = 2; + EasyBuzzer.beep(1000,100,50,2,500,1); + setcam(1, 20, 600); + } + + if (is_descent(v_down(VDOWN), 0)) { + flight_mode = 3; + EasyBuzzer.beep(3000,100,50,3,500,1); + setcam(1, 60, 600); + init_time = millis(); + } + + if ((atof(fix_type.value()) < 2) and (NO_INIT == false) and (flight_started == false)) { + flight_mode = 0; + } + + if (I_WANT_TO_FLY) { + flight_mode = 5; + } + + } +} + +//------------------- 2 -------------------// + +void flight_ascent() { + if (baro_vspeed<0.5) { + flight_mode = 1; + } + if ((gps.altitude.meters()-ground_altitude)>10) { + flight_started = true; + } +} + +//------------------- 3 -------------------// + +void flight_descent() { + + if (is_ascent(0, 0)) { + flight_mode = 1; + } + if (is_descent(v_down(VDOWN), 0) and (merged_alt < dep_altitude or (millis()-init_time>DESCENT_TIMER))) { + flight_mode = 4; + EasyBuzzer.beep(3000,100,50,5,500,1); + deployed = true; + init_time = millis(); + setcam(1, 60, 600); + } + } + +//------------------- 4 -------------------// + +void flight_gliding() { + + if ((millis()-init_time) >= GLIDING_TIMER) { + wing_opened = true; + + if (failsafe == true) { + flight_mode = 5; + setcam(1, 20, 600); + myPID.SetMode(AUTOMATIC); + } + + else { + flight_mode = 6; + myPID.SetMode(MANUAL); + } + } +} + +//------------------- 5 -------------------// + +void flight_gliding_auto() { + + if (!gps_ok) { + flight_mode = 11; + myPID.SetMode(MANUAL); + } + + if (failsafe == false) { + flight_mode = 6; + myPID.SetMode(MANUAL); + } + + if (I_WANT_TO_FLY == true) { + flight_mode = 5; + } + + navigation(); +} + +//------------------- 6 -------------------// + +void flight_gliding_manual() { + if (failsafe == true) { + flight_mode = 5; + myPID.SetMode(AUTOMATIC); + } +} + +//------------------- 7 -------------------// + +void landed() { +} + +//------------------- 8 -------------------// + +void motorised_man() { + + if (channels[6] > 1000) { + flight_mode = 9; + myPID.SetMode(AUTOMATIC); + } + + if (failsafe == true) { + flight_mode = 10; + myPID.SetMode(AUTOMATIC); + } +} + +//------------------- 9 -------------------// + +void motorised_auto() { + + navigation(); + if (channels[6] < 1000) { + flight_mode = 8; + } +} + +//------------------- 10 -------------------// + +void motorised_failSafe() { + + navigation(); + + if (failsafe == false) { + flight_mode = 8; + myPID.SetMode(MANUAL); + } +} + +//------------------- 11 -------------------// + +void flight_gliding_no_gps() { + if (gps_ok) { + flight_mode = 5; + myPID.SetMode(AUTOMATIC); + } +} + +//------------------- STATE MACHINE -------------------// + +void cmpt_flight_state() { + switch(flight_mode) { + case 0: + flight_init(); + setled(255, 0, 0, 25, 2000); + break; + + case 1: + ready_steady(); + setled(0, 255, 0, 25, 500); + break; + + case 2: + flight_ascent(); + setled(0, 0, 255, 25, 2000); + break; + + case 3: + flight_descent(); + setled(255, 128, 0, 25, 1000); + break; + + case 4: + flight_gliding(); + break; + + case 5: + flight_gliding_auto(); + setled(255, 255, 0, 25, 1000); + break; + + case 6: + flight_gliding_manual(); + setled(0, 255, 255, 25, 1000); + break; + + case 7: + landed(); + setled(128, 0, 255, 25, 1000); + break; + + case 8: + motorised_man(); + setled(0, 0, 255, 25, 500); + break; + + case 9: + motorised_auto(); + setled(0, 255, 255, 25, 500); + break; + + case 10: + motorised_failSafe(); + setled(0, 255, 255, 25, 500); + break; + + case 11: + flight_gliding_no_gps(); + setled(255, 0, 0, 25, 200); + break; + } +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp new file mode 100644 index 0000000..a35886b --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp @@ -0,0 +1,175 @@ +#include "config.h" +#include +#include + +TinyGPSPlus gps; +TinyGPSCustom fix_type(gps, "GNGSA", 2); +unsigned char serial2bufferRead[1000]; + +float prev_alt_gps = 0; +float gps_vspeed = 0; +unsigned long gpsB = 0; +float prev_cog = 0; +bool new_gps = false; +bool new_cog = false; + +movingAvg g_vs(GPS_VS_AVG); + +bool gps_ok = false; +bool cog_ok = false; + +void cmpt_vertical_speed_gps(float da, int dt) { + gps_vspeed = (da/(dt/1000.0)); + gps_vspeed = g_vs.reading(gps_vspeed*100); + gps_vspeed = (gps_vspeed/100); +} + +void get_gps() { + + while (GPS_PORT.available()) { + gps.encode(GPS_PORT.read()); + } + + if (((gps.location.age()) < ((1000/GPS_FREQ)*2)) and (atof(fix_type.value()) == 3)) { + gps_ok = true; + } + else { gps_ok = false; } + + if (gps.course.isUpdated()) { + if (abs(prev_cog-gps.course.deg())>0.1 or gps.speed.mps()>3) { + cog_ok = true; + new_cog = true; + prev_cog = gps.course.deg(); + } + else { cog_ok = false; } + } + + if (gps.altitude.isUpdated()) { + cmpt_vertical_speed_gps(prev_alt_gps-gps.altitude.meters(), millis()-gpsB); + gpsB = millis(); + prev_alt_gps = gps.altitude.meters(); + new_gps = true; + } +} + +void sendPacket(byte *packet, byte len){ + for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); } +} + +void gps_setup(int a, int b, int c, int d, int e){ + + g_vs.begin(); + +if (a == 9600) { + GPS_PORT.begin(9600); + delay(100); + byte packet1[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0xB5}; + sendPacket(packet1, sizeof(packet1)); + } + +if (a == 57600) { + GPS_PORT.begin(9600); + delay(100); + byte packet2[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xE1, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xCE, 0xC9}; + sendPacket(packet2, sizeof(packet2)); + GPS_PORT.end(); + GPS_PORT.begin(57600); + delay(100); + } + +if (a == 115200) { + GPS_PORT.begin(9600); + delay(100); + byte packet3[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB0, 0x7E}; + sendPacket(packet3, sizeof(packet3)); + GPS_PORT.end(); + GPS_PORT.begin(115200); + delay(100); + } + +if (b == 1) { + byte packet4[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39}; + sendPacket(packet4, sizeof(packet4)); + } + +if (b == 5) { + byte packet5[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A}; + sendPacket(packet5, sizeof(packet5)); + } + +if (b == 10) { + byte packet6[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12}; + sendPacket(packet6, sizeof(packet6)); + } + +if (c == 0) { + byte packet7[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7E, 0x3C}; + sendPacket(packet7, sizeof(packet7)); + } + +if (c == 1) { + byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x84, 0x08}; + sendPacket(packet8, sizeof(packet8)); + } + +if (c == 2) { + byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x85, 0x2A}; + sendPacket(packet8, sizeof(packet8)); + } + +if (c == 4) { + byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x86, 0x4C}; + sendPacket(packet8, sizeof(packet8)); + } + +if (d == 1) { + byte packet9[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x03, 0x35}; + byte packet10[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x05, 0x43}; + sendPacket(packet9, sizeof(packet9)); + sendPacket(packet10, sizeof(packet10)); + } + +if (e == 1) { + byte packet11[] = {0xB5, 0x62, 0x06, 0x1E, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0x32, 0x00, 0x00, 0x99, 0x4C, 0x00, 0x00, 0x5B, 0x10}; + sendPacket(packet11, sizeof(packet11)); + } + + GPS_PORT.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead)); + +} + +void dateTime(uint16_t* date, uint16_t* time){ + + *date = FAT_DATE(gps.date.year(), gps.date.month(), gps.date.day()); + *time = FAT_TIME(gps.time.hour(), gps.time.minute(), gps.time.second()); +} + +String date_time() { + String date_year = String(gps.date.year()); + String date_month = String(gps.date.month()); + String date_day = String(gps.date.day()); + + String time_hour = String(gps.time.hour()); + String time_minute = String(gps.time.minute()); + String time_second = String(gps.time.second()); + + return date_year+":"+date_month+":"+date_day+","+time_hour+":"+time_minute+":"+time_second; +} + + +String gps_text() { + String lat_A_text = String(gps.location.lat(), 10); + String lon_A_text = String(gps.location.lng(), 10); + String alt_gps_text = String(gps.altitude.meters(), 1); + String cog_text = String(gps.course.deg()); + String speed_text = String(gps.speed.mps(), 1); + String sat_text = String(gps.satellites.value()); + String fix_type_text= String(atof(fix_type.value())); + String hdop_text = String(gps.hdop.value()); + String pos_age_text; + + if (gps.location.age()>10000) { pos_age_text = String(999); } + else { pos_age_text = String(gps.location.age()); } + + return date_time()+","+lat_A_text+","+lon_A_text+","+alt_gps_text+","+cog_text+","+speed_text+","+sat_text+","+hdop_text+","+pos_age_text+","+fix_type_text; +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino new file mode 100644 index 0000000..819ba94 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino @@ -0,0 +1,97 @@ +#include +#include "flight_state.hpp" + +servo_cmd steering; + +float setPoint_waypoint = 0; +float error_waypoint = 0; +float cmd_to_waypoint = 0; + +void setup() { + + Serial.begin(115200); + TLM_PORT.begin(57600); + + battery_setup(); + buzzer_setup(); + camera_setup(); + gps_setup(57600, GPS_FREQ, 2, 1, 0); // baud, Hz, mode, nmea, cog filter (0 = Off, 1 = On) + led_setup(); + rc_setup(); + servo_setup(); + position_setup(); + + getconfig(); + + watchdog.reset(); + + barometer_setup(); + navigation_setup(); + + buzzer_end_setup(); +} + +void loop() { + + watchdog.reset(); + + getdata(); // Getting data from all the sensors + datacmpt(); // Using this data to do all what we have to do + loop_count++; + + if ((millis()-tloop)>=1000) { + tloop = millis(); + loop_rate = loop_count; + loop_count = 0; + } +} + +void getdata() { + get_gps(); + get_baro(0); + get_rc(); + get_vbatt(); + +} + +void datacmpt() { + + if (new_cog) { + new_cog = false; + setPoint_waypoint = cmpt_setpoint(current_waypoint); + error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint); + cmd_to_waypoint = cmpt_cmd(error_waypoint); + } + + if (I_WANT_TO_FLY) { + cmd_to_waypoint = sim(); + Serial.println(cmd_to_waypoint); + cog_ok = true; + } + + steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok); + update_servo_cmd(steering, SERVO_RATE); + + cmpt_flight_state(); + cmpt_data_rate(flight_mode); + cmpt_fusion(); + cmpt_vertical_state(); + + if ((millis()-sd)>=delaySD) { + sd = millis(); + cmpt_string_data(flight_mode, initialised, deployed, wing_opened); + save_data(initialised); + } + + if (millis()-tlm>=delayTLM) { + tlm = millis(); + send_data(); + } + + buzzer_turn(flight_mode, setPoint_waypoint); + buzzer_batt(); + update_buzzer(); + updateled(); + updatecam(); + +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/navigation.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/navigation.hpp new file mode 100644 index 0000000..4b30737 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/navigation.hpp @@ -0,0 +1,67 @@ +#include "config.h" +#include "position.hpp" +#include "control.hpp" + +struct gps_location { + double latitude = 0; + double longitude = 0; + double radius = 0; +}; + +gps_location waypoint[17]; +gps_location current_waypoint; + +int waypoint_number = 0; +int last_waypoint_number = 0; + +float getangle(float a, float b) { + float angle = 0; + if (abs(a-b) < 180) { angle = (b-a);} + else { + if ((a-b) < 0) { angle = (-360) +(b-a);} + else { angle = (360) + (b-a);} + } + return angle; +} + +float cmpt_setpoint(gps_location waypoint) { + return TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),waypoint.latitude,waypoint.longitude); +} + +float cmpt_error(float cog, float setPoint) { + return getangle(cog, setPoint); +} + +void navigation() { + + if (NAV_WAYPOINT == true) { + + float distance_to = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),current_waypoint.latitude,current_waypoint.longitude); + + if (distance_to < current_waypoint.radius) { + + if (waypoint_number < 15) { + waypoint_number++; + } + + if ((waypoint[waypoint_number].latitude !=0) and (waypoint[waypoint_number].longitude !=0)) { + current_waypoint = waypoint[waypoint_number]; + } + } + } +} + +String control_text() { + String setPoint_Home_text = String(cmpt_setpoint(current_waypoint),2); + String errHome_text = String(cmpt_error(gps.course.deg(), cmpt_setpoint(current_waypoint)),2); + String lat_B_text = String(current_waypoint.latitude,5); + String lon_B_text = String(current_waypoint.longitude,5); + String waypoint_text = String(waypoint_number); + String waypoint_distance = String(TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),current_waypoint.latitude,current_waypoint.longitude)); + + return setPoint_Home_text+","+errHome_text+","+lat_B_text+","+lon_B_text+","+waypoint_text+","+waypoint_distance; +} + +String nav_text() { + return pos_text()+","+control_text(); +} diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp new file mode 100644 index 0000000..78e0e6a --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp @@ -0,0 +1,130 @@ +#include "config.h" +#include "barometer.hpp" +#include "gps.hpp" +#include + +float merged_alt = 0; +float merged_vspeed = 0; + +float baro_alt_weight = 1; +float gps_alt_weight = 1; +float baro_vspeed_weight = 1; +float gps_vspeed_weight = 1; + +float ground_altitude = 0; +unsigned long tstab = 0; + +float gps_stab_factor = 0; +float baro_stab_factor = 0; + +bool new_baro_fusion = false; +bool new_gps_fusion = false; + +movingAvg gps_v(GPS_SAFE_AVG); +movingAvg baro_v(BARO_SAFE_AVG); +movingAvg ps_p(PRE_PE_AVG); + +float pressure_percentage; + +void position_setup() { + gps_v.begin(); + baro_v.begin(); + ps_p.begin(); +} + +void cmpt_fusion() { + if (new_baro_fusion == true and new_gps_fusion == true) { + new_baro_fusion = false; + new_gps_fusion = false; + + pressure_percentage = (ps_p.reading((pressure_baro / (baro_set*100.0))*100.0)/100.0) ; + baro_alt_weight = pressure_percentage; + baro_vspeed_weight = pressure_percentage*pressure_percentage; + + double h_dop = gps.hdop.value(); + + if (gps_ok) { + gps_alt_weight = 50.0/h_dop; + gps_vspeed_weight = 1; + } + else { + gps_alt_weight = 0; + gps_vspeed_weight = 0; + } + + merged_alt = ((alt_baro*baro_alt_weight)+(gps.altitude.meters()*gps_alt_weight))/(baro_alt_weight+gps_alt_weight); + merged_vspeed = ((baro_vspeed*baro_vspeed_weight)+(gps_vspeed*gps_vspeed_weight))/(baro_vspeed_weight+gps_vspeed_weight); + + } +} + +bool cmpt_vertical_state() { + if (new_baro) { + new_baro = false; + new_baro_fusion = true; + baro_stab_factor = (baro_v.reading(baro_vspeed*100.0))/100.0; + } + if (new_gps) { + new_gps = false; + new_gps_fusion = true; + gps_stab_factor = (gps_v.reading(gps_vspeed*100))/100.0; + } +} + +bool is_ascent(int v_trigger, bool mode) { + if (mode == 0) { + if (baro_stab_factor>v_trigger) { + return true; + } + else { + return false; + } + } + else { + if (gps_stab_factor>v_trigger and baro_stab_factor>v_trigger) { + return true; + } + else { + return false; + } + } +} + +bool is_descent(int v_trigger, bool mode) { + if (mode == 0) { + if (baro_stab_factor +#include "config.h" + +SBUS rx(RX_PORT); +uint16_t channels[16]; +bool failsafe; +bool lostFrame; + +void rc_setup() { + rx.begin(); +} + +void get_rc() { + rx.read(&channels[0], &failsafe, &lostFrame); + + if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; } + else { failsafe = false; } +} + +String rc_text() { + String a_text = String(channels[0]); + String b_text = String(channels[1]); + String c_text = String(channels[2]); + String d_text = String(channels[3]); + String e_text = String(channels[4]); + String f_text = String(channels[5]); + String g_text = String(channels[6]); + + return a_text+","+b_text+","+c_text+","+d_text+","+e_text+","+f_text+","+g_text; +} + diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/servo.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/servo.hpp new file mode 100644 index 0000000..9b35257 --- /dev/null +++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/servo.hpp @@ -0,0 +1,188 @@ +#include "config.h" +#include + +PWMServo steer; +PWMServo deploy; +PWMServo left; +PWMServo right; +PWMServo esc; + +int servo_left = 0; +int servo_right = 0; +int servo_aux = 0; + +unsigned long tpwm = 0; + +struct servo_cmd { + float left = 1500; + float right = 1500; + float aux = 1500; +}; + +void servo_setup() { + left.attach(6, 1000, 2000); + right.attach(7, 1000, 2000); + if (DROP == true) { deploy.attach(8, 1000, 2000); } + else { esc.attach(8, 1000, 2000); } +} + +servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool deployed, bool failsafe, bool cog_ok) { + + float roll = map(channels[0], 67, 1982, 1000, 2000); + float pitch = map(channels[1], 67, 1982, 1000, 2000); + float aux = map(channels[2], 67, 1982, 1000, 2000); + float sw = map(channels[6], 67, 1982, 1000, 2000); + + roll = constrain(roll, 1000, 2000); + pitch = constrain(pitch, 1000, 2000); + aux = constrain(aux, 1000, 2000); + sw = constrain(sw, 1000, 2000); + + servo_cmd steering_cmpt ; + + switch(flight_mode) { + case 0: + case 1: + case 6: + case 8: + // ---------- Stage 1 - RC mode ---------- // + switch (RC_MODE) { + + case 0: // roll only + steering_cmpt.right = roll; + steering_cmpt.left = 3000-roll; + break; + + case 1: // pitch and roll mixed + steering_cmpt.right = roll; + steering_cmpt.left = 3000-roll; + steering_cmpt.left = steering_cmpt.left+(pitch-1500); + steering_cmpt.right = steering_cmpt.right+(pitch-1500); + break; + + case 2: // pitch and roll separated + steering_cmpt.left = roll; + steering_cmpt.right = pitch; + break; + } + if (failsafe) { + steering_cmpt.left = 1500; + steering_cmpt.right = 1500; + } + break; + + case 9: + case 10: + case 5: + if (cog_ok) { + steering_cmpt.right = autopilot; + steering_cmpt.left = 3000-autopilot; + } + else { + steering_cmpt.right = 1500; + steering_cmpt.left = 1500; + } + break; + + case 11: + steering_cmpt.right = 1500; + steering_cmpt.left = 1500; + break; + + } + + // ---------- Stage 2 - Linear mode ---------- // + switch (LINEAR_MODE) { + + case 0: // control is fully linear + break; + + case 1: // control start at servo_start with an offset + if (steering_cmpt.left>=(1500+TRIG)) { + steering_cmpt.left = map(steering_cmpt.left, 1500, 2000, 1500+LEFT_OFFSET, SERVO_MAX_M); + } + else if (steering_cmpt.left<=(1500-TRIG)) { + steering_cmpt.left = map(steering_cmpt.left, 1000, 1500, 1000, 1500-LEFT_OFFSET); + } + if (steering_cmpt.right>=(1500+TRIG)) { + steering_cmpt.right = map(steering_cmpt.right, 1500, 2000, 1500+RIGHT_OFFSET, SERVO_MAX_M); + } + else if (steering_cmpt.right<=(1500-TRIG)) { + steering_cmpt.right = map(steering_cmpt.right, 1000, 1500, 1000, 1500-RIGHT_OFFSET); + } + break; + } + + // ---------- Stage 3 - Control mode ---------- // + switch (CONTROL_MODE) { + + case 0: // neutral is center + break; + + case 1: // neutral is hands up + steering_cmpt.right = constrain(map(steering_cmpt.right, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C); + steering_cmpt.left = constrain(map(steering_cmpt.left, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C); + break; + } + +// -------------------------- Deployment Servo and ESC -------------------------- // + + switch(flight_mode) { + + case 0: + case 1: + case 2: + case 3: + case 4: + case 5: + case 6: + case 7: + if (deployed == true) { + steering_cmpt.aux = 1000; + } + else { + if (failsafe == true) { + steering_cmpt.aux = 2000; + } + else { steering_cmpt.aux = sw; } + } + break; + + case 8: + steering_cmpt.aux = aux; + break; + + case 9: + case 10: + steering_cmpt.aux = 1000; + break; + + case 11: + steering_cmpt.aux = 1000; + break; + } + + servo_left = steering_cmpt.left; + servo_right = steering_cmpt.right; + servo_aux = steering_cmpt.aux; + + return steering_cmpt; +} + +void update_servo_cmd(servo_cmd steering_apply, int a) { + + if ((millis()-tpwm)>=(1000/a)) { + tpwm = millis(); + left.write(map(steering_apply.left, 1000, 2000, 0, 180)); + right.write(map(steering_apply.right, 1000, 2000, 0, 180)); + deploy.write(map(steering_apply.aux, 1000, 2000, 0, 180)); + } + +} + +String servo_text() { + String left_text = String(servo_left); + String right_text = String(servo_right); + String aux_text = String(servo_aux); + return left_text+","+right_text+","+aux_text; +}