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 index 39bf175..032a432 100644 --- 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 @@ -112,22 +112,11 @@ void get_baro(int mode) { 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; - } - } -} + new_baro = true; + } +} + String baro_text() { String alt_baro_text = String(alt_baro, 3); 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 index 395f30b..1e585b7 100644 --- 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 @@ -61,14 +61,12 @@ void send_data() { } } -void save_data(bool initialised) { - if (initialised == true) { - if (sd_ok == true and SD_WRITING == true) { - if (DEBUG) { Serial.println("Saving Data"); } - dataFile = SD.open(namebuff, FILE_WRITE); - dataFile.println(mainSD); - dataFile.close(); - } +void save_data() { + if (sd_ok == true and SD_WRITING == true) { + if (DEBUG) { Serial.println("Saving Data"); } + dataFile = SD.open(namebuff, FILE_WRITE); + dataFile.println(mainSD); + dataFile.close(); } } @@ -91,8 +89,10 @@ void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool win 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+","+spiral_text+","+failsafe_text+","+vbatt_text+","+loop_rate_text; - - mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text()+","+servo_max_cmd_text(); + + if (initialised) { + mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text()+","+servo_max_cmd_text(); + } mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*"; } @@ -111,7 +111,7 @@ void newfile() { 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), Spiral (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), Pressure (hpa), 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), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C"); + dataFile.println("time (ms), Packet_Count (text), Mode (text), Initialised (text), Deployed (text), Wing_Opened (text), GPS_Ok (text), COG_Ok (text), Spiral (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), Pressure (hpa), Baro_Vspeed (m/s), Altitude (m), Merged_Vspeed (m/s), 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), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C"); dataFile.close(); } if (CONFIG_FILE_SV) { 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 index 75efff2..cc22291 100644 --- 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 @@ -39,13 +39,13 @@ void flight_init() { current_waypoint = waypoint[waypoint_number]; } - baroset(gps.altitude.meters(), 1); - get_baro(1); - cmpt_fusion(); - b_vs.reset(); b_al.reset(); g_vs.reset(); + + baroset(gps.altitude.meters(), 1); + get_baro(1); + merged_alt = gps.altitude.meters(); cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral); @@ -81,7 +81,7 @@ void ready_steady() { setcam(1, 20, 600); } - if (is_descent(v_down(VDOWN), 0)) { + if (is_descent(v_down(VDOWN), 1)) { flight_mode = 3; EasyBuzzer.beep(3000,100,50,3,500,1); setcam(1, 60, 600); @@ -159,7 +159,7 @@ void flight_gliding_auto() { myPID.SetMode(MANUAL); } - if (is_descent(v_down(-5), 0)) { + if (is_descent(v_down(-5), 1)) { spiral = true; spiral_time = millis(); } @@ -244,9 +244,9 @@ void flight_gliding_no_gps() { void cmpt_flight_state() { - if (flight_mode!=prev_flight_mode) { + if (flight_mode!=prev_flight_mode and prev_flight_mode !=0) { cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral); - save_data(initialised); + save_data(); send_data(); prev_flight_mode = flight_mode; } 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 index 20c9e1d..d4631fb 100644 --- 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 @@ -52,6 +52,8 @@ void get_gps() { new_gps = true; } } + + void sendPacket(byte *packet, byte len){ for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); } 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 index 37197cf..ce88229 100644 --- 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 @@ -92,15 +92,18 @@ void datacmpt() { cmpt_flight_state(); cmpt_data_rate(flight_mode); + if (initialised) { + cmpt_vertical_state(); cmpt_fusion(); } - cmpt_vertical_state(); - + if ((millis()-sd)>=delaySD) { sd = millis(); cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral); - save_data(initialised); + if (initialised) { + save_data(); + } } if (millis()-tlm>=delayTLM) { 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 index ca3da8e..8c1fbd5 100644 --- 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 @@ -46,8 +46,8 @@ void cmpt_fusion() { double h_dop = gps.hdop.value(); if (gps_ok) { - gps_alt_weight = 50.0/h_dop; - gps_vspeed_weight = 1; + gps_alt_weight = constrain((gps.altitude.meters()/3000), 0.1, 10); + gps_vspeed_weight = constrain((gps.altitude.meters()/6000), 0.05, 10); } else { gps_alt_weight = 0; @@ -55,7 +55,7 @@ void cmpt_fusion() { } 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); + merged_vspeed = ((baro_stab_factor*baro_vspeed_weight)+(gps_stab_factor*gps_vspeed_weight))/(baro_vspeed_weight+gps_vspeed_weight); } } @@ -64,7 +64,7 @@ void 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; + baro_stab_factor = (baro_v.reading(baro_vspeed*100.0))/100.0; } if (new_gps) { new_gps = false; @@ -102,7 +102,7 @@ bool is_descent(int v_trigger, bool mode) { } } else { - if (gps_stab_factor