diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
index 1f64131a9a..837e2d241c 100644
--- a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
@@ -11,10 +11,12 @@
 <airframe name="Quadrotor LisaMX_2.1 pwm">
 
   <firmware name="rotorcraft">
-    <target name="ap" board="lisa_mx_2.1">
+    <target name="ap" board="lisa_mx_2.1_chibios">
       <module name="radio_control" type="sbus">
         <configure name="SBUS_PORT" value="UART5"/>
       </module>
+      <configure name="HAS_LUFTBOOT" value="1"/>
+      <configure name="FLASH_MODE" value="DFU"/>
     </target>
 
     <configure name="PERIODIC_FREQUENCY" value="160"/>
@@ -34,12 +36,12 @@
 
     <module name="motor_mixing"/>
     <module name="actuators"     type="pwm">
-      <define name="SERVO_HZ" value="PERIODIC_FREQUENCY"/>
+      <define name="SERVO_HZ" value="400"/>
     </module>
 
     <module name="telemetry"   type="transparent">
       <configure name="MODEM_PORT"        value="UART3"/>
-      <configure name="MODEM_BAUD"        value="B57600"/>
+      <configure name="MODEM_BAUD"        value="B115200"/>
     </module>
 
     <module name="ins"       type="vectornav">
@@ -62,12 +64,12 @@
     <module name="lidar_lite">
       <configure name="LIDAR_LITE_I2C_DEV" value="i2c2"/>
     </module>
-    <module name="battery_monitor.xml">
+    <!--module name="battery_monitor.xml">
       <define name="BATMON_CURRENT_OFFSET" value="-120"/>
       <define name="BATMON_CURRENT_SENSITIVITY" value="25.6"/>
       <define name="BATMON_TEMP_OFFSET" value="250"/>
       <define name="BATMON_TEMP_SENSITIVITY" value="10"/>
-    </module>
+    </module-->
   </modules>
 
     <servos driver="Pwm">
diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml
index 4a470aeaa0..28794ae253 100644
--- a/conf/airframes/AGGIEAIR/aggieair_conf.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml
@@ -7,7 +7,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/battery_monitor.xml modules/lidar_lite.xml modules/gps.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
+   settings_modules="modules/lidar_lite.xml modules/gps.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
    gui_color="#ffff954c0000"
   />
   <aircraft
@@ -51,7 +51,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
    flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
    settings="settings/fixedwing_basic.xml settings/nps.xml"
-   settings_modules="modules/battery_monitor.xml modules/lidar_sf11.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml modules/nav_basic_fw.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
+   settings_modules="modules/lidar_sf11.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml modules/nav_basic_fw.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
    gui_color="#00009e93ffff"
   />
 </conf>
diff --git a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
index 08f43d6263..ca18ad87ac 100644
--- a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
@@ -116,6 +116,25 @@
      </program>
    </session>
 
+   <session name="HITL USB-serial@115200">
+     <program name="Data Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="115200"/>
+     </program>
+     <program name="Server">
+      <arg flag="" constant="-no_md5_check"/>
+     </program>
+     <program name="GCS"/>
+     <program name="Messages"/>
+     <program name="Simulator">
+      <arg flag="-a" constant="@AIRCRAFT"/>
+      <arg flag="-f" constant="127.0.0.1"/>
+      <arg flag="-b" constant="127.255.255.255"/>
+      <arg flag="--fg_fdm" constant=""/>
+     </program>
+   </session>
+
+
    <session name="NPS Flight Gear">
      <program name="Data Link">
        <arg flag="-udp"/>
@@ -154,6 +173,27 @@
      </program>
    </session>
 
+   <session name="HITL+SBUS USB-serial@115200">
+     <program name="Data Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="115200"/>
+     </program>
+     <program name="Server">
+      <arg flag="" constant="-no_md5_check"/>
+     </program>
+     <program name="GCS"/>
+     <program name="Messages"/>
+     <program name="Simulator">
+      <arg flag="-a" constant="@AIRCRAFT"/>
+      <arg flag="-f" constant="127.0.0.1"/>
+      <arg flag="-b" constant="127.255.255.255"/>
+      <arg flag="--fg_fdm" constant=""/>
+     </program>
+     <program name="Sbus Fakerator">
+       <arg flag="-p" constant="/dev/ttyUSB3"/>
+     </program>
+   </session>
+
    <session name="Flight USB-XBee-API@57600">
       <program name="Data Link">
         <arg flag="-d" constant="/dev/paparazzi/xbee"/>
diff --git a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
index f14f9a18df..30cdee4bd0 100644
--- a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
@@ -7,7 +7,7 @@ RP3 Lisa MX
 <airframe name="RP3 Lisa MX">
 
   <firmware name="fixedwing">
-    <target name="ap" board="lisa_mx_2.1_chibios">
+    <target name="ap" board="lisa_mx_2.1">
       <module name="radio_control" type="sbus">
         <configure name="SBUS_PORT" value="UART5"/>
       </module>
@@ -25,7 +25,7 @@ RP3 Lisa MX
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="radio_control" type="spektrum"/>
-      <!--configure name="USE_HITL" value="1"/-->
+      <configure name="USE_HITL" value="1"/>
       <configure name="INS_DEV" value="/dev/ttyUSB1"/>
       <configure name="INS_BAUD" value="B921600"/>
       <configure name="AP_DEV" value="/dev/ttyUSB2"/>
@@ -37,7 +37,7 @@ RP3 Lisa MX
 
     <module name="telemetry"   type="transparent">
       <configure name="MODEM_PORT"        value="UART3"/>
-      <configure name="MODEM_BAUD"        value="B57600"/>
+      <configure name="MODEM_BAUD"        value="B115200"/>
     </module>
 
     <module name="ins"       type="vectornav">
@@ -63,14 +63,16 @@ RP3 Lisa MX
       <configure name="EXTRA_DL_PORT" value="UART1"/>
       <configure name="EXTRA_DL_BAUD" value="B921600"/>
     </module>
-    <module name="lidar_sf11"/>
-    <module name="battery_monitor.xml">
+    <module name="lidar_sf11">
+      <configure name="LIDAR_SF11_I2C_DEV" value="i2c2"/>
+    </module>
+    <!--module name="battery_monitor.xml">
       <define name="BATMON_REV4" value="1"/>
       <define name="BATMON_CURRENT_OFFSET" value="-120"/>
       <define name="BATMON_CURRENT_SENSITIVITY" value="25.6"/>
       <define name="BATMON_TEMP_OFFSET" value="250"/>
       <define name="BATMON_TEMP_SENSITIVITY" value="10"/>
-    </module>
+    </module-->
   </modules>
 
   <!-- commands section -->
diff --git a/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml
index 372204c6be..22c0479fe2 100644
--- a/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml
+++ b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml
@@ -4,37 +4,24 @@
   <process name="Ap">
     <mode name="default">
       <message name="AUTOPILOT_VERSION"   period="11.1"/>
-      <message name="AIRSPEED"            period="1"/>
       <message name="ALIVE"               period="5.1"/>
       <message name="GPS"                 period="0.25"/>
       <message name="NAVIGATION"          period="1."/>
       <message name="ATTITUDE"            period="0.1"/>
       <message name="ESTIMATOR"           period="0.5"/>
-      <message name="ENERGY"              period="2.4"/>
       <message name="WP_MOVED"            period="0.5"/>
       <message name="CIRCLE"              period="1.05"/>
       <message name="DESIRED"             period="0.2"/>
       <message name="BAT"                 period="1.1"/>
       <message name="SEGMENT"             period="1.2"/>
-      <message name="CALIBRATION"         period="2.1"/>
       <message name="NAVIGATION_REF"      period="9."/>
       <message name="PPRZ_MODE"           period="4.9"/>
       <message name="SETTINGS"            period="5."/>
-      <message name="STATE_FILTER_STATUS" period="2.2"/>
       <message name="DATALINK_REPORT"     period="5.1"/>
       <message name="DL_VALUE"            period="1.5"/>
       <message name="IR_SENSORS"          period="1.2"/>
       <message name="SURVEY"              period="2.1"/>
-      <message name="GPS_SOL"             period="2.0"/>
-      <message name="IMU_ACCEL"           period=".8"/>
-      <message name="IMU_GYRO"            period=".6"/>
-      <message name="IMU_MAG"             period="1.3"/>
-      <message name="CAM"                 period="0.5"/>
-      <message name="CAM_POINT"           period="1.0"/>
-      <message name="COMMANDS"            period="1"/>
       <message name="FBW_STATUS"          period="2"/>
-      <message name="AIR_DATA"            period="1.3"/>
-      <message name="ESC"                 period="0.9"/>
       <message name="VECTORNAV_INFO"      period="1.0"/>
     </mode>
     <mode name="minimal">
diff --git a/sw/airborne/subsystems/ins/ins_vectornav.c b/sw/airborne/subsystems/ins/ins_vectornav.c
index 62d6bee183..16e759c8e2 100644
--- a/sw/airborne/subsystems/ins/ins_vectornav.c
+++ b/sw/airborne/subsystems/ins/ins_vectornav.c
@@ -299,6 +299,12 @@ void ins_vectornav_propagate()
   gps.hmsl =  (int32_t)((ins_vn.lla_pos.alt - geoid_h)* 1000.0f);
   SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
 
+  // Set GPS fix
+  gps.fix = ins_vn.vn_data.gps_fix;
+
+  // Set GPS num_sv
+  gps.num_sv = ins_vn.vn_data.num_sv;
+
   // set position uncertainty
   ins_vectornav_set_pacc();
 
diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c
index d5368fe961..e71e150d64 100644
--- a/sw/simulator/nps/nps_ivy.c
+++ b/sw/simulator/nps/nps_ivy.c
@@ -154,10 +154,11 @@ void nps_ivy_send_WORLD_ENV_REQ(void)
 
 int find_launch_index(void)
 {
-  static const char ap_launch[] = "lau"; // short name has only 3 first letters
+  static const char ap_launch[] = "aut_lau"; // short name
   char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
 
-  // list through the settinsg
+  // list through the settings
+  // TODO: maybe search for a substring with if(strstr(sent, word) != NULL)
   for (uint8_t idx=0;idx<NB_SETTING;idx++) {
    if (strcmp(ap_settings[idx],ap_launch) == 0) {
      return (int)idx;
@@ -185,6 +186,7 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
   DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
   printf("setting %d %f\n", index, value);
 
+
   /*
    * In case of HITL, update nps_autopilot.launch from DL_SETTINGS
    * so the plane can be properly launched.
diff --git a/sw/simulator/nps/nps_main_common.c b/sw/simulator/nps/nps_main_common.c
index ffa7544e6d..9d88d70cd8 100644
--- a/sw/simulator/nps/nps_main_common.c
+++ b/sw/simulator/nps/nps_main_common.c
@@ -284,8 +284,10 @@ void *nps_flight_gear_loop(void *data __attribute__((unused)))
       nanosleep(&waitFor, NULL);
     } else {
       // task took longer than the period
+#ifdef PRINT_TIME
       printf("FG THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
              (double)task_ns / 1E6, (double)period_ns / 1E6);
+#endif
     }
   }
 
@@ -329,8 +331,10 @@ void *nps_main_display(void *data __attribute__((unused)))
       nanosleep(&waitFor, NULL);
     } else {
       // task took longer than the period
+#ifdef PRINT_TIME
       printf("IVY DISPLAY THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
              (double)task_ns / 1E6, (double)period_ns / 1E6);
+#endif
     }
 
   }
diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c
index 73ca3a1622..040aad7cd6 100644
--- a/sw/simulator/nps/nps_main_hitl.c
+++ b/sw/simulator/nps/nps_main_hitl.c
@@ -96,6 +96,7 @@ void nps_radio_and_autopilot_init(void)
 void nps_update_launch_from_dl(uint8_t value)
 {
   nps_autopilot.launch = value;
+  printf("Launch value=%u\n",nps_autopilot.launch);
 }
 
 void nps_main_run_sim_step(void)
@@ -173,8 +174,10 @@ void *nps_ins_data_loop(void *data __attribute__((unused)))
       nanosleep(&waitFor, NULL);
     } else {
       // task took longer than the period
+#ifdef PRINT_TIME
       printf("INS THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
              (double)task_ns / 1E6, (double)period_ns / 1E6);
+#endif
     }
   }
   return(NULL);
@@ -326,8 +329,10 @@ void *nps_main_loop(void *data __attribute__((unused)))
       nanosleep(&waitFor, NULL);
     } else {
       // task took longer than the period
+#ifdef PRINT_TIME
       printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
              (double)task_ns / 1E6, (double)period_ns / 1E6);
+#endif
     }
   }
   return(NULL);
diff --git a/sw/simulator/nps/nps_main_sitl.c b/sw/simulator/nps/nps_main_sitl.c
index bdd56573fc..ab30db6bee 100644
--- a/sw/simulator/nps/nps_main_sitl.c
+++ b/sw/simulator/nps/nps_main_sitl.c
@@ -176,8 +176,10 @@ void *nps_main_loop(void *data __attribute__((unused)))
       nanosleep(&waitFor, NULL);
     } else {
       // task took longer than the period
+#ifdef PRINT_TIME
       printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
              (double)task_ns / 1E6, (double)period_ns / 1E6);
+#endif
     }
   }
   return(NULL);