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 35c373d..0b988c2 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
@@ -5,7 +5,7 @@
 
 Watchdog watchdog; 
 
-Adafruit_BMP280 bmp(&Wire);
+Adafruit_BMP280 bmp(&BARO_PORT);
 int baro_adress = 0x00; 
 double baro_set  = 1000.0; 
 
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/cmd_cmpt.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/cmd_cmpt.hpp
new file mode 100644
index 0000000..f7b3f96
--- /dev/null
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/cmd_cmpt.hpp
@@ -0,0 +1,15 @@
+// This file is used as an "add on", the only function defined here should be "gps_location where to go(float gps_latitude, float gps_longitude)"
+// This function takes in parameter the current position, and should return a waypoint goal, which of course should be choosen based on the current position. 
+
+#include <PID_v1.h>
+#include "config.h" 
+
+double Setpoint, Input, Output;
+PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
+
+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/config.h b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h
index adfa76c..7bb50a6 100644
--- 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
@@ -6,22 +6,24 @@
 #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_WAYPOINT    false // Doing the waypoint sequence before reaching home? 
 #define NAV_HOME        true  // 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 CONFIG_FILE_SV  false // Will also save the config file with the name of the file to debug if the config was wrong 
-#define LED_MODEL       0     // Big led = 1, small led = 0. 
-#define CAM_CYCLE       true  // If true, camera will only be powered on for 20s every 10min, used to save battery on long HAB flight 
+#define LED_MODEL       1     // 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 DEBUG           false // Will show all the steps on the screen
+#define X_SERVO         false // Is an additional servo used for separation from balloon or D servo is used with two positions?
 #define GPS_PORT Serial7
 #define TLM_PORT Serial5 
 #define RX_PORT  Serial1
+#define BARO_PORT Wire
 #define TIME_OUT 300
 
 //---------- FLIGHT SETTINGS ----------// 
@@ -29,28 +31,32 @@
 #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 DROP            false  // R2Home's version, drop or motorised
+#define LAUNCH_SPEED    0  // m/s, in motorised version needed horizontal speed to go into controled mode 
+ 
 #define DEP_MODE        1     // If 0, the ALTITUDE will be used if 1, the TIMER will be used, to trigger deployment once in descent mode
-#define DESCENT_TIMER   4000  
-#define OPENING_TIMER   3000
-#define SPIRAL_RECOVER  5000
-#define DEP_ALT         300   // m above ground altitude 
-#define SEP_ALT         10    // m above sea level
-#define VUP             1     // m/s 
-#define VDOWN           -1    // m/s
+#define DESCENT_TIMER   500  
+#define OPENING_TIMER   5000
+#define SPIRAL_RECOVER  1000
+#define DEP_ALT         1000   // m above ground altitude 
+#define ALT_MODE        1     // If 1, sep altitude is above ground, if 0, sep altitude is above sea level 
+#define SEP_ALT         100   // m above sea level
+#define VUP             3     // m/s 
+#define VDOWN           -2    // m/s
 
-// PID for navigation, better to not touch them and touch the servo max command settings instead 
-#define NKP   1
-#define NKI   0.05
-#define NKD   0.1
+
+
+// PID for navigation
+#define NKP   3
+#define NKI   0.2
+#define NKD   1
 
 #define BCRITICAL 3.4
 #define BLOW      3.5
 #define NO_BATT   4.0 
 
-#define AUTO_GAIN_WEIGHT    true  // Servo max command will be set automatically at initialization based on given payload weight 
-#define AUTO_GAIN_PRESSURE  true  // Servo max command will be set automatically during the flight based on the ratio between ground pressure and current pressure 
+#define AUTO_GAIN_WEIGHT    false  // Servo max command will be set automatically at initialization based on given payload weight 
+#define AUTO_GAIN_PRESSURE  false  // Servo max command will be set automatically during the flight based on the ratio between ground pressure and current pressure 
 
 #define SYSTEM_WEIGHT  500 // System weight in gram
 #define PAYLOAD_WEIGHT 500 // Payload weight in gram 
@@ -73,7 +79,7 @@
 
 #define GPS_VS_AVG  1
 
-#define GPS_SAFE_AVG  10
-#define BARO_SAFE_AVG 5
+#define GPS_SAFE_AVG  5
+#define BARO_SAFE_AVG 1
 
 #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
index c5b913c..d08d6a8 100644
--- 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
@@ -1,5 +1,6 @@
 #include <PID_v1.h>
 #include "config.h" 
+#include "cmd_cmpt.hpp"
 
 int SERVO_MAX_M_W = 0; 
 int SERVO_MAX_C_W = 0; 
@@ -7,9 +8,6 @@ int SERVO_MAX_C_W = 0;
 int SERVO_MAX_M = 0; 
 int SERVO_MAX_C = 0; 
 
-double Setpoint, Input, Output;
-PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
-
 unsigned long time_gain = 0; 
 
 void navigation_setup() {
@@ -19,13 +17,6 @@ void navigation_setup() {
   if (DEBUG) { Serial.println("PIDs set correctly"); } 
 }
 
-float cmpt_cmd(float err) {      
-  Input = -err; 
-  Setpoint = 0; 
-  myPID.Compute();
-  return map(Output, -180, 180, 1000, 2000); 
-}
-
 void cmpt_weight_gain() {
   if (AUTO_GAIN_WEIGHT) {
     int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
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 8161f2e..5f3007a 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
@@ -1,7 +1,6 @@
 #include <SD.h>
 #include "feedback.hpp"
 
-
 bool sd_ok = false; 
 unsigned int delaySD = 100;    // Datalog 
 unsigned int delayTLM = 1000;   // Tlm 
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
index 4f3007f..3dc3ffc 100644
--- 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
@@ -163,6 +163,9 @@ void buzzer_batt() {
 void update_buzzer() {
   sweep_beep_update();
   EasyBuzzer.update();
+  if (arming_error) {
+     EasyBuzzer.singleBeep(3000,200);
+  }
 }
 
 void led_setup() { 
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 97e5274..4b940d3 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
@@ -14,6 +14,10 @@ bool separation = false;
 unsigned long spiral_time = 0; 
 unsigned long init_time = 0; 
 
+float setPoint_waypoint = 0; 
+float error_waypoint    = 0; 
+float cmd_to_waypoint   = 0;
+
 
 //------------------- 0 -------------------//
 
@@ -54,13 +58,22 @@ void flight_init() {
     ground_altitude = constrain(ground_altitude, 0, 2000); 
     
     dep_altitude = (DEP_ALT+ground_altitude);
-    sep_altitude = SEP_ALT; 
+
+    if (ALT_MODE) {
+      sep_altitude = (SEP_ALT+ground_altitude); 
+    }
+    else {
+      sep_altitude = SEP_ALT; 
+    }
   
     setcam(1, 60, 60); 
     newfile();
 
     if (DROP == true) { flight_mode = 1;}
-    else { flight_mode = 8; }
+    else { 
+      myPID.SetMode(MANUAL); 
+      flight_mode = 7; 
+    }
 
     init_time = millis(); 
     initialised = true;      
@@ -105,15 +118,19 @@ void ready_steady() {
 //------------------- 2 -------------------//
 
 void flight_ascent() { 
+  
   if (is_descent(0, 0)) {
     flight_mode = 1;
-  }  
+  }
+    
   if ((gps.altitude.meters()-ground_altitude)>10) {
     flight_started = true; 
   }
+  
   if (gps.altitude.meters()>sep_altitude and gps_ok) {
     separation = true;
   }
+  
 }
 
 //------------------- 3 -------------------//
@@ -163,12 +180,14 @@ void flight_gliding_auto() {
   }
 
   if (is_descent(v_down(-5), 1)) {
+    myPID.SetMode(MANUAL);
     spiral = true; 
     spiral_time = millis();
   }
 
   if (millis()-spiral_time>SPIRAL_RECOVER) {
     spiral = false;
+    myPID.SetMode(AUTOMATIC);
   }
   
   if (failsafe == false) {
@@ -179,8 +198,15 @@ void flight_gliding_auto() {
   if (I_WANT_TO_FLY == true) { 
     flight_mode = 5;
   } 
+
+  if (new_cog) {
+    new_cog = false;
+    if (DEBUG) { Serial.println("New direction and command computed"); }  
+    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
+    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
+    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
+  }
   
-  navigation(); 
 }
 
 //------------------- 6 -------------------//
@@ -194,7 +220,10 @@ void flight_gliding_manual() {
 
 //------------------- 7 -------------------//
 
-void landed() {  
+void on_ground() {  
+  if (gps.speed.mps()>=LAUNCH_SPEED) {
+    flight_mode = 8; 
+  }
 }
 
 //------------------- 8 -------------------//
@@ -216,7 +245,14 @@ void motorised_man() {
 
 void motorised_auto() { 
   
-  navigation(); 
+  if (new_cog) {
+    new_cog = false;
+    if (DEBUG) { Serial.println("New direction and command computed"); }  
+    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
+    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
+    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
+  }
+  
   if (channels[6] < 1000) { 
     flight_mode = 8; 
   }
@@ -226,12 +262,38 @@ void motorised_auto() {
 
 void motorised_failSafe() { 
   
-  navigation();
+  if (new_cog) {
+    new_cog = false;
+    if (DEBUG) { Serial.println("New direction and command computed"); }  
+    setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng())); 
+    error_waypoint    = cmpt_error(gps.course.deg(), setPoint_waypoint);
+    cmd_to_waypoint   = cmpt_cmd(error_waypoint);
+  }
    
   if (failsafe == false) { 
     flight_mode = 8; 
     myPID.SetMode(MANUAL); 
   }
+  
+
+  /* DANGEROUS: WOULD SET MOTOR TO MAX!!!
+  if (!gps_ok) {
+    flight_mode = 11;  
+    myPID.SetMode(MANUAL);
+  }
+  */
+
+  if (is_descent(v_down(-5), 1)) {
+    myPID.SetMode(MANUAL);
+    spiral = true; 
+    spiral_time = millis();
+  }
+
+  if (millis()-spiral_time>SPIRAL_RECOVER) {
+    spiral = false;
+    myPID.SetMode(AUTOMATIC);
+  }
+  
 }
 
 //------------------- 11 -------------------//
@@ -290,7 +352,7 @@ void cmpt_flight_state() {
     break;
 
     case 7: 
-      landed(); 
+      on_ground(); 
       setled(128, 0, 255, 25, 1000); 
     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
index 5a41c56..ead0b67 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
@@ -54,8 +54,6 @@ void get_gps() {
   }
 }
 
-
- 
 void sendPacket(byte *packet, byte len){
     for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); }
 }
@@ -161,7 +159,6 @@ String date_time() {
   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);
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 0bb5d95..4fa11cd 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
@@ -7,9 +7,6 @@
 
 servo_cmd steering; 
 
-float setPoint_waypoint = 0; 
-float error_waypoint    = 0; 
-float cmd_to_waypoint   = 0;
 
 void setup() {
 
@@ -68,7 +65,7 @@ void loop() {
 
 void getdata() { 
  get_gps(); 
- get_baro(0); 
+ get_baro(0);
  get_rc(); 
  get_vbatt(); 
  
@@ -76,15 +73,7 @@ void getdata() {
 
 void datacmpt() {
 
-  cmpt_pressure_gain(pressure_sqrt_ratio()); 
-  
-  if (new_cog) {
-    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);
-  }
+  cmpt_pressure_gain(pressure_sqrt_ratio());  
 
   if (I_WANT_TO_FLY) {
     cmd_to_waypoint = sim(); 
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
index b424b44..3237c65 100644
--- 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
@@ -1,17 +1,8 @@
 #include "config.h"
 #include "position.hpp"
 #include "servo.hpp"
+#include "where_to_go.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) { 
@@ -24,36 +15,17 @@ float getangle(float a, float b) {
   return angle;    
 }
 
-float cmpt_setpoint(gps_location waypoint) {                                                
-  return TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),waypoint.latitude,waypoint.longitude);                                      
+float cmpt_setpoint(float gps_latitude, float gps_longitude, gps_location waypoint) {                                                
+  return TinyGPSPlus::courseTo(gps_latitude, gps_longitude, 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 setPoint_Home_text = String(cmpt_setpoint(gps.location.lat(), gps.location.lng(), current_waypoint),2);
+  String errHome_text = String(cmpt_error(gps.course.deg(), cmpt_setpoint(gps.location.lat(), gps.location.lng(), 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); 
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 b437928..82aa816 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
@@ -120,7 +120,6 @@ float v_down(int vdown) {
   return pressure_sqrt_ratio()*vdown;
 }
 
-
 String pos_text() {
   String merged_alt_text = String(merged_alt,3);  
   String gps_weight_text = String(gps_alt_weight,2);
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp
index b0ae1ce..e8f594a 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp
@@ -6,17 +6,29 @@ uint16_t channels[16];
 bool failsafe;
 bool lostFrame;
 
+unsigned long last_rc(0); 
+
 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; } 
-  if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }  
+     
+    rx.read(&channels[0], &failsafe, &lostFrame);    
+    if (channels[3]==0) { failsafe = true; }
+    //else { failsafe = false; } 
+    
+    if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }  
+
+    /* 
+    Serial.print(channels[0]); Serial.print(","); 
+    Serial.print(channels[1]); Serial.print(","); 
+    Serial.print(channels[2]); Serial.print(","); 
+    Serial.print(channels[4]); Serial.print(",");
+    Serial.print(channels[5]); Serial.print(",");
+    Serial.println(channels[6]);
+    */
 }
 
 String rc_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
index 9bb9921..5677537 100644
--- 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
@@ -1,9 +1,10 @@
 #include "config.h"
 #include "control.hpp"
 #include <PWMServo.h>
+#include <movingAvg.h>
 
 PWMServo steer;
-PWMServo deploy;
+PWMServo aux;
 PWMServo aux_deploy;
 PWMServo left; 
 PWMServo right; 
@@ -14,6 +15,9 @@ int servo_right = 0;
 int servo_aux   = 0; 
 int servo_aux_deploy = 0; 
 
+bool armed = false;
+bool arming_error = false;
+
 unsigned long tpwm = 0; 
  
 struct servo_cmd {
@@ -26,17 +30,17 @@ struct servo_cmd {
 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);    }
+  aux.attach(8, 1000, 2000); 
   aux_deploy.attach(9, 1000, 2000); 
+
 }
 
 servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool deployed, bool failsafe, bool cog_ok, bool spiral, bool separation) {
   
-  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);
+  float roll  = map(channels[0], 172, 1811, 1000, 2000);
+  float pitch = map(channels[1], 172, 1811, 1000, 2000);
+  float aux =   map(channels[2], 365, 1681, 1000, 2000);
+  float sw =    map(channels[6], 172, 1811, 1000, 2000);
   
   roll  = constrain(roll, 1000, 2000); 
   pitch = constrain(pitch, 1000, 2000); 
@@ -49,6 +53,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
     case 0: 
     case 1: 
     case 6: 
+    case 7: 
     case 8: 
     // ---------- Stage 1 - RC mode ---------- // 
     switch (RC_MODE) {
@@ -93,6 +98,11 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
     steering_cmpt.right = 1500; 
     steering_cmpt.left = 1500; 
     break; 
+
+    case 4: 
+    steering_cmpt.right = 2000; 
+    steering_cmpt.left = 2000; 
+    break;
     
   }
 
@@ -138,10 +148,9 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
     case 1:
     case 2:
     case 3:
-    case 4:
     case 5: 
-    case 6:
-    case 7: 
+    case 4:
+    case 6: 
       // Deployment Servo
       if (deployed == true) { 
         steering_cmpt.aux = 1000; 
@@ -160,8 +169,15 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
       else {
         steering_cmpt.aux_deploy = 2000;
       }
+
+      if (!X_SERVO) {
+        steering_cmpt.aux_deploy = map(steering_cmpt.aux_deploy, 1000, 2000, 1500, 2000);
+        steering_cmpt.aux_deploy = constrain(steering_cmpt.aux_deploy, 1500, 2000); 
+        steering_cmpt.aux = min(steering_cmpt.aux, steering_cmpt.aux_deploy); 
+      }
     break; 
 
+    case 7: 
     case 8:
       steering_cmpt.aux = aux;
     break; 
@@ -172,10 +188,40 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
      break; 
 
      case 11:
-      steering_cmpt.aux = 1000;
+      steering_cmpt.aux = 2000;
       steering_cmpt.aux_deploy = 2000;
      break;  
   } 
+
+  if (!DROP) {
+    switch (armed) {
+      case true:
+      if (channels[4] < 1000) {
+        armed = false;
+        arming_error = false;
+      }
+      break;
+  
+      case false: 
+      if (channels[4] > 1000 and aux >1001) {
+        arming_error = true; 
+      }
+      else if (channels[4] > 1000 and aux <=1001 and !arming_error and flight_mode !=0) {
+        armed = true; 
+        arming_error = false;
+      }
+      else if (channels[4] > 1000 and flight_mode ==0) {
+        arming_error = true;
+      }
+      if (channels[4] < 1000) {
+        arming_error = false; 
+      }
+    }
+  
+    if (!armed and !DROP) {
+      steering_cmpt.aux = 1000;
+    }
+  }
   
   servo_left = steering_cmpt.left; 
   servo_right = steering_cmpt.right; 
@@ -191,7 +237,7 @@ void update_servo_cmd(servo_cmd steering_apply, unsigned int 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));
+    aux.write(map(steering_apply.aux, 1000, 2000, 0, 180));
     aux_deploy.write(map(steering_apply.aux_deploy, 1000, 2000, 0, 180));
   }
  
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/where_to_go.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/where_to_go.hpp
new file mode 100644
index 0000000..67e9de8
--- /dev/null
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/where_to_go.hpp
@@ -0,0 +1,39 @@
+// This file is used as an "add on", the only function defined here should be "gps_location where to go(float gps_latitude, float gps_longitude)"
+// This function takes in parameter the current position, and should return a waypoint goal, which of course should be choosen based on the current position. 
+
+#include "config.h"
+#include <TinyGPS++.h>
+
+struct gps_location { 
+  double latitude = 0; 
+  double longitude = 0; 
+  double radius = 0; 
+};
+
+gps_location waypoint[17]; 
+gps_location current_waypoint; 
+
+int waypoint_number = 0; 
+
+gps_location where_to_go(float gps_latitude, float gps_longitude) {
+   
+  if (NAV_WAYPOINT == true) {
+    
+    float distance_to = TinyGPSPlus::distanceBetween(gps_latitude, gps_longitude, current_waypoint.latitude, current_waypoint.longitude);
+     
+      if (waypoint_number < 15) { 
+        
+        if (distance_to < current_waypoint.radius) { 
+           
+        if ((waypoint[waypoint_number+1].latitude !=0) and (waypoint[waypoint_number+1].longitude !=0)) {
+          waypoint_number++;
+          current_waypoint = waypoint[waypoint_number]; 
+         
+        }  
+      }  
+    }  
+  }
+
+  return current_waypoint; 
+
+}
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO/R2Home_SERVO.ino b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO/R2Home_SERVO.ino
index 832fc74..93a03ca 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO/R2Home_SERVO.ino
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO/R2Home_SERVO.ino
@@ -24,7 +24,7 @@ unsigned long tread     = 0;
 unsigned long tchange   = 0; 
 unsigned long tparallax = 0; 
 
-float Kp        = 0.75;
+float Kp        = 0.6;
 float Kd        = 0; 
 float lasterror = 0;  
 
@@ -103,7 +103,7 @@ void updatecmd(int a) {
 
       // ------------------------------------------------------------------------------------ // 
 
-      parallax_steer_b = map(parallax_steer_a, 1000, 2000, 1100, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)" 
+      parallax_steer_b = map(parallax_steer_a, 1000, 2000, 1150, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)" 
 
       Serial.print(parallax_steer_b); Serial.print(","); Serial.println(feedback_value);