Update with new debug mode (make sure to disable it before flight!!)

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp

@@ -54,6 +54,7 @@ void barometer_setup() {
   Adafruit_BMP280::FILTER_X16,      
   Adafruit_BMP280::STANDBY_MS_1);
   baroset(0, 1);  
+  if (DEBUG) { Serial.println("Baro was set correctly"); } 
 }
 
 void cmpt_vertical_speed_baro(float da, int dt) {  
@@ -64,6 +65,7 @@ void cmpt_vertical_speed_baro(float da, int dt) {
 
 void get_baro(int mode) {
   if ((millis()-baroA)>=10 and mode == 0) { 
+    if (DEBUG) { Serial.println("Got a Baro message"); } 
     
     baroA = millis();
     unsigned waitd = millis(); 

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp

@@ -13,6 +13,7 @@ int cells = 0;
 void battery_setup() {
   pinMode(vpin, INPUT); 
   voltage.begin();
+  if (DEBUG) { Serial.println("RC was set correctly"); } 
 }
 
 void get_vbatt() {

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp

@@ -34,6 +34,7 @@ void setcam(int a, int b, int c) {
     break;    
   }
   camReady = true; 
+  if (DEBUG) { Serial.println("Camera was set correctly"); } 
 }
 
 void updatecam() {

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h

@@ -16,7 +16,8 @@
 #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 TLM_MONITOR     false // Show telemetry on serial monitor 
+#define DEBUG           true  // Will show all the steps on the screen
 #define GPS_PORT Serial7
 #define TLM_PORT Serial5 
 #define RX_PORT  Serial1

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp

@@ -14,6 +14,7 @@ void navigation_setup() {
   myPID.SetTunings(NKP, NKI, NKD);
   myPID.SetOutputLimits(-180, 180);
   myPID.SetMode(MANUAL);
+  if (DEBUG) { Serial.println("PIDs set correctly"); } 
 }
 
 float cmpt_cmd(float err) {      

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp

@@ -56,6 +56,7 @@ void cmpt_data_rate(int flight_mode) {
 void send_data() { 
   packet_count = (packet_count +1); 
   TLM_PORT.println(mainTLM); 
+  if (DEBUG) { Serial.println("Sending Data"); }
   if (TLM_MONITOR) {  
     Serial.println(mainTLM);  
   }
@@ -64,6 +65,7 @@ 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();
@@ -99,9 +101,12 @@ void newfile() {
   dtostrf(time_number, 1, 0, sdnamebuff); 
   sprintf(namebuff, "%s.txt", sdnamebuff);
   sprintf(nameconfig, "config_%s.txt", sdnamebuff);
+
+  if (DEBUG) { Serial.println("Trying to create a new LOG file"); } 
     
-  if (!SD.begin(chipSelect)) { sd_ok = false; }
+  if (!SD.begin(chipSelect)) { sd_ok = false;  if (DEBUG) { Serial.println("Failure"); } }
   else { 
+    if (DEBUG) { Serial.println("Success"); }
     sd_ok = true; 
     SdFile::dateTimeCallback(dateTime);
     dataFile = SD.open(namebuff, FILE_WRITE);
@@ -129,8 +134,10 @@ void getconfig() {
   if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
   else {
     sd_ok = true; 
+    if (DEBUG) { Serial.println("Trying to read the file"); }
     File configFile = SD.open("config.txt", FILE_READ);
     if (configFile) {
+      if (DEBUG) { Serial.println("Success"); } 
 
       // Reading the baro adress 
       String memory = ""; 
@@ -170,6 +177,7 @@ void getconfig() {
       configFile.close();
     }
     else { 
+      if (DEBUG) { Serial.println("Failure"); }
       delay(1500) ;
     }
   }  

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp

@@ -49,6 +49,7 @@ void buzzer_setup() {
   tone(BUZZER_PIN, 762); 
   delay(200); 
   noTone(BUZZER_PIN);
+  if (DEBUG) { Serial.println("Buzzer was set correctly"); } 
 }
 
 void buzzer_end_setup() {
@@ -169,6 +170,7 @@ void led_setup() {
   strip.begin();           
   strip.show();            
   strip.setBrightness(255);
+  if (DEBUG) { Serial.println("LED was set correctly"); } 
 }
 
 void colorWipe(uint32_t color, int wait) {

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp

@@ -27,6 +27,7 @@ void cmpt_vertical_speed_gps(float da, int dt) {
 void get_gps() {  
   
   while (GPS_PORT.available()) { 
+    if (DEBUG) { Serial.println("Got a GPS message"); } 
     gps.encode(GPS_PORT.read()); 
   }
   
@@ -135,6 +136,7 @@ if (e == 1) {
   }
 
   GPS_PORT.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead));
+  if (DEBUG) { Serial.println("GPS was set correctly"); } 
    
 }
 

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino

@@ -10,8 +10,13 @@ float cmd_to_waypoint   = 0;
 void setup() {
 
   Serial.begin(115200);
+
+  if (DEBUG) { Serial.println("Just barely waking up"); } 
+  
   TLM_PORT.begin(57600); 
 
+  if (DEBUG) { Serial.println("Ready to initialise all the hardware"); } 
+
   battery_setup(); 
   buzzer_setup(); 
   camera_setup();
@@ -43,7 +48,12 @@ void loop() {
     tloop = millis(); 
     loop_rate = loop_count;
     loop_count = 0; 
-  } 
+  }
+
+  if (DEBUG) {
+    delay(10); 
+  }
+  
 }
 
 void getdata() { 
@@ -57,7 +67,8 @@ void getdata() {
 void datacmpt() {
   
   if (new_cog) {
-    new_cog = false; 
+    new_cog = false;
+    if (DEBUG) { Serial.println("New direction and command computed"); }  
     setPoint_waypoint = cmpt_setpoint(current_waypoint); 
     error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
     cmd_to_waypoint   = cmpt_cmd(error_waypoint);

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp

@@ -8,13 +8,15 @@ bool lostFrame;
 
 void rc_setup() {
   rx.begin();
+  if (DEBUG) { Serial.println("RC was set correctly"); } 
 }
 
 void get_rc() {
   rx.read(&channels[0], &failsafe, &lostFrame);
   
   if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
-  else { failsafe = false; }  
+  else { failsafe = false; } 
+  if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }  
 }
 
 String rc_text() {