Nps update - proof of concept of HITL+SITL integration (#1831)

* FlighGear and Ivy thread working * Main loop replaced with main_thread * Cleaned up nps_main * HITL main - test TX with Vectornav * Added AggieAir control panel * [modules] Flight logger module added * [fixedwing] Airframe configuration updated * [hitl] Proof Of Concept - IO and simulation running * [nps] Minor changes in main hitl * [nps] Performance Test with Pthread library * [hitl] Working with pthreads instead of glib threads * [hitl] Minor updates * [nps] Refactoring to get rid of Gthreads * [nps] Minor changes, added mutex guards * [nps] Minor Ivy fix * [nps] Threads working properly [hitl] Ins separated * [hitl] Check for launch variable * [nps] Refactoring makefile into common and hitl/sitl specific * [hitl] Hitl works, sitl too * [hitl] ins_vectornav comments * [hitl] Updated launch routine * [nps_hitl Cleaned up version of NPS/HITL - added support for ACTUATORS message (Rotorcraft) - resolved start issue for Fixedwing (works fine now) - minor improvements in threads (fg_thread, ap_init) * [nps] Minor fixes to save current work * [nps] Minor fixes * Update to latest pprzlink * FlighGear and Ivy thread working * Main loop replaced with main_thread * Cleaned up nps_main * HITL main - test TX with Vectornav * Added AggieAir control panel * [modules] Flight logger module added * [fixedwing] Airframe configuration updated * [hitl] Proof Of Concept - IO and simulation running * [nps] Minor changes in main hitl * [nps] Performance Test with Pthread library * [hitl] Working with pthreads instead of glib threads * [hitl] Minor updates * [nps] Refactoring to get rid of Gthreads * [nps] Minor changes, added mutex guards * [nps] Minor Ivy fix * [nps] Threads working properly [hitl] Ins separated * [hitl] Check for launch variable * [nps] Refactoring makefile into common and hitl/sitl specific * [hitl] Hitl works, sitl too * [hitl] ins_vectornav comments * [hitl] Updated launch routine * [nps_hitl Cleaned up version of NPS/HITL - added support for ACTUATORS message (Rotorcraft) - resolved start issue for Fixedwing (works fine now) - minor improvements in threads (fg_thread, ap_init) * [nps] Minor fixes to save current work * [nps] Minor fixes * Update to latest pprzlink * Fixed the feed function * Compilation fixes * More compilation fixes * Last compilation fixes * Added a simple automated flight plan (auto take off and landing) * Fixes in NPS * Removed SBUS fakerator * Removed superfluous "else" * Renamed "flight recorder" to "xgear" * Fixed formatting * Updated extra_dl module * Initial support for extra_dl to run over USB serial * Fixed usb_serial for extra_dl * extra_dl_periodic() called at TELEMETRY_FREQUENCY
2026-05-09 22:49:53 +08:00 · 2016-10-04 05:42:10 -07:00
parent 7e837c38da
commit 49e058010e
37 changed files with 2899 additions and 353 deletions
@@ -16,29 +16,52 @@
        <configure name="SBUS_PORT" value="UART5"/>
      </module>
    </target>
+    
+    <configure name="PERIODIC_FREQUENCY" value="160"/>
+

    <target name="nps" board="pc">
      <module name="fdm" type="jsbsim"/>
      <module name="radio_control" type="spektrum"/>
+      <!--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"/>
+      <configure name="AP_BAUD" value="B921600"/>
    </target>
+    
+    <module name="stabilization" type="int_quat"/>

    <module name="motor_mixing"/>
    <module name="actuators"     type="pwm">
-      <define name="SERVO_HZ" value="400"/>
+      <define name="SERVO_HZ" value="PERIODIC_FREQUENCY"/>
+    </module>
+
+    <module name="telemetry"   type="transparent">
+      <configure name="MODEM_PORT"        value="UART3"/>
+      <configure name="MODEM_BAUD"        value="B57600"/>
+    </module>
+
+    <module name="ins"       type="vectornav">
+      <configure name="VN_PORT" value="UART2"/>
+      <configure name="VN_BAUD" value="B921600"/>
    </module>

-    <module name="telemetry"     type="transparent"/>
-    <module name="stabilization" type="float_euler"/>
-    <module name="ins" type="vectornav"/>
    <define name="GEOFENCE_DATALINK_LOST_TIME" value="45"/>
  </firmware>


  <modules>
    <module name="sys_mon"/>
+    <module name="flight_logger">
+      <!-- in order to use uart1 without chibios we need to remap the peripheral-->
+      <define name="REMAP_UART1" value="TRUE"/>
+      <configure name="FLIGHTLOGGER_PORT" value="UART1"/>
+      <configure name="FLIGHTLOGGER_BAUD" value="B921600"/>
+    </module>
  </modules>

-  <servos driver="Pwm">
+    <servos driver="Pwm">
    <servo name="FL" no="0" min="1000" neutral="1100" max="1900"/>
    <servo name="FR" no="1" min="1000" neutral="1100" max="1900"/>
    <servo name="BR" no="2" min="1000" neutral="1100" max="1900"/>
@@ -90,7 +113,24 @@
    <define name="H_Z" value="0.9259921"/>
  </section>

+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_P" value="140" unit="deg/s"/>
+    <define name="SP_MAX_Q" value="140" unit="deg/s"/>
+    <define name="SP_MAX_R" value="140" unit="deg/s"/>
+    <define name="DEADBAND_P" value="20"/>
+    <define name="DEADBAND_Q" value="20"/>
+    <define name="DEADBAND_R" value="200"/>

+    <!-- feedback -->
+    <define name="GAIN_P" value="800"/>
+    <define name="GAIN_Q" value="800"/>
+    <define name="GAIN_R" value="700"/>
+
+    <define name="IGAIN_P" value="240"/>
+    <define name="IGAIN_Q" value="240"/>
+    <define name="IGAIN_R" value="160"/>
+  </section>

  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
    <!-- setpoints -->
@@ -154,11 +194,22 @@
  </section>

  <section name="SIMULATOR" prefix="NPS_">
-    <define name="ACTUATOR_NAMES"  value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
+    <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
    <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
    <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
    <!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
-    <define name="JS_AXIS_MODE" value="4"/>
+    <define name="JS_AXIS_THROTTLE" value="0"/>
+    <define name="JS_AXIS_THROTTLE_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_ROLL" value="1"/>
+
+    <define name="JS_AXIS_PITCH" value="2"/>
+    <define name="JS_AXIS_PITCH_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_YAW" value="3"/>
+    <define name="JS_AXIS_YAW_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_MODE" value="7"/>
  </section>

  <section name="AUTOPILOT">
@@ -168,10 +219,19 @@
  </section>

  <section name="BAT">
+    <!-- Flight values -->
+    <!--
    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
    <define name="LOW_BAT_LEVEL" value="10.1" unit="V"/>
    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
+    -->
+    
+    <!-- Simulator values -->
+    <define name="MAX_BAT_LEVEL" value="5.0" unit="V" />
+    <define name="LOW_BAT_LEVEL" value="4.0" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="3.5" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
  </section>

 </airframe>
@@ -3,22 +3,22 @@
   name="Ark_Quad"
   ac_id="2"
   airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml"
-   radio="radios/AGGIEAIR/aggieair_taranis.xml"
-   telemetry="telemetry/default_rotorcraft.xml"
+   radio="radios/AGGIEAIR/aggieair_sbus_fakerator.xml"
+   telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
   flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml"
-   settings_modules=""
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml"
+   settings_modules="modules/gps.xml"
   gui_color="#ffff954c0000"
  />
  <aircraft
   name="Minion_Lia"
   ac_id="1"
   airframe="airframes/AGGIEAIR/aggieair_rp3_lia.xml"
-   radio="radios/AGGIEAIR/aggieair_taranis.xml"
-   telemetry="telemetry/default_fixedwing.xml"
+   radio="radios/AGGIEAIR/aggieair_sbus_fakerator.xml"
+   telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
   flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
   settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
-   settings_modules="modules/nav_survey_poly_osam.xml"
+   settings_modules="modules/gps.xml modules/nav_survey_poly_osam.xml modules/nav_skid_landing.xml"
   gui_color="#00009e93ffff"
  />
 </conf>
@@ -0,0 +1,216 @@
+<?xml version="1.0"?>
+<control_panel name="aggieair control panel">
+
+  <section name="programs">
+    <program name="Server" command="sw/ground_segment/tmtc/server"/>
+    <program name="Data Link" command="sw/ground_segment/tmtc/link">
+      <arg flag="-d" constant="/dev/ttyUSB0"/>
+    </program>
+    <program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
+    <program name="GCS" command="sw/ground_segment/cockpit/gcs"/>
+    <program name="Flight Plan Editor" command="sw/ground_segment/cockpit/gcs">
+      <arg flag="-edit"/>
+    </program>
+    <program name="Messages" command="sw/ground_segment/tmtc/messages"/>
+    <program name="Messages (Python)" command="sw/ground_segment/python/messages_app/messagesapp.py"/>
+    <program name="Settings" command="sw/ground_segment/tmtc/settings">
+      <arg flag="-ac" constant="@AIRCRAFT"/>
+    </program>
+    <program name="Settings (Python)" command="sw/ground_segment/python/settings_app/settingsapp.py">
+      <arg flag="--ac_id" constant="@AC_ID"/>
+    </program>
+    <program name="GPSd position display" command="sw/ground_segment/tmtc/gpsd2ivy"/>
+    <program name="Log Plotter" command="sw/logalizer/logplotter"/>
+    <program name="Real-time Plotter" command="sw/logalizer/plotter"/>
+    <program name="Real-time Plotter (Python)" command="sw/ground_segment/python/real_time_plot/realtimeplotapp.py"/>
+    <program name="Log File Player" command="sw/logalizer/play"/>
+    <program name="Simulator" command="sw/simulator/pprzsim-launch">
+      <arg flag="-a" constant="@AIRCRAFT"/>
+    </program>
+    <program name="Video Synchronizer" command="sw/ground_segment/misc/video_synchronizer"/>
+    <program name="Joystick" command="sw/ground_segment/joystick/input2ivy">
+      <arg flag="-ac" constant="@AIRCRAFT"/>
+      <arg flag="xbox_gamepad.xml"/>
+    </program>
+    <program name="Sbus Fakerator" command="sw/tools/sbus_fakerator/sbus_fakerator">
+      <arg flag="-p" constant="/dev/ttyUSB0"/>
+    </program>
+    <program name="Environment Simulator" command="sw/simulator/gaia"/>
+    <program name="Http Server" command="$python">
+      <arg flag="-m" constant="SimpleHTTPServer"/>
+      <arg flag="8889"/>
+    </program>
+    <program name="Plot Meteo Profile" command="sw/logalizer/plotprofile"/>
+    <program name="Weather Station" command="sw/ground_segment/misc/davis2ivy">
+      <arg flag="-d" constant="/dev/ttyUSB1"/>
+    </program>
+    <program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
+    <program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
+    <program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
+    <program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
+      <arg flag="--port" constant="/dev/ttyUSB1"/>
+      <arg flag="--id" constant="1"/>
+    </program>
+    <program name="BluegigaUsbDongleScanner" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
+      <arg flag="/dev/ttyACM2"/>
+      <arg flag="scan" />
+    </program>
+    <program name="BluegigaUsbDongle" command="sw/tools/bluegiga_usb_dongle/bluegiga_usb_driver">
+      <arg flag="/dev/ttyACM2"/>
+      <arg flag="00:07:00:2d:d6:bb" />
+      <arg flag="4242" />
+      <arg flag="4252" />
+    </program>
+    <program name="ADS-B Intruders receiver" command="sw/ground_segment/misc/sbs2ivy">
+      <arg flag="--ac" constant="@AC_ID"/>
+    </program>
+ </section>
+
+  <section name="sessions">
+
+   <session name="Flight USB-serial@9600">
+      <program name="Data Link">
+        <arg flag="-d" constant="/dev/ttyUSB0"/>
+      </program>
+      <program name="Server"/>
+      <program name="GCS"/>
+    </session>
+
+   <session name="Flight USB-serial@57600">
+     <program name="Data Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="57600"/>
+     </program>
+     <program name="Server"/>
+     <program name="GCS"/>
+     <program name="Messages"/>
+   </session>
+
+   <session name="HITL USB-serial@57600">
+     <program name="Data Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="57600"/>
+     </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="HITL+SBUS USB-serial@57600">
+     <program name="Data Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="57600"/>
+     </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"/>
+        <arg flag="-transport" constant="xbee"/>
+        <arg flag="-s" constant="57600"/>
+      </program>
+      <program name="Server"/>
+      <program name="GCS"/>
+    </session>
+
+    <session name="Messages and Settings">
+      <program name="Data Link">
+        <arg flag="-d" constant="/dev/ttyUSB0"/>
+        <arg flag="-s" constant="57600"/>
+      </program>
+      <program name="Server">
+        <arg flag="-n"/>
+      </program>
+      <program name="Messages"/>
+      <program name="Settings">
+        <arg flag="-ac" constant="@AIRCRAFT"/>
+      </program>
+    </session>
+
+    <session name="Raw Sensors">
+      <program name="Data Link">
+        <arg flag="-d" constant="/dev/ttyUSB0"/>
+        <arg flag="-s" constant="57600"/>
+      </program>
+      <program name="Server">
+        <arg flag="-n"/>
+      </program>
+      <program name="Messages">
+        <arg flag="-g" constant="300x400+0-220"/>
+      </program>
+      <program name="Settings">
+        <arg flag="-ac" constant="@AIRCRAFT"/>
+        <arg flag="-g" constant="800x200+0-0"/>
+      </program>
+      <program name="Real-time Plotter">
+        <arg flag="-g" constant="1000x250-0+0"/>
+        <arg flag="-t" constant="ACC"/>
+        <arg flag="-u" constant="0.05"/>
+        <arg flag="-c" constant="0.00"/>
+        <arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ax"/>
+        <arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:ay"/>
+        <arg flag="-c" constant="*:telemetry:IMU_ACCEL_RAW:az"/>
+        <arg flag="-n"/>
+        <arg flag="-g" constant="1000x250-0+250"/>
+        <arg flag="-t" constant="GYRO"/>
+        <arg flag="-u" constant="0.05"/>
+        <arg flag="-c" constant="0.00"/>
+        <arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gp"/>
+        <arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gq"/>
+        <arg flag="-c" constant="*:telemetry:IMU_GYRO_RAW:gr"/>
+        <arg flag="-n"/>
+        <arg flag="-g" constant="1000x250-0+500"/>
+        <arg flag="-t" constant="MAG"/>
+        <arg flag="-u" constant="0.05"/>
+        <arg flag="-c" constant="0.00"/>
+        <arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mx"/>
+        <arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:my"/>
+        <arg flag="-c" constant="*:telemetry:IMU_MAG_RAW:mz"/>
+        <arg flag="-n"/>
+        <arg flag="-g" constant="1000x250-0+750"/>
+        <arg flag="-t" constant="BARO"/>
+        <arg flag="-u" constant="0.05"/>
+        <arg flag="-c" constant="101325.0"/>
+        <arg flag="-c" constant="*:telemetry:BARO_RAW:abs"/>
+      </program>
+    </session>
+
+
+    <session name="Log Replay">
+      <program name="Log File Player"/>
+      <program name="Server">
+        <arg flag="-n"/>
+      </program>
+      <program name="GCS"/>
+      <program name="Messages">
+        <arg flag="-g" constant="300x400+0-220"/>
+      </program>
+      <program name="Real-time Plotter"/>
+      <program name="Environment Simulator"/>
+    </session>
+
+  </section>
+
+</control_panel>
@@ -7,17 +7,26 @@ RP3 Lisa MX
 <airframe name="RP3 Lisa MX">

  <firmware name="fixedwing">
-    <define name="CONTROL_FREQUENCY" value="100"/>
-    <configure name="PERIODIC_FREQUENCY" value="100"/>
    <target name="ap" board="lisa_mx_2.1_chibios">
      <module name="radio_control" type="sbus">
        <configure name="SBUS_PORT" value="UART5"/>
      </module>
    </target>

+    <!-- NOTE: if you want to use extra_dl module for HITL
+    you have to set TELEMETRY_FREQUENCY to CONTROL_FREQUENCY -->
+    <configure name="PERIODIC_FREQUENCY" value="160"/>
+    <define name="CONTROL_FREQUENCY" value="160"/>
+    <configure name="TELEMETRY_FREQUENCY" value="160"/>
+
    <target name="nps" board="pc">
      <module name="fdm" type="jsbsim"/>
-      <module name="radio_control" type="ppm"/>
+      <module name="radio_control" type="spektrum"/>
+      <!--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"/>
+      <configure name="AP_BAUD" value="B921600"/>
    </target>

    <module name="control"/>
@@ -37,7 +46,7 @@ RP3 Lisa MX
    <define name="POLY_OSAM_HALF_SWEEP_ENABLED" value="FALSE"/>
    <define name="GEOFENCE_DATALINK_LOST_TIME" value="45"/>
  </firmware>
-
+  
  <modules>
    <module name="nav" type="line"/>
    <module name="nav" type="flower"/>
@@ -45,17 +54,23 @@ RP3 Lisa MX
    <module name="nav" type="launcher"/>
    <module name="nav" type="skid_landing"/>
    <module name="sys_mon"/>
+    <module name="extra_dl">
+      <!-- in order to use uart1 without chibios we need to remap the peripheral-->
+      <define name="REMAP_UART1" value="TRUE"/>
+      <configure name="EXTRA_DL_PORT" value="UART1"/>
+      <configure name="EXTRA_DL_BAUD" value="B921600"/>
+    </module>
  </modules>

  <!-- commands section -->
  <!-- Servo Configuration -->
  <servos>
-    <servo name="THROTTLE"		no="0" min="1100" neutral="1100" max="1900"/>
-    <servo name="AILERON_RIGHT"     no="1" min="900" neutral="1500" max="2100"/>
+    <servo name="THROTTLE"		  no="0" min="1100" neutral="1100" max="1900"/>
+    <servo name="AILERON_RIGHT" no="1" min="900" neutral="1500" max="2100"/>
    <servo name="AILERON_LEFT"	no="2" min="2100" neutral="1500" max="900"/>
    <servo name="ELEVATOR"      no="3" min="2100" neutral="1500" max="900"/>
    <servo name="RUDDER"        no="4" min="900" neutral="1500" max="2100"/>
-    <servo name="FLAP"		no="5" min="900" neutral="900" max="2100"/>
+    <servo name="FLAP"		      no="5" min="900" neutral="900" max="2100"/>
  </servos>

  <!-- Servo Command Structure -->
@@ -107,7 +122,6 @@ RP3 Lisa MX
    <define name="MAX_PITCH" value="0.6"/>
  </section>

-
  <section name="IMU" prefix="IMU_">
    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -130,10 +144,20 @@ RP3 Lisa MX
  <section name="BAT">
    <define name="MILLIAMP_AT_FULL_THROTTLE" value="50000"/>
    <define name="CURRENT_ESTIMATION_NONLINEARITY" value="1.3"/>
-    <define name="CATASTROPHIC_BAT_LEVEL" value="13.5" unit="V"/>
+    
+    <!-- Flight values -->
+    <!--
+    <define name="MAX_BAT_LEVEL" value="16.5" unit="V" />
    <define name="LOW_BAT_LEVEL" value="14.1" unit="V"/>
    <define name="CRITIC_BAT_LEVEL" value="13.8" unit="V"/>
-    <define name="MAX_BAT_LEVEL" value="16.5" unit="V" />
+    <define name="CATASTROPHIC_BAT_LEVEL" value="13.5" unit="V"/>
+    -->
+    
+    <!-- Simulator values -->
+    <define name="MAX_BAT_LEVEL" value="5.0" unit="V" />
+    <define name="LOW_BAT_LEVEL" value="4.0" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="3.5" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
  </section>

  <section name="MISC">
@@ -144,7 +168,8 @@ RP3 Lisa MX
  </section>

  <section name="VERTICAL CONTROL" prefix="V_CTL_">
-    <define name="POWER_CTL_BAT_NOMINAL" value="23.2" unit="volt"/>
+    <!-- TODO: double check the power controls settings, this value seems to be outdated -->
+    <!--define name="POWER_CTL_BAT_NOMINAL" value="23.2" unit="volt"/-->
    <!-- outer loop proportional gain -->
    <define name="ALTITUDE_PGAIN" value="0.136"/>
    <!-- outer loop saturation -->
@@ -247,6 +272,10 @@ RP3 Lisa MX

    <define name="JS_AXIS_FLAPS" value="5"/>
    <define name="JS_AXIS_MODE" value="7"/>
+
+    <define name="JSBSIM_ROLL_TRIM" value="0.008"/>
+    <define name="JSBSIM_PITCH_TRIM" value="0.0375"/>
+    <define name="JSBSIM_YAW_TRIM" value="0.001"/>
  </section>

 </airframe>
@@ -0,0 +1,283 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+ARK RT
+-->
+
+<airframe name="Quadrotor LisaMX_2.1 pwm">
+
+  <firmware name="rotorcraft">
+
+    <target name="ap" board="lisa_mx_2.1">
+      <subsystem name="radio_control" type="sbus">
+        <configure name="SBUS_PORT" value="UART5"/>
+      </subsystem>
+    </target>
+
+    <!-- NOTE: if you want to use extra_dl module for HITL
+    you have to set TELEMETRY_FREQUENCY to PERIODIC_FREQUENCY -->
+    <configure name="PERIODIC_FREQUENCY" value="160"/>
+    <configure name="TELEMETRY_FREQUENCY" value="160"/>
+
+    <target name="nps" board="pc">
+      <module name="fdm" type="jsbsim"/>
+      <module name="radio_control" type="spektrum"/>
+      <!--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"/>
+      <configure name="AP_BAUD" value="B921600"/>
+    </target>
+
+    <subsystem name="motor_mixing"/>
+    <subsystem name="actuators"     type="pwm">
+      <define name="SERVO_HZ" value="160"/>
+    </subsystem>
+
+    <subsystem name="telemetry"     type="transparent">
+      <configure name="MODEM_PORT" value="UART3"/>
+      <configure name="MODEM_BAUD" value="B57600"/>
+    </subsystem>
+
+    <subsystem name="stabilization" type="float_euler"/>
+
+    <module name="ins"       type="vectornav">
+      <configure name="VN_PORT" value="UART2"/>
+      <configure name="VN_BAUD" value="B921600"/>
+    </module>
+  </firmware>
+
+  <modules>
+    <module name="sys_mon"/>
+    <module name="extra_dl">
+      <!-- in order to use uart1 without chibios we need to remap the peripheral-->
+      <define name="REMAP_UART1" value="TRUE"/>
+      <configure name="EXTRA_DL_PORT" value="UART1"/>
+      <configure name="EXTRA_DL_BAUD" value="B921600"/>
+    </module>
+  </modules>
+
+  <!--These values are set for the Castle Creations HV Lite 60A ESCs-->
+  <servos driver="Pwm">
+    <servo name="FRONT"       no="0" min="1010" neutral="1150" max="2000"/>
+    <servo name="FRONT_RIGHT" no="1" min="1010" neutral="1150" max="2000"/>
+    <servo name="BACK_RIGHT"  no="2" min="1010" neutral="1150" max="2000"/>
+    <servo name="BACK"        no="3" min="1010" neutral="1150" max="2000"/>
+    <servo name="BACK_LEFT"   no="4" min="1010" neutral="1150" max="2000"/>
+    <servo name="FRONT_LEFT"  no="5" min="1010" neutral="1150" max="2000"/>
+  </servos>
+
+  <commands>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <section name="MIXING" prefix="MOTOR_MIXING_">
+    <define name="TRIM_ROLL" value="0"/>
+    <define name="TRIM_PITCH" value="0"/>
+    <define name="TRIM_YAW" value="0"/>
+    <define name="NB_MOTOR" value="6"/>
+    <define name="SCALE" value="256"/>
+    <!-- hex plus A -->
+    <define name="ROLL_COEF"   value="{   0, -256, -256,  0,  256,  256 }"/>
+    <define name="PITCH_COEF"  value="{  256,  128, -128, -256, -128, 128 }"/>
+    <define name="YAW_COEF"    value="{  128, -128, 128, -128, 128,  -128 }"/>
+    <define name="THRUST_COEF" value="{  256,  256,  256,  256,  256,  256 }"/>
+  </section>
+
+  <command_laws>
+    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
+    <set servo="FRONT_RIGHT" value="motor_mixing.commands[SERVO_FRONT_RIGHT]"/>
+    <set servo="BACK_RIGHT"   value="motor_mixing.commands[SERVO_BACK_RIGHT]"/>
+    <set servo="BACK"    value="motor_mixing.commands[SERVO_BACK]"/>
+    <set servo="BACK_LEFT"  value="motor_mixing.commands[SERVO_BACK_LEFT]"/>
+    <set servo="FRONT_LEFT"   value="motor_mixing.commands[SERVO_FRONT_LEFT]"/>
+  </command_laws>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="BODY_TO_IMU_PHI"   value="0.0" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0.0" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0.0" unit="deg"/>
+
+    <define name="BODY_TO_IMU2_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU2_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU2_PSI"   value="270." unit="deg"/>
+
+    <define name="ACCEL_X_NEUTRAL" value="11"/>
+    <define name="ACCEL_Y_NEUTRAL" value="11"/>
+    <define name="ACCEL_Z_NEUTRAL" value="-25"/>
+
+    <!-- replace this with your own calibration -->
+    <define name="MAG_X_NEUTRAL" value="-179"/>
+    <define name="MAG_Y_NEUTRAL" value="-21"/>
+    <define name="MAG_Z_NEUTRAL" value="79"/>
+    <define name="MAG_X_SENS" value="4.17334785618" integer="16"/>
+    <define name="MAG_Y_SENS" value="3.98885954135" integer="16"/>
+    <define name="MAG_Z_SENS" value="4.40442339014" integer="16"/>
+  </section>
+
+    <section name="AHRS" prefix="AHRS_">
+    <define name="H_X" value="0.3770441"/>
+    <define name="H_Y" value="0.0193986"/>
+    <define name="H_Z" value="0.9259921"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+  </section>
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_P" value="10000"/>
+    <define name="SP_MAX_Q" value="10000"/>
+    <define name="SP_MAX_R" value="10000"/>
+    <define name="DEADBAND_P" value="20"/>
+    <define name="DEADBAND_Q" value="20"/>
+    <define name="DEADBAND_R" value="200"/>
+    <define name="REF_TAU" value="4"/>
+
+    <!-- feedback -->
+    <define name="GAIN_P" value="000"/>
+    <define name="GAIN_Q" value="000"/>
+    <define name="GAIN_R" value="00"/>
+
+    <define name="IGAIN_P" value="0"/>
+    <define name="IGAIN_Q" value="0"/>
+    <define name="IGAIN_R" value="0"/>
+
+    <!-- feedforward -->
+    <define name="DDGAIN_P" value="300"/>
+    <define name="DDGAIN_Q" value="300"/>
+    <define name="DDGAIN_R" value="300"/>
+  </section>
+
+
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="35." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="35." unit="deg"/>
+    <define name="SP_MAX_R"       value="40." unit="deg/s"/>
+    <define name="DEADBAND_A"     value="5"/>
+    <define name="DEADBAND_E"     value="5"/>
+    <define name="DEADBAND_R"     value="10"/>
+
+    <!-- inetgrator limits -->
+    <define name="MAX_SUM_ERR"   value="50"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- Big fat ARK Gains -->
+    <define name="PHI_PGAIN"  value="2700"/>
+    <define name="PHI_DGAIN"  value="700"/>
+    <define name="PHI_IGAIN"  value="5.0"/>
+
+    <define name="THETA_PGAIN"  value="2700"/>
+    <define name="THETA_DGAIN"  value="700"/>
+    <define name="THETA_IGAIN"  value="5.0"/>
+
+    <define name="PSI_PGAIN" value="4000"/>
+    <define name="PSI_DGAIN" value="3500"/>
+    <define name="PSI_IGAIN" value="1.5"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value="5"/>
+    <define name="THETA_DDGAIN" value="5"/>
+    <define name="PSI_DDGAIN"   value="10"/>
+  </section>
+
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+    <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+    <define name="MAX_SUM_ERR" value="2000000"/>
+    <define name="HOVER_KP"    value="265"/>
+    <define name="HOVER_KD"    value="339"/>
+    <define name="HOVER_KI"    value="52"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value="0.50"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="MAX_BANK" value="20" unit="deg"/>
+    <define name="USE_SPEED_REF" value="TRUE"/>
+    <define name="REF_MAX_SPEED" value="8.0" unit="m/s"/>
+    <define name="REF_MAX_ACCEL" value="5.66" unit="m/s2"/>
+    <define name="PGAIN" value="27"/>
+    <define name="DGAIN" value="21"/>
+    <define name="AGAIN" value="17"/>
+    <define name="IGAIN" value="16"/>
+  </section>
+
+  <section name="NAVIGATION" prefix="NAV_">
+    <define name="CLIMB_VSPEED"   value="1.5" />
+    <define name="DESCEND_VSPEED" value="-1.0" />
+  </section>
+
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_NAMES"  value="f_motor, fr_motor, br_motor, b_motor, bl_motor, fl_motor" type="string[]"/>
+    <define name="JSBSIM_MODEL"    value="AGGIEAIR/aggieair_ark_hexa" type="string"/>
+    <define name="SENSORS_PARAMS"  value="nps_sensors_params_default.h" type="string"/>
+    
+    <define name="JS_AXIS_THROTTLE" value="0"/>
+    <define name="JS_AXIS_THROTTLE_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_ROLL" value="1"/>
+
+    <define name="JS_AXIS_PITCH" value="2"/>
+    <define name="JS_AXIS_PITCH_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_YAW" value="3"/>
+    <define name="JS_AXIS_YAW_REVERSED" value="1"/>
+
+    <define name="JS_AXIS_MODE" value="7"/>
+  </section>
+
+  <section name="GCS">
+    <define name="ICONS_THEME"         value="flat_theme"/>
+    <define name="ALT_SHIFT_PLUS_PLUS" value="30"/>
+    <define name="ALT_SHIFT_PLUS"      value="10"/>
+    <define name="ALT_SHIFT_MINUS"     value="-10"/>
+    <define name="AC_ICON"             value="hexarotor"/>
+  </section>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_NAV"/>
+  </section>
+
+  <section name="BAT">
+    <!-- Flight values -->
+    <!--
+    <define name="CATASTROPHIC_BAT_LEVEL" value="19.2" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="20.0" unit="V"/>
+    <define name="LOW_BAT_LEVEL" value="21.0" unit="V"/>
+    <define name="MAX_BAT_LEVEL" value="25.3" unit="V"/>
+    -->
+
+    <!-- Simulator values -->
+    <define name="MAX_BAT_LEVEL" value="5.0" unit="V" />
+    <define name="LOW_BAT_LEVEL" value="4.0" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="3.5" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
+  </section>
+
+</airframe>
@@ -2,10 +2,10 @@
  <aircraft
   name="Ark_Quad"
   ac_id="41"
-   airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml"
+   airframe="airframes/AGGIEAIR/ark_hexa_1-8.xml"
   radio="radios/AGGIEAIR/aggieair_taranis.xml"
-   telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
+   telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_basic.xml"
   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml"
   settings_modules="modules/gps.xml"
   gui_color="#ffff954c0000"
@@ -213,7 +213,7 @@
   ac_id="42"
   airframe="airframes/AGGIEAIR/aggieair_rp3_lia.xml"
   radio="radios/AGGIEAIR/aggieair_taranis.xml"
-   telemetry="telemetry/default_fixedwing.xml"
+   telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
   flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
   settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
   settings_modules="modules/gps.xml modules/nav_survey_poly_osam.xml modules/nav_skid_landing.xml"
@@ -1,21 +1,26 @@
 # Hey Emacs, this is a -*- makefile -*-

 #
-# NPS SITL Simulator
+# NPS Simulator
+#
+# Common makefile for both SITL/HITL simulation
 #
 # still needs a FDM backend to be specified, e.g.
 # <subsystem name="fdm" type="jsbsim"/>
 #

+USE_HITL ?= 0
+
 nps.ARCHDIR = sim

 # include Makefile.nps instead of Makefile.sim
 nps.MAKEFILE = nps

 nps.CFLAGS  += -DSITL -DUSE_NPS
-nps.CFLAGS  += $(shell pkg-config glib-2.0 --cflags)
-nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy $(shell pcre-config --libs) -lgsl -lgslcblas
+nps.LDFLAGS += -lm -livy $(shell pcre-config --libs) -lgsl -lgslcblas -lrt
 nps.CFLAGS  += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_SRC)/sw/simulator/nps -I$(PAPARAZZI_HOME)/conf/simulator/nps
+
+# sdl needed for joystick input
 nps.LDFLAGS += $(shell sdl-config --libs)


@@ -25,7 +30,7 @@ nps.LDFLAGS += $(shell sdl-config --libs)
 VPATH += $(PAPARAZZI_SRC)/sw/simulator

 NPSDIR = nps
-nps.srcs += $(NPSDIR)/nps_main.c                 \
+nps.srcs +=                                      \
       $(NPSDIR)/nps_random.c                    \
       $(NPSDIR)/nps_sensors.c                   \
       $(NPSDIR)/nps_sensors_utils.c             \
@@ -39,11 +44,20 @@ nps.srcs += $(NPSDIR)/nps_main.c                 \
       $(NPSDIR)/nps_sensor_temperature.c        \
       $(NPSDIR)/nps_electrical.c                \
       $(NPSDIR)/nps_atmosphere.c                \
+       $(NPSDIR)/nps_ivy.c                       \
+       $(NPSDIR)/nps_flightgear.c                \
       $(NPSDIR)/nps_radio_control.c             \
       $(NPSDIR)/nps_radio_control_joystick.c    \
       $(NPSDIR)/nps_radio_control_spektrum.c    \
-       $(NPSDIR)/nps_ivy.c                       \
-       $(NPSDIR)/nps_flightgear.c
+       $(NPSDIR)/nps_main_common.c
+
+ifeq ($(USE_HITL),1)
+$(info USE_HITL defined)
+include $(CFG_SHARED)/nps_hitl.makefile
+else
+$(info USE_HITL undefined)
+include $(CFG_SHARED)/nps_sitl.makefile
+endif

 # for geo mag calculation
 nps.srcs += math/pprz_geodetic_wmm2015.c
@@ -0,0 +1,28 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+#
+# NPS HITL Simulator
+#
+# HITL specific makefile
+#
+
+nps.srcs += $(NPSDIR)/nps_main_hitl.c
+
+# TODO: have this in ins_vectornav.xml 
+# will hopefully work better once nps and HITL are separate targets
+nps.srcs += $(NPSDIR)/nps_ins_vectornav.c
+
+# glib is still needed for some components (such as radio input)
+nps.CFLAGS  += $(shell pkg-config glib-2.0 --cflags)
+nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs)
+
+INS_DEV ?= \"/dev/ttyUSB1\"
+INS_BAUD ?= B921600
+
+AP_DEV ?= \"/dev/ttyUSB2\"
+AP_BAUD ?= B921600
+
+nps.CFLAGS += -DAP_DEV=\"$(AP_DEV)\"
+nps.CFLAGS += -DAP_BAUD=$(AP_BAUD)
+nps.CFLAGS += -DINS_DEV=\"$(INS_DEV)\"
+nps.CFLAGS += -DINS_BAUD=$(INS_BAUD)
@@ -0,0 +1,13 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+#
+# NPS SITL Simulator
+#
+# SITL specific makefile
+#
+
+# glib is still needed for some components (such as radio input)
+nps.CFLAGS  += $(shell pkg-config glib-2.0 --cflags)
+nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs)
+
+nps.srcs += $(NPSDIR)/nps_main_sitl.c
@@ -56,6 +56,11 @@
      <set value="1" var="kill_throttle"/>
      <while cond="TRUE"/>
    </block>
+    <block name="Launch" strip_button="TakeOff (wp CLIMB)" strip_icon="takeoff.png">
+      <call fun="nav_launcher_setup()"/>
+      <call fun="nav_launcher_run()"/>
+      <deroute block="Standby"/>
+    </block>
    <block name="CircleUpTo1000">
      <circle alt="2350" radius="500" wp="STDBY"/>
    </block>
@@ -0,0 +1,92 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+<!-- A simple flightplan for automated SITL/HITL verification: it launches and lands itself, giving us an idea whether a given airframe 
+     can finish it -->
+<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
+  <header>
+#include "subsystems/datalink/datalink.h"
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="0" y="0"/>
+    <waypoint name="STDBY" x="49.5" y="100.1"/>
+    <waypoint name="1" x="10.1" y="189.9"/>
+    <waypoint name="2" x="132.3" y="139.1"/>
+    <waypoint name="MOB" x="137.0" y="-11.6"/>
+    <waypoint name="S1" x="-119.2" y="69.6"/>
+    <waypoint name="S2" x="274.4" y="209.5"/>
+    <waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
+    <waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
+    <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
+    <waypoint name="CLIMB" x="-114.5" y="162.3"/>
+  </waypoints>
+  <exceptions>
+    <!--exception cond="datalink_time > 22" deroute="Standby"/-->
+  </exceptions>
+  <blocks>
+    <block name="Wait GPS">
+      <set value="1" var="kill_throttle"/>
+      <while cond="!GpsFixValid()"/>
+    </block>
+    <block name="Geo init">
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call fun="NavSetGroundReferenceHere()"/>
+    </block>
+    <block name="Holding point">
+      <set value="1" var="kill_throttle"/>
+      <attitude roll="0" throttle="0" vmode="throttle"/>
+      <exception cond="NavBlockTime() > 10" deroute="Takeoff"/>
+    </block>
+    <block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
+      <exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
+      <set value="0" var="kill_throttle"/>
+      <set value="0" var="autopilot_flight_time"/>
+      <set value="1" var="launch"/>
+      <set value="0" var="kill_throttle"/>
+      <while cond="LessThan(NavBlockTime(), 3)"/>
+      <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
+    </block>
+    <block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
+      <circle radius="nav_radius" wp="STDBY"/>
+      <exception cond="NavBlockTime() > 30" deroute="Figure 8 around wp 1"/>
+    </block>
+    <block key="F8" name="Figure 8 around wp 1" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png" group="base_pattern">
+      <eight center="1" radius="nav_radius" turn_around="2"/>
+      <exception cond="NavBlockTime() > 120" deroute="Oval 1-2"/>
+    </block>
+    <block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png" group="base_pattern">
+      <oval p1="1" p2="2" radius="nav_radius"/>
+      <exception cond="NavBlockTime() > 90" deroute="Survey S1-S2"/>
+    </block>
+    <block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png" group="extra_pattern">
+      <survey_rectangle grid="150" wp1="S1" wp2="S2"/>
+      <exception cond="NavBlockTime() > 180" deroute="Path 1,2,S1,S2,STDBY"/>
+    </block>
+    <block name="Path 1,2,S1,S2,STDBY" strip_button="Path (1,2,S1,S2,STDBY)" strip_icon="path.png" group="extra_pattern">
+      <path wpts="1,2 S1"/>
+      <path wpts="S1, S2 STDBY" approaching_time="1" pitch="auto" throttle="0.4"/>
+      <deroute block="MOB"/>
+    </block>
+    <block name="MOB" strip_button="Circle around here" strip_icon="mob.png" group="base_pattern">
+      <call fun="NavSetWaypointHere(WP_MOB)"/>
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <circle radius="nav_radius" wp="MOB"/>
+      <exception cond="NavBlockTime() > 90" deroute="Land Right AF-TD"/>
+    </block>
+    <block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
+      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
+      <deroute block="land"/>
+    </block>
+    <block name="land">
+      <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
+      <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
+      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
+    </block>
+    <block name="final">
+      <exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
+      <go from="AF" hmode="route" vmode="glide" wp="TD"/>
+    </block>
+    <block name="flare">
+      <go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
+      <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>
@@ -3,25 +3,43 @@
 <module name="extra_dl" dir="datalink">
  <doc>
    <description>Extra datalink (PPRZ transport)</description>
-    <configure name="EXTRA_DL_PORT" value="UARTX|UDPX" description="Select port for extra datalink"/>
+    <configure name="EXTRA_DL_PORT" value="UARTX|UDPX|usb_serial" description="Select port for extra datalink"/>
    <configure name="EXTRA_DL_BAUD" value="B57600" description="Baudrate for extra datalink if link device is UART"/>
  </doc>
  <header>
    <file name="extra_pprz_dl.h"/>
  </header>
  <init fun="extra_pprz_dl_init()"/>
+  <periodic fun="extra_pprz_dl_periodic()" freq="TELEMETRY_FREQUENCY" autorun="TRUE"/>
  <event fun="ExtraDatalinkEvent()"/>
  <makefile target="ap">
    <configure name="EXTRA_DL_PORT" default="uart1" case="upper|lower"/>
    <define name="EXTRA_DOWNLINK_DEVICE" value="$(EXTRA_DL_PORT_LOWER)"/>
    <define name="USE_$(EXTRA_DL_PORT_UPPER)"/>
    <raw>
+      # Check for UDP port
      ifneq (,$(findstring udp,$(EXTRA_DL_PORT_LOWER)))
      include $(CFG_SHARED)/udp.makefile
      else
+      ifneq (,$(findstring usb_serial,$(EXTRA_DL_PORT_LOWER)))
+        # usb_serial telemetry chosen, add files based on architecture
+        ifeq ($(ARCH), lpc21)
+        $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
+        $(TARGET).srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
+        else
+        ifeq ($(ARCH), stm32)
+        $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c
+        else
+        ifneq ($(ARCH), sim)
+        $(error telemetry_transparent_usb currently only implemented for the lpc21 and stm32)
+        endif
+        endif
+        endif
+      else
      EXTRA_DL_BAUD ?= B57600
      $(TARGET).CFLAGS += -D$(EXTRA_DL_PORT_UPPER)_BAUD=$(EXTRA_DL_BAUD)
-      endif
+      endif # USB serial
+      endif # UDP
    </raw>
    <file name="extra_pprz_dl.c"/>
    <file name="pprz_transport.c" dir="pprzlink/src"/>
@@ -48,4 +48,11 @@
      endif
    </raw>
  </makefile>
+  
+  <!-- TODO: make this dynamically loadable - will hopefully work better
+  once nps and HITL are separate targets
+  <makefile target="nps">
+    <file name="nps_ins_vectornav.c" dir="nps"/>
+  </makefile>
+  -->
 </module>
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<!--
+Attributes of root (Radio) tag :
+name: name of RC
+data_min: min width of a pulse to be considered as a data pulse
+data_max: max width of a pulse to be considered as a data pulse
+sync_min: min width of a pulse to be considered as a synchro pulse
+sync_max: max width of a pulse to be considered as a synchro pulse
+min, max and sync are expressed in micro-seconds
+-->
+
+<!--
+Attributes of channel tag :
+ctl: name of the command on the transmitter - only for displaying
+function: logical command
+average: (boolean) channel filtered through several frames (for discrete commands)
+min: minimum pulse length (micro-seconds)
+max: maximum pulse length (micro-seconds)
+neutral: neutral pulse length (micro-seconds)
+Note: a command may be reversed by exchanging min and max values
+-->
+
+<!DOCTYPE radio SYSTEM "../radio.dtd">
+<radio name="Taranis" data_min="987" data_max="2159" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
+ <channel ctl="1"  function="THROTTLE"           min="987" neutral="987" max="2159" average="0"/>
+ <channel ctl="2"  function="ROLL"               min="987" neutral="1573" max="2159" average="0"/>
+ <channel ctl="3"  function="PITCH"              min="2159" neutral="1573" max="1497" average="0"/>
+ <channel ctl="4"  function="YAW"                min="987" neutral="1573" max="2159" average="0"/>
+ <channel ctl="5"  function="SWITCH_A"           min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="6"  function="FLAP"               min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="7"  function="SWITCH_C"           min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="8"  function="MODE"               min="2159" neutral="1573" max="987" average="1"/>
+ <channel ctl="9"  function="SWITCH_E"           min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="10" function="KILL_SWITCH"        min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="11" function="SIM_SRC"           min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="12" function="SWITCH_H"           min="987" neutral="1573" max="2159" average="1"/>
+ <channel ctl="13" function="POT_S1"             min="987" neutral="1573" max="2159" average="0"/>
+ <channel ctl="14" function="POT_S2"             min="987" neutral="1573" max="2159" average="0"/>
+ <channel ctl="15" function="LEFT_SLIDER"        min="987" neutral="1573" max="2159" average="0"/>
+ <channel ctl="16" function="RIGHT_SLIDER"       min="987" neutral="1573" max="2159" average="0"/>
+</radio>
+
+
@@ -0,0 +1,125 @@
+<?xml version="1.0"?>
+<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
+<telemetry>
+  <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">
+      <message name="ALIVE"               period="5"/>
+      <message name="ATTITUDE"            period="4"/>
+      <message name="GPS"                 period="1.05"/>
+      <message name="ESTIMATOR"           period="1.3"/>
+      <message name="WP_MOVED"            period="1.4"/>
+      <message name="CIRCLE"              period="3.05"/>
+      <message name="DESIRED"             period="4.05"/>
+      <message name="BAT"                 period="1.1"/>
+      <message name="SEGMENT"             period="3.2"/>
+      <message name="CALIBRATION"         period="5.1"/>
+      <message name="NAVIGATION_REF"      period="9."/>
+      <message name="NAVIGATION"          period="3."/>
+      <message name="PPRZ_MODE"           period="5."/>
+      <message name="STATE_FILTER_STATUS" period="5."/>
+      <message name="DATALINK_REPORT"     period="5.1"/>
+      <message name="DL_VALUE"            period="1.5"/>
+      <message name="IR_SENSORS"          period="5.2"/>
+      <message name="SURVEY"              period="2.1"/>
+      <message name="GPS_SOL"             period="5.0"/>
+    </mode>
+    <mode name="extremal">
+      <message name="ALIVE"               period="5"/>
+      <message name="GPS"                 period="5.1"/>
+      <message name="ESTIMATOR"           period="5.3"/>
+      <message name="BAT"                 period="10.1"/>
+      <message name="DESIRED"             period="10.2"/>
+      <message name="NAVIGATION"          period="5.4"/>
+      <message name="PPRZ_MODE"           period="7.5"/>
+      <message name="STATE_FILTER_STATUS" period="8."/>
+      <message name="DATALINK_REPORT"     period="5.7"/>
+    </mode>
+    <mode name="raw_sensors">
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="IMU_ACCEL_RAW"     period=".05"/>
+      <message name="IMU_GYRO_RAW"      period=".05"/>
+      <message name="IMU_MAG_RAW"       period=".05"/>
+      <message name="BARO_RAW"          period="0.5"/>
+    </mode>
+    <mode name="scaled_sensors">
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="IMU_GYRO"          period=".075"/>
+      <message name="IMU_ACCEL"         period=".075"/>
+      <message name="IMU_MAG"           period=".1"/>
+    </mode>
+    <mode name="debug_imu">
+      <message name="ATTITUDE"            period="0.1"/>
+      <message name="ALIVE"               period="5"/>
+      <message name="GPS"                 period="5.1"/>
+      <message name="ESTIMATOR"           period="5.3"/>
+      <message name="BAT"                 period="10.1"/>
+      <message name="DESIRED"             period="10.2"/>
+      <message name="NAVIGATION"          period="5.4"/>
+      <message name="PPRZ_MODE"           period="5.5"/>
+      <message name="STATE_FILTER_STATUS" period="5."/>
+      <message name="DATALINK_REPORT"     period="5.7"/>
+      <message name="IMU_ACCEL"           period=".5"/>
+      <message name="IMU_GYRO"            period=".5"/>
+      <message name="IMU_MAG"             period=".5"/>
+      <message name="IMU_ACCEL_RAW"       period=".5"/>
+      <message name="IMU_GYRO_RAW"        period=".5"/>
+      <message name="IMU_MAG_RAW"         period=".5"/>
+    </mode>
+  </process>
+  <process name="Fbw">
+    <mode name="default">
+      <message name="COMMANDS"            period="5"/>
+      <message name="FBW_STATUS"          period="2"/>
+      <message name="ACTUATORS"           period="5"/> <!-- For trimming -->
+    </mode>
+    <mode name="debug">
+      <message name="PPM"                 period="0.5"/>
+      <message name="RC"                  period="0.5"/>
+      <message name="COMMANDS"            period="0.5"/>
+      <message name="FBW_STATUS"          period="1"/>
+      <message name="ACTUATORS"           period="5"/> <!-- For trimming -->
+    </mode>
+  </process>
+  <process name="Extra">
+    <mode name="default">
+    <message name="COMMANDS"            period="0.01"/>
+    </mode>
+  </process>
+</telemetry>
@@ -0,0 +1,173 @@
+<?xml version="1.0"?>
+<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
+<telemetry>
+
+
+  <process name="Main">
+
+    <mode name="default" key_press="d">
+      <message name="AUTOPILOT_VERSION"      period="11.1"/>
+      <message name="DL_VALUE"               period="1.1"/>
+      <message name="ROTORCRAFT_STATUS"      period="1.2"/>
+      <message name="ROTORCRAFT_FP"          period="0.25"/>
+      <message name="ALIVE"                  period="2.1"/>
+      <message name="INS_REF"                period="5.1"/>
+      <message name="ROTORCRAFT_NAV_STATUS"  period="1.6"/>
+      <message name="WP_MOVED"               period="1.3"/>
+      <message name="ROTORCRAFT_CAM"         period="1."/>
+      <message name="GPS_INT"                period=".25"/>
+      <message name="INS"                    period=".25"/>
+      <message name="I2C_ERRORS"             period="4.1"/>
+      <message name="UART_ERRORS"            period="3.1"/>
+      <message name="SUPERBITRF"             period="3"/>
+      <message name="ENERGY"                 period="2.5"/>
+      <message name="DATALINK_REPORT"        period="5.1"/>
+      <message name="STATE_FILTER_STATUS"    period="3.2"/>
+      <message name="AIR_DATA"               period="1.3"/>
+      <message name="SURVEY"                 period="2.5"/>
+      <message name="OPTIC_FLOW_EST"         period="0.05"/>
+      <message name="VECTORNAV_INFO"         period="1.0"/>
+      <message name="ACTUATORS"              period="0.5"/>
+      <message name="MOTOR_MIXING"           period="0.5"/>
+    </mode>
+
+    <mode name="ppm">
+      <message name="DL_VALUE"                 period="0.5"/>
+      <message name="ALIVE"                    period="2.1"/>
+      <message name="ROTORCRAFT_CMD"           period=".05"/>
+      <message name="PPM"                      period="0.5"/>
+      <message name="RC"                       period="0.5"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
+      <message name="ROTORCRAFT_STATUS"        period="1"/>
+      <message name="BEBOP_ACTUATORS"          period="0.2"/>
+    </mode>
+
+    <mode name="raw_sensors">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="IMU_ACCEL_RAW"     period=".05"/>
+      <message name="IMU_GYRO_RAW"      period=".05"/>
+      <message name="IMU_MAG_RAW"       period=".05"/>
+      <message name="BARO_RAW"          period=".1"/>
+      <message name="ARDRONE_NAVDATA" period=".05"/>
+    </mode>
+
+    <mode name="scaled_sensors">
+      <message name="ROTORCRAFT_STATUS"      period="1.2"/>
+      <message name="DL_VALUE"               period="0.5"/>
+      <message name="ALIVE"                  period="2.1"/>
+      <message name="IMU_GYRO_SCALED"        period=".075"/>
+      <message name="IMU_ACCEL_SCALED"       period=".075"/>
+      <message name="IMU_MAG_SCALED"         period=".1"/>
+    </mode>
+
+    <mode name="ahrs">
+      <message name="ROTORCRAFT_STATUS"  period="1.2"/>
+      <message name="DL_VALUE"           period="0.5"/>
+      <message name="ALIVE"              period="2.1"/>
+      <message name="FILTER_ALIGNER"     period="2.2"/>
+      <message name="FILTER"             period=".5"/>
+      <message name="GEO_MAG"            period="5."/>
+      <message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
+      <message name="AHRS_QUAT_INT"      period=".25"/>
+      <message name="AHRS_EULER_INT"     period=".1"/>
+      <message name="AHRS_EULER"         period=".1"/>
+<!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->
+    </mode>
+
+    <mode name="rate_loop">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="RATE_LOOP"         period=".02"/>
+    </mode>
+
+    <mode name="attitude_setpoint_viz" key_press="v">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
+      <message name="AHRS_REF_QUAT" period="0.05"/>
+    </mode>
+
+    <mode name="attitude_loop" key_press="a">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="STAB_ATTITUDE_INT"     period=".03"/>
+      <message name="STAB_ATTITUDE_REF_INT" period=".03"/>
+      <message name="STAB_ATTITUDE_FLOAT"     period=".03"/>
+      <message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
+      <message name="STAB_ATTITUDE_INDI"     period=".25"/>
+    </mode>
+
+    <mode name="vert_loop" key_press="v">
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="DL_VALUE"          period="0.5"/>
+      <message name="ALIVE"             period="0.9"/>
+      <message name="VFF"               period=".05"/>
+      <message name="VFF_EXTENDED"      period=".05"/>
+      <message name="VERT_LOOP"         period=".05"/>
+      <message name="INS_Z"             period=".05"/>
+      <message name="INS"               period=".11"/>
+      <message name="INS_REF"           period="5.1"/>
+    </mode>
+
+    <mode name="vel_guidance" key_press="q">
+      <message name="ALIVE" period="0.9"/>
+      <message name="HYBRID_GUIDANCE" period="0.062"/>
+      <!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
+      <message name="ROTORCRAFT_FP" period="0.8"/>
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
+      <message name="INS_REF" period="5.1"/>
+      <message name="WP_MOVED" period="1.3"/>
+      <message name="GPS_INT" period="1.0"/>
+      <message name="INS" period="1.0"/>
+    </mode>
+
+    <mode name="h_loop" key_press="h">
+      <message name="ALIVE"                 period="0.9"/>
+      <message name="HOVER_LOOP"            period="0.062"/>
+      <message name="GUIDANCE_H_REF_INT"    period="0.062"/>
+      <message name="STAB_ATTITUDE_INT"     period="0.4"/>
+      <message name="STAB_ATTITUDE_FLOAT"   period="0.4"/>
+      <!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
+      <message name="ROTORCRAFT_FP"         period="0.8"/>
+      <message name="ROTORCRAFT_STATUS"     period="1.2"/>
+      <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
+      <message name="INS_REF"               period="5.1"/>
+      <!-- HFF messages are only sent if USE_HFF -->
+      <message name="HFF"                   period=".05"/>
+      <message name="HFF_GPS"               period=".03"/>
+      <message name="HFF_DBG"               period=".2"/>
+    </mode>
+
+    <mode name="aligner">
+      <message name="ALIVE"             period="0.9"/>
+      <message name="FILTER_ALIGNER"    period="0.02"/>
+    </mode>
+
+    <mode name="tune_hover">
+      <message name="DL_VALUE"          period="1.1"/>
+      <message name="ROTORCRAFT_STATUS" period="1.2"/>
+      <message name="ALIVE"             period="2.1"/>
+      <message name="GUIDANCE_H_INT"    period="0.05"/>
+      <message name="ROTORCRAFT_TUNE_HOVER"    period=".1"/>
+      <!-- <message name="GPS_INT"               period=".20"/> -->
+      <!--<message name="INS2"              period=".05"/>
+      <message name="INS3"              period=".20"/>-->
+      <message name="INS_REF"           period="5.1"/>
+    </mode>
+
+  </process>
+
+  <process name="Extra">
+    <mode name="default">
+      <message name="MOTOR_MIXING"            period="0.00625"/>
+    </mode>
+  </process>
+
+</telemetry>
+
@@ -1,6 +1,7 @@
 /*
 * Copyright (C) 2006  Pascal Brisset, Antoine Drouin
 * Copyright (C) 2010  ENAC
+ * Copyright (C) 2016  2016 Michal Podhradsky <http://github.com/podhrmic>
 *
 * This file is part of paparazzi.
 *
@@ -20,8 +21,31 @@
 * Boston, MA 02111-1307, USA.
 *
 */
+/**
+ * @file "modules/datalink/extra_pprz_dl.c"
+ * Extra datalink and telemetry using PPRZ protocol
+ *
+ * NOTES (for future reference):
+ * This note is not needed unless we want to define our own messages - in such case you would define the two
+ * structs below and then define individual messages and registered them in the same way as
+ * periodic telemetry.
+ * struct telemetry_cb_slots telemetry_cbs_logger[TELEMETRY_PPRZ_NB_MSG] = TELEMETRY_PPRZ_CBS;
+ * struct periodic_telemetry logger_telemetry = { TELEMETRY_PPRZ_NB_MSG, telemetry_cbs_logger };
+ *
+ * The registration should be done in the init function:
+ * register_periodic_telemetry(&extra_telemetry, PPRZ_MSG_ID_xxx, send_xxx_message);
+ *
+ * Two extra notes for the periodic function:
+ * 1) this sends registered messages from ExtraTelemetry process (as mentioned above) over dedicated port do:
+ *    periodic_telemetry_send_ExtraTelemetryr(&extra_telemetry, &pprz_tp_extra.trans_tx, &(EXTRA_TELEMETRY_PORT).device);
+ * 2) to send ExtraTelemetry messages over default channel just change to:
+ *    periodic_telemetry_send_ExtraTelemetry(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
+ */
+#define PERIODIC_C_EXTRA

 #include "modules/datalink/extra_pprz_dl.h"
+#include "subsystems/datalink/telemetry.h"
+

 struct pprz_transport extra_pprz_tp;

@@ -30,3 +54,11 @@ void extra_pprz_dl_init(void)
  pprz_transport_init(&extra_pprz_tp);
 }

+void extra_pprz_dl_periodic(void)
+{
+#if PERIODIC_TELEMETRY
+  // send periodic messages as defined in the Extra process, we are using DefaultPeriodic so we can send standard messages
+  periodic_telemetry_send_Extra(DefaultPeriodic, &extra_pprz_tp.trans_tx, &(EXTRA_DOWNLINK_DEVICE).device);
+#endif
+}
+
@@ -1,5 +1,6 @@
 /*
 * Copyright (C) 2010  ENAC
+ * Copyright (C) 2016  2016 Michal Podhradsky <http://github.com/podhrmic>
 *
 * This file is part of paparazzi.
 *
@@ -19,12 +20,10 @@
 * Boston, MA 02111-1307, USA.
 *
 */
-
-/** \file extra_pprz_dl.h
- *  \brief Extra datalink using PPRZ protocol
- *
+/**
+ * @file "modules/datalink/extra_pprz_dl.h"
+ * Extra datalink and telemetry using PPRZ protocol
 */
-
 #ifndef EXTRA_PPRZ_DL_H
 #define EXTRA_PPRZ_DL_H

@@ -35,12 +34,15 @@
 #include "mcu_periph/udp.h"
 #endif

+#if USE_USB_SERIAL
+#include "mcu_periph/usb_serial.h"
+#endif
+
 /* PPRZ transport structure */
 extern struct pprz_transport extra_pprz_tp;

 /* Datalink Event */
-
-#define ExtraDatalinkEvent() {                                          \
+#define ExtraDatalinkEvent() {                            \
    pprz_check_and_parse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp, dl_buffer, &dl_msg_available); \
    DlCheckAndParse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp.trans_tx, dl_buffer); \
  }
@@ -48,5 +50,11 @@ extern struct pprz_transport extra_pprz_tp;
 /** Init function */
 extern void extra_pprz_dl_init(void);

+/** Periodic function
+ *
+ * should be called at TELEMETRY_FREQUENCY
+ */
+extern void extra_pprz_dl_periodic(void);
+
 #endif /* EXTRA_PPRZ_DL_H */

@@ -63,7 +63,7 @@ struct spi_transaction bluegiga_spi;

 uint8_t broadcast_msg[20];

-void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans);
+void bluegiga_load_tx(struct bluegiga_periph *p);
 void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data);
 void bluegiga_receive(struct spi_transaction *trans);

@@ -214,7 +214,7 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data)
 }

 /* Load waiting data into tx peripheral buffer */
-void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans)
+void bluegiga_load_tx(struct bluegiga_periph *p)
 {
  uint8_t packet_len;
  // check data available in buffer to send
@@ -330,7 +330,7 @@ void bluegiga_receive(struct spi_transaction *trans)
    }

    // load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register
-    bluegiga_load_tx(&bluegiga_p, trans);
+    bluegiga_load_tx(&bluegiga_p);

    // register spi slave read for next transaction
    spi_slave_register(&(BLUEGIGA_SPI_DEV), trans);
@@ -70,7 +70,7 @@ static inline void main_init(void)
  mcu_init();
  sys_time_register_timer((0.5 / PERIODIC_FREQUENCY), NULL);
  downlink_init();
-  ppz_can_init(main_on_can_msg);
+  ppz_can_init((can_rx_callback_t)main_on_can_msg);
 }

 static inline void main_periodic_task(void)
@@ -101,7 +101,7 @@ void nps_atmosphere_update(double dt)
  req_time += dt;
  if (req_time - nps_atmosphere.last_world_env_req >= NPS_WORLD_ENV_UPDATE) {
    nps_atmosphere.last_world_env_req = req_time;
-    nps_ivy_send_WORLD_ENV_REQ();
+    nps_ivy_send_world_env = true;
  }

  nps_fdm_set_wind_ned(nps_atmosphere.wind.x, nps_atmosphere.wind.y, nps_atmosphere.wind.z);
@@ -15,14 +15,9 @@
 #if defined MOTOR_MIXING_NB_MOTOR
 #define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR
 #else
-#ifdef NPS_ACTUATOR_NAMES
 #define NPS_COMMANDS_NB COMMANDS_NB
-#else
-/* not using explicitly set NPS_ACTUATOR_NAMES -> throttle,roll,pitch,yaw commands */
-#define NPS_COMMANDS_NB 4
-#endif
-#endif
-#endif
+#endif /* #if defined MOTOR_MIXING_NB_MOTOR */
+#endif /* #ifndef NPS_COMMANDS_NB */

 struct NpsAutopilot {
  double commands[NPS_COMMANDS_NB];
@@ -77,7 +77,9 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha

  autopilot.launch = FALSE;

-  nps_radio_control_init(type_rc, num_rc_script, rc_dev);
+  if (rc_dev != NULL)  {
+    nps_radio_control_init(type_rc, num_rc_script, rc_dev);
+  }
  nps_electrical_init();

  nps_bypass_ahrs = NPS_BYPASS_AHRS;
@@ -180,10 +182,8 @@ void nps_autopilot_run_step(double time)
 #ifdef COMMAND_YAW
  PRINT_CONFIG_VAR(COMMAND_YAW)
  autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ;
-#else
-  autopilot.commands[3] = 0.;
-#endif
-#endif
+#endif /* COMMAND_YAW */
+#endif /* NPS_ACTUATOR_NAMES */

  // do the launch when clicking launch in GCS
  autopilot.launch = launch && !kill_throttle;
@@ -52,7 +52,7 @@
 // end ignore unused param warnings in JSBSim
 #pragma GCC diagnostic pop

-
+#include "nps_autopilot.h"
 #include "nps_fdm.h"
 #include "math/pprz_geodetic.h"
 #include "math/pprz_geodetic_double.h"
@@ -124,12 +124,13 @@
 * Around 1/10000 seems to be good for ground impacts
 */
 #define MIN_DT (1.0/10240.0)
+// TODO: maybe lower for slower CPUs & HITL?
+//#define MIN_DT (1.0/1000.0)

 using namespace JSBSim;
 using namespace std;

 static void feed_jsbsim(double *commands, int commands_nb);
-static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder);
 static void fetch_state(void);
 static int check_for_nan(void);

@@ -289,7 +290,7 @@ void nps_fdm_set_temperature(double temp, double h)
 * @param commands    Pointer to array of doubles holding actuator commands
 * @param commands_nb Number of commands (length of array)
 */
-static void feed_jsbsim(double *commands, int commands_nb)
+static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
 {
 #ifdef NPS_ACTUATOR_NAMES
  char buf[64];
@@ -302,42 +303,48 @@ static void feed_jsbsim(double *commands, int commands_nb)
    property = string(buf);
    FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
  }
-#else
-  if (commands_nb != 4) {
-    cerr << "commands_nb must be 4!" << endl;
-    exit(-1);
-  }
-  /* call version that directly feeds throttle, aileron, elevator, rudder */
-  feed_jsbsim(commands[COMMAND_THROTTLE], commands[COMMAND_ROLL], commands[COMMAND_PITCH], commands[3]);
-#endif
-}
+#else /* use COMMAND names */

-static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder)
-{
+  // get FGFCS instance
  FGFCS *FCS = FDMExec->GetFCS();
-  FGPropulsion *FProp = FDMExec->GetPropulsion();

  // Set trims
  FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM);
  FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM);
  FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM);

-  // Set commands
-  FCS->SetDaCmd(aileron);
-  FCS->SetDeCmd(elevator);
-  FCS->SetDrCmd(rudder);
-
+#ifdef COMMAND_THROTTLE
+  FGPropulsion *FProp = FDMExec->GetPropulsion();
  for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) {
-    FCS->SetThrottleCmd(i, throttle);
+    FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]);

-    if (throttle > 0.01) {
+    // Hack to show spinning propellers in flight gear models
+    if (commands[COMMAND_THROTTLE] > 0.01) {
      FProp->SetStarter(1);
    } else {
      FProp->SetStarter(0);
    }
  }
-}
+#endif /* COMMAND_THROTTLE */

+#ifdef COMMAND_ROLL
+  FCS->SetDaCmd(commands[COMMAND_ROLL]);
+#endif /* COMMAND_ROLL */
+
+#ifdef COMMAND_PITCH
+  FCS->SetDeCmd(commands[COMMAND_PITCH]);
+#endif /* COMMAND_PITCH */
+
+#ifdef COMMAND_YAW
+  FCS->SetDrCmd(commands[COMMAND_YAW]);
+#endif /* COMMAND_YAW */
+
+#ifdef COMMAND_FLAP
+  FCS->SetDfCmd(commands[COMMAND_FLAP]);
+#endif /* COMMAND_FLAP */
+
+#endif /* NPS_ACTUATOR_NAMES */
+}

 /**
 * Populates the NPS fdm struct after a simulation step.
@@ -597,7 +604,8 @@ static void init_jsbsim(double dt)
  }

  // initial commands to zero
-  feed_jsbsim(0.0, 0.0, 0.0, 0.0);
+  double init_commands[NPS_COMMANDS_NB] = {0.0};
+  feed_jsbsim(init_commands, NPS_COMMANDS_NB);

  //loop JSBSim once w/o integrating
  if (!FDMExec->RunIC()) {
@@ -10,10 +10,11 @@
 #include <stdio.h>
 #include <math.h>
 #include <stdlib.h>
-
+#include <pthread.h>

 #include "std.h"
 #include "../flight_gear.h"
+#include "nps_main.h"
 #include "nps_fdm.h"
 #include "nps_atmosphere.h"

@@ -25,6 +26,9 @@ static struct  {
  unsigned int time_offset;
 } flightgear;

+pthread_t th_fg_rx; // fligh gear receive thread
+
+void* nps_flightgear_receive(void* data __attribute__((unused)));

 double htond(double x)
 {
@@ -98,6 +102,9 @@ void nps_flightgear_init(const char *host,  unsigned int port, unsigned int port
  time_t t = time(NULL);
  flightgear.initial_time = t;
  flightgear.time_offset = time_offset;
+
+  // launch rx thread
+  pthread_create(&th_fg_rx, NULL, nps_flightgear_receive, NULL);
 }

 /**
@@ -221,7 +228,8 @@ void nps_flightgear_send()
 /**
 * Receive Flight Gear environment messages
 */
-void nps_flightgear_receive() {
+void* nps_flightgear_receive(void* data __attribute__((unused)))
+{

  if (flightgear.socket_in != -1) {
    // socket is correctly opened
@@ -230,32 +238,37 @@ void nps_flightgear_receive() {
    size_t s_env = sizeof(env);
    int bytes_read;

-    //read first message
-    memset(&env, 0, s_env);
-    bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
-    while (bytes_read != -1) { // while we read a message (empty buffer)
-      if (bytes_read == (int)s_env){
-        // Update wind info
-        nps_atmosphere_set_wind_ned(
-            (double)env.wind_from_north,
-            (double)env.wind_from_east,
-            (double)env.wind_from_down);
-      }
-      else {
-        //error
-        printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env);
-      }
-
-      //read next message
+    while(TRUE)
+    {
+      //read first message
      memset(&env, 0, s_env);
-      bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
-    }
+      bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL);
+      while (bytes_read != -1) { // while we read a message (empty buffer)
+        if (bytes_read == (int)s_env){
+          // Update wind info
+          pthread_mutex_lock(&fdm_mutex);
+          nps_atmosphere_set_wind_ned(
+              (double)env.wind_from_north,
+              (double)env.wind_from_east,
+              (double)env.wind_from_down);
+          pthread_mutex_unlock(&fdm_mutex);
+        }
+        else {
+          //error
+          printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env);
+        }

-    if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) {
-      perror("nps_flightgear_receive recvfrom()");
+        //read next message
+        memset(&env, 0, s_env);
+        bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL);
+      }
+
+      if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) {
+        perror("nps_flightgear_receive recvfrom()");
+      }
    }
  }
-
+  return NULL;
 }


@@ -5,7 +5,6 @@
 extern void nps_flightgear_init(const char* host,  unsigned int port, unsigned int port_in, unsigned int time_offset);
 extern void nps_flightgear_send();
 extern void nps_flightgear_send_fdm();
-extern void nps_flightgear_receive();


 #endif /* NPS_FLIGHTGEAR_H */
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+#ifndef NPS_INS_H
+#define NPS_INS_H
+
+#include "std.h"
+#include "nps_fdm.h"
+
+// if undefined, match with control frequency because that is how it should be used
+#ifndef INS_FREQUENCY
+#ifdef CONTROL_FREQUENCY
+#define INS_FREQUENCY CONTROL_FREQUENCY
+#else
+#define INS_FREQUENCY PERIODIC_FREQUENCY
+#endif
+#endif
+
+extern uint8_t *ins_buffer;
+
+extern void nps_ins_init(void);
+void nps_ins_fetch_data(struct NpsFdm *fdm_ins);
+uint16_t nps_ins_fill_buffer(void);
+
+#endif /* NPS_INS_H */
@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "nps_ins.h"
+#include <sys/time.h>
+#include "nps_fdm.h"
+#include <time.h>
+#include <stdio.h>
+
+/*
+ * Vectornav info
+ */
+#define VN_DATA_START 10
+#define VN_BUFFER_SIZE 512
+#define GPS_SEC_IN_DAY 86400
+
+static uint8_t VN_SYNC = 0xFA;
+static uint8_t VN_OUTPUT_GROUP = 0x39;
+static uint16_t VN_GROUP_FIELD_1 = 0x01E9;
+static uint16_t VN_GROUP_FIELD_2 = 0x061A;
+static uint16_t VN_GROUP_FIELD_3 = 0x0140;
+static uint16_t VN_GROUP_FIELD_4 = 0x0009;
+
+uint8_t vn_buffer[VN_BUFFER_SIZE];
+
+uint8_t *ins_buffer;
+
+struct VectornavData {
+  uint64_t TimeStartup;
+  float YawPitchRoll[3];
+  float AngularRate[3];
+  double Position[3];
+  float Velocity[3];
+  float Accel[3];
+  uint64_t Tow;
+  uint8_t NumSats;
+  uint8_t Fix;
+  float PosU[3];
+  float VelU;
+  float LinearAccelBody[3];
+  float YprU[3];
+  uint16_t InsStatus;
+  float VelBody[3];
+};
+
+struct VectornavData vn_data;
+
+void ins_vectornav_init(void) {}
+void ins_vectornav_event(void) {}
+
+/**
+ * Calculates the 16-bit CRC for the given ASCII or binary message.
+ * The CRC is calculated over the packet starting just after the sync byte (not including the sync byte)
+ * and ending at the end of payload.
+ */
+unsigned short vn_calculate_crc(unsigned char data[], unsigned int length)
+{
+  unsigned int i;
+  unsigned short crc = 0;
+  for (i = 0; i < length; i++) {
+    crc = (unsigned char)(crc >> 8) | (crc << 8);
+    crc ^= data[i];
+    crc ^= (unsigned char)(crc & 0xff) >> 4;
+    crc ^= crc << 12;
+    crc ^= (crc & 0x00ff) << 5;
+  }
+  return crc;
+}
+
+void nps_ins_init(void)
+{
+  ins_buffer = &vn_buffer[0];
+}
+
+
+/**
+ * @return GPS TOW
+ */
+static uint64_t vn_get_time_of_week(void)
+{
+  struct timeval curTime;
+  gettimeofday(&curTime, NULL);
+  int milli = curTime.tv_usec / 1000;
+  struct tm t_res;
+  localtime_r(&curTime.tv_sec, &t_res);
+  struct tm *tt = &t_res;
+
+  uint64_t tow = GPS_SEC_IN_DAY * tt->tm_wday + 3600 * tt->tm_hour + 60 * tt->tm_min + tt->tm_sec; // sec
+  tow = tow * 1000; // tow to ms
+  tow = tow + milli; // tow with added ms
+  tow = tow * 1e6; // tow in nanoseconds
+
+  return tow;
+}
+
+void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
+{
+  struct NpsFdm fdm_data;
+  memcpy(&fdm_data, fdm_ins, sizeof(struct NpsFdm));
+
+  // Timestamp
+  vn_data.TimeStartup = (uint64_t)(fdm_data.time * 1000000000.0);
+
+  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
+  vn_data.YawPitchRoll[0] = DegOfRad((float)fdm_data.ltp_to_body_eulers.psi); // yaw
+  vn_data.YawPitchRoll[1] = DegOfRad((float)fdm_data.ltp_to_body_eulers.theta); // pitch
+  vn_data.YawPitchRoll[2] = DegOfRad((float)fdm_data.ltp_to_body_eulers.phi); // roll
+
+  // Rates (imu frame), float, [rad/s]
+  vn_data.AngularRate[0] = (float)fdm_data.body_ecef_rotvel.p;
+  vn_data.AngularRate[1] = (float)fdm_data.body_ecef_rotvel.q;
+  vn_data.AngularRate[2] = (float)fdm_data.body_ecef_rotvel.r;
+
+  //Pos LLA, double,[beg, deg, m]
+  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
+  vn_data.Position[0] = DegOfRad(fdm_data.lla_pos.lat);
+  vn_data.Position[1] = DegOfRad(fdm_data.lla_pos.lon);
+  vn_data.Position[2] = fdm_data.lla_pos.alt; // TODO: make sure it shows the correct starting point
+
+  //VelNed, float [m/s]
+  //The estimated velocity in the North East Down (NED) frame, given in m/s.
+  vn_data.Velocity[0] = (float)fdm_data.ltp_ecef_vel.x;
+  vn_data.Velocity[1] = (float)fdm_data.ltp_ecef_vel.y;
+  vn_data.Velocity[2] = (float)fdm_data.ltp_ecef_vel.z;
+
+  // Accel (imu-frame), float, [m/s^-2]
+  vn_data.Accel[0] = (float)fdm_data.body_ecef_accel.x;
+  vn_data.Accel[1] = (float)fdm_data.body_ecef_accel.y;
+  vn_data.Accel[2] = (float)fdm_data.body_ecef_accel.z;
+
+  // tow (in nanoseconds), uint64
+  vn_data.Tow = vn_get_time_of_week();
+
+  //num sats, uint8
+  vn_data.NumSats = 8; // random number
+
+  //gps fix, uint8
+  vn_data.Fix = 3; // 3D fix
+
+  //posU, float[3]
+  // TODO
+
+  //velU, float
+  // TODO
+
+  //linear acceleration imu-body frame, float [m/s^2]
+  vn_data.LinearAccelBody[0] = (float)fdm_data.ltp_ecef_vel.x;
+  vn_data.LinearAccelBody[1] = (float)fdm_data.ltp_ecef_vel.y;
+  vn_data.LinearAccelBody[2] = (float)fdm_data.ltp_ecef_vel.z;
+
+  //YprU, float[3]
+  // TODO
+
+  //instatus, uint16
+  vn_data.InsStatus = 0x02;
+
+  //Vel body, float [m/s]
+  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
+  vn_data.VelBody[0] = (float)fdm_data.body_accel.x;
+  vn_data.VelBody[1] = (float)fdm_data.body_accel.y;
+  vn_data.VelBody[2] = (float)fdm_data.body_accel.z;
+}
+
+
+uint16_t nps_ins_fill_buffer(void)
+{
+  static uint16_t idx;
+
+  vn_buffer[0] = VN_SYNC;
+  vn_buffer[1] = VN_OUTPUT_GROUP;
+  vn_buffer[2] = (uint8_t)(VN_GROUP_FIELD_1 >> 8);
+  vn_buffer[3] = (uint8_t)(VN_GROUP_FIELD_1);
+  vn_buffer[4] = (uint8_t)(VN_GROUP_FIELD_2 >> 8);
+  vn_buffer[5] = (uint8_t)(VN_GROUP_FIELD_2);
+  vn_buffer[6] = (uint8_t)(VN_GROUP_FIELD_3 >> 8);
+  vn_buffer[7] = (uint8_t)(VN_GROUP_FIELD_3);
+  vn_buffer[8] = (uint8_t)(VN_GROUP_FIELD_4 >> 8);
+  vn_buffer[9] = (uint8_t)(VN_GROUP_FIELD_4);
+
+  idx = VN_DATA_START;
+
+  // Timestamp
+  memcpy(&vn_buffer[idx], &vn_data.TimeStartup, sizeof(uint64_t));
+  idx += sizeof(uint64_t);
+
+  //Attitude, float, [degrees], yaw, pitch, roll, NED frame
+  memcpy(&vn_buffer[idx], &vn_data.YawPitchRoll, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  // Rates (imu frame), float, [rad/s]
+  memcpy(&vn_buffer[idx], &vn_data.AngularRate, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  //Pos LLA, double,[beg, deg, m]
+  //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully.
+  memcpy(&vn_buffer[idx], &vn_data.Position, 3 * sizeof(double));
+  idx += 3 * sizeof(double);
+
+  //VelNed, float [m/s]
+  //The estimated velocity in the North East Down (NED) frame, given in m/s.
+  memcpy(&vn_buffer[idx], &vn_data.Velocity, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  // Accel (imu-frame), float, [m/s^-2]
+  memcpy(&vn_buffer[idx], &vn_data.Accel, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  // tow (in nanoseconds), uint64
+  memcpy(&vn_buffer[idx], &vn_data.Tow, sizeof(uint64_t));
+  idx += sizeof(uint64_t);
+
+  //num sats, uint8
+  vn_buffer[idx] = vn_data.NumSats;
+  idx++;
+
+  //gps fix, uint8
+  vn_buffer[idx] = vn_data.Fix;
+  idx++;
+
+  //posU, float[3]
+  memcpy(&vn_buffer[idx], &vn_data.PosU, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  //velU, float
+  memcpy(&vn_buffer[idx], &vn_data.VelU, sizeof(float));
+  idx += sizeof(float);
+
+  //linear acceleration imu-body frame, float [m/s^2]
+  memcpy(&vn_buffer[idx], &vn_data.LinearAccelBody, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  //YprU, float[3]
+  memcpy(&vn_buffer[idx], &vn_data.YprU, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  //instatus, uint16
+  memcpy(&vn_buffer[idx], &vn_data.InsStatus, sizeof(uint16_t));
+  idx += sizeof(uint16_t);
+
+  //Vel body, float [m/s]
+  // The estimated velocity in the body (i.e. imu) frame, given in m/s.
+  memcpy(&vn_buffer[idx], &vn_data.VelBody, 3 * sizeof(float));
+  idx += 3 * sizeof(float);
+
+  // calculate checksum & send
+  uint16_t chk = vn_calculate_crc(&vn_buffer[1], idx - 1);
+  vn_buffer[idx] = (uint8_t)(chk >> 8);
+  idx++;
+  vn_buffer[idx] = (uint8_t)(chk & 0xFF);
+  idx++;
+
+  return idx;
+}
@@ -5,7 +5,9 @@
 #include <sys/types.h>
 #include <unistd.h>
 #include <Ivy/ivy.h>
-#include <Ivy/ivyglibloop.h>
+
+#include <Ivy/ivyloop.h>
+#include <pthread.h>

 #include "generated/airframe.h"
 #include "math/pprz_algebra_float.h"
@@ -16,7 +18,9 @@
 #include "nps_sensors.h"
 #include "nps_atmosphere.h"

-//#include "subsystems/navigation/common_flight_plan.h"
+#include "generated/settings.h"
+#include "pprzlink/dl_protocol.h"
+#include "subsystems/datalink/downlink.h"

 #if USE_GPS
 #include "subsystems/gps.h"
@@ -24,6 +28,12 @@

 #include NPS_SENSORS_PARAMS

+pthread_t th_ivy_main; // runs main Ivy loop
+static MsgRcvPtr ivyPtr = NULL;
+static int seq = 1;
+static int ap_launch_index;
+
+
 /* Gaia Ivy functions */
 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
                         void *user_data __attribute__((unused)),
@@ -34,6 +44,18 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
                          void *user_data __attribute__((unused)),
                          int argc __attribute__((unused)), char *argv[]);

+void* ivy_main_loop(void* data __attribute__((unused)));
+
+int find_launch_index(void);
+
+
+void* ivy_main_loop(void* data __attribute__((unused)))
+{
+  IvyMainLoop();
+
+  return NULL;
+}
+
 void nps_ivy_init(char *ivy_bus)
 {
  const char *agent_name = AIRFRAME_NAME"_NPS";
@@ -56,6 +78,14 @@ void nps_ivy_init(char *ivy_bus)
  } else {
    IvyStart(ivy_bus);
  }
+
+  nps_ivy_send_world_env = false;
+
+  ap_launch_index = find_launch_index();
+
+  // Launch separate thread with IvyMainLoop()
+  pthread_create(&th_ivy_main, NULL, ivy_main_loop, NULL);
+
 }

 /*
@@ -90,8 +120,7 @@ static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
 /*
 * Send a WORLD_ENV_REQ message
 */
-static MsgRcvPtr ivyPtr = NULL;
-static int seq = 1;
+

 void nps_ivy_send_WORLD_ENV_REQ(void)
 {
@@ -102,24 +131,41 @@ void nps_ivy_send_WORLD_ENV_REQ(void)
  }

  int pid = (int)getpid();
+
  // Bind to the reply
  ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq);
+
  // Send actual request
+  struct NpsFdm fdm_ivy;
+  memcpy(&fdm_ivy, &fdm, sizeof(struct NpsFdm));
+
  IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
      pid, seq,
-      DegOfRad(fdm.lla_pos_pprz.lat),
-      DegOfRad(fdm.lla_pos_pprz.lon),
-      (fdm.hmsl),
-      (fdm.ltpprz_pos.x),
-      (fdm.ltpprz_pos.y),
-      (fdm.ltpprz_pos.z));
+      DegOfRad(fdm_ivy.lla_pos_pprz.lat),
+      DegOfRad(fdm_ivy.lla_pos_pprz.lon),
+      (fdm_ivy.hmsl),
+      (fdm_ivy.ltpprz_pos.x),
+      (fdm_ivy.ltpprz_pos.y),
+      (fdm_ivy.ltpprz_pos.z));
  seq++;
+
+  nps_ivy_send_world_env = false;
 }

+int find_launch_index(void)
+{
+  static const char ap_launch[] = "lau"; // short name has only 3 first letters
+  char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
+
+  // list through the settinsg
+  for (uint8_t idx=0;idx<NB_SETTING;idx++) {
+   if (strcmp(ap_settings[idx],ap_launch) == 0) {
+     return (int)idx;
+    }
+  }
+  return -1;
+}

-#include "generated/settings.h"
-#include "pprzlink/dl_protocol.h"
-#include "subsystems/datalink/downlink.h"
 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
                          void *user_data __attribute__((unused)),
                          int argc __attribute__((unused)), char *argv[])
@@ -133,68 +179,90 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
   * but since we currently change this variable via settings we have to allow it
   * TODO: only allow changing the datalink_enabled setting
   */
-
  uint8_t index = atoi(argv[2]);
  float value = atof(argv[3]);
  DlSetting(index, value);
  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
  printf("setting %d %f\n", index, value);
+
+  /*
+   * In case of HITL, update autopilot.launch from DL_SETTINGS
+   * so the plane can be properly launched.
+   *
+   * In case of STIL nps_update_launch_from_dl() is an empty function
+   */
+  if ((ap_launch_index >= 0) || (ap_launch_index < NB_SETTING)) {
+    if (index==ap_launch_index){
+      nps_update_launch_from_dl(value);
+    }
+  }
 }

-void nps_ivy_display(void)
+
+void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
 {
+  struct NpsFdm fdm_ivy;
+  memcpy (&fdm_ivy, fdm_data, sizeof(struct NpsFdm));
+
+  struct NpsSensors sensors_ivy;
+  memcpy (&sensors_ivy, sensors_data, sizeof(struct NpsSensors));
+
  IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
             AC_ID,
-             DegOfRad(fdm.body_ecef_rotvel.p),
-             DegOfRad(fdm.body_ecef_rotvel.q),
-             DegOfRad(fdm.body_ecef_rotvel.r),
-             DegOfRad(fdm.ltp_to_body_eulers.phi),
-             DegOfRad(fdm.ltp_to_body_eulers.theta),
-             DegOfRad(fdm.ltp_to_body_eulers.psi));
+             DegOfRad(fdm_ivy.body_ecef_rotvel.p),
+             DegOfRad(fdm_ivy.body_ecef_rotvel.q),
+             DegOfRad(fdm_ivy.body_ecef_rotvel.r),
+             DegOfRad(fdm_ivy.ltp_to_body_eulers.phi),
+             DegOfRad(fdm_ivy.ltp_to_body_eulers.theta),
+             DegOfRad(fdm_ivy.ltp_to_body_eulers.psi));
  IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
             AC_ID,
-             (fdm.lla_pos_pprz.lat),
-             (fdm.lla_pos_geod.lat),
-             (fdm.lla_pos_geoc.lat),
-             (fdm.lla_pos_pprz.lon),
-             (fdm.lla_pos_geod.lon),
-             (fdm.lla_pos_pprz.alt),
-             (fdm.lla_pos_geod.alt),
-             (fdm.agl),
-             (fdm.hmsl));
+             (fdm_ivy.lla_pos_pprz.lat),
+             (fdm_ivy.lla_pos_geod.lat),
+             (fdm_ivy.lla_pos_geoc.lat),
+             (fdm_ivy.lla_pos_pprz.lon),
+             (fdm_ivy.lla_pos_geod.lon),
+             (fdm_ivy.lla_pos_pprz.alt),
+             (fdm_ivy.lla_pos_geod.alt),
+             (fdm_ivy.agl),
+             (fdm_ivy.hmsl));
  IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
             AC_ID,
-             (fdm.ltpprz_ecef_accel.x),
-             (fdm.ltpprz_ecef_accel.y),
-             (fdm.ltpprz_ecef_accel.z),
-             (fdm.ltpprz_ecef_vel.x),
-             (fdm.ltpprz_ecef_vel.y),
-             (fdm.ltpprz_ecef_vel.z),
-             (fdm.ltpprz_pos.x),
-             (fdm.ltpprz_pos.y),
-             (fdm.ltpprz_pos.z));
+             (fdm_ivy.ltpprz_ecef_accel.x),
+             (fdm_ivy.ltpprz_ecef_accel.y),
+             (fdm_ivy.ltpprz_ecef_accel.z),
+             (fdm_ivy.ltpprz_ecef_vel.x),
+             (fdm_ivy.ltpprz_ecef_vel.y),
+             (fdm_ivy.ltpprz_ecef_vel.z),
+             (fdm_ivy.ltpprz_pos.x),
+             (fdm_ivy.ltpprz_pos.y),
+             (fdm_ivy.ltpprz_pos.z));
  IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
             AC_ID,
-             DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x) + sensors.gyro.bias_initial.x),
-             DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y) + sensors.gyro.bias_initial.y),
-             DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z) + sensors.gyro.bias_initial.z));
+             DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x),
+             DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y),
+             DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z));

  /* transform magnetic field to body frame */
  struct DoubleVect3 h_body;
-  double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);
+  double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h);

  IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
             AC_ID,
-             ((sensors.accel.value.x - sensors.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
-             ((sensors.accel.value.y - sensors.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
-             ((sensors.accel.value.z - sensors.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
+             ((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
+             ((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
+             ((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
             h_body.x,
             h_body.y,
             h_body.z);

  IvySendMsg("%d NPS_WIND %f %f %f",
             AC_ID,
-             fdm.wind.x,
-             fdm.wind.y,
-             fdm.wind.z);
+             fdm_ivy.wind.x,
+             fdm_ivy.wind.y,
+             fdm_ivy.wind.z);
+
+  if(nps_ivy_send_world_env){
+    nps_ivy_send_WORLD_ENV_REQ();
+  }
 }
@@ -1,8 +1,13 @@
 #ifndef NPS_IVY
 #define NPS_IVY

+#include "nps_fdm.h"
+#include "nps_sensors.h"
+
+bool nps_ivy_send_world_env;
+
 extern void nps_ivy_init(char *ivy_bus);
-extern void nps_ivy_display(void);
+extern void nps_ivy_display(struct NpsFdm* fdm_ivy, struct NpsSensors* sensors_ivy);
 extern void nps_ivy_send_WORLD_ENV_REQ(void);

 #endif /* NPS_IVY */
@@ -1,6 +1,62 @@
 #ifndef NPS_MAIN_H
 #define NPS_MAIN_H

-extern void nps_set_time_factor(float time_factor);
+#include <pthread.h>
+#include <sys/time.h>
+#include "nps_fdm.h"
+#include "mcu_periph/sys_time.h"
+#include "nps_atmosphere.h"
+#include "nps_sensors.h"
+#include "nps_autopilot.h"
+
+#define SIM_DT     (1./SYS_TIME_FREQUENCY)
+#define DISPLAY_DT (1./30.)
+#define HOST_TIMEOUT_MS 40
+
+pthread_t th_flight_gear; // sends/receives flight gear packets
+pthread_t th_display_ivy; // sends Ivy messages
+pthread_t th_main_loop; // handles simulation
+
+pthread_mutex_t fdm_mutex; // mutex for fdm data
+
+int pauseSignal; // for catching SIGTSTP
+
+bool nps_main_parse_options(int argc, char **argv);
+
+int nps_main_init(int argc, char **argv);
+void nps_radio_and_autopilot_init(void);
+void nps_main_run_sim_step(void);
+void nps_set_time_factor(float time_factor);
+
+void* nps_main_loop(void* data __attribute__((unused)));
+void* nps_flight_gear_loop(void* data __attribute__((unused)));
+void* nps_main_display(void* data __attribute__((unused)));
+
+void tstp_hdl(int n __attribute__((unused)));
+void cont_hdl(int n __attribute__((unused)));
+
+double time_to_double(struct timeval *t);
+double ntime_to_double(struct timespec *t);
+
+void nps_update_launch_from_dl(uint8_t value);
+
+struct NpsMain {
+  double real_initial_time;
+  double scaled_initial_time;
+  double host_time_factor;
+  double sim_time;
+  double display_time;
+  char *fg_host;
+  unsigned int fg_port;
+  unsigned int fg_port_in;
+  unsigned int fg_time_offset;
+  int fg_fdm;
+  char *js_dev;
+  char *spektrum_dev;
+  int rc_script;
+  char *ivy_bus;
+};
+
+struct NpsMain nps_main;

 #endif /* NPS_MAIN_H */
@@ -1,6 +1,7 @@
 /*
 * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
 * Copyright (C) 2012 The Paparazzi Team
+ * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
 *
 * This file is part of paparazzi.
 *
@@ -20,51 +21,14 @@
 * Boston, MA 02111-1307, USA.
 */

-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
+#include "nps_main.h"
 #include <signal.h>
-#include <glib.h>
-#include <sys/time.h>
+#include <stdio.h>
 #include <getopt.h>

-#include "nps_main.h"
-#include "nps_fdm.h"
-#include "nps_sensors.h"
-#include "nps_atmosphere.h"
-#include "nps_autopilot.h"
-#include "nps_ivy.h"
 #include "nps_flightgear.h"
-#include "mcu_periph/sys_time.h"
-#define SIM_DT     (1./SYS_TIME_FREQUENCY)
-#define DISPLAY_DT (1./30.)
-#define HOST_TIMEOUT_MS 40

-
-static struct {
-  double real_initial_time;
-  double scaled_initial_time;
-  double host_time_factor;
-  double sim_time;
-  double display_time;
-  char *fg_host;
-  unsigned int fg_port;
-  unsigned int fg_port_in;
-  unsigned int fg_time_offset;
-  int fg_fdm;
-  char *js_dev;
-  char *spektrum_dev;
-  int rc_script;
-  char *ivy_bus;
-} nps_main;
-
-static bool nps_main_parse_options(int argc, char **argv);
-static void nps_main_init(void);
-static void nps_main_display(void);
-static void nps_main_run_sim_step(void);
-static gboolean nps_main_periodic(gpointer data __attribute__((unused)));
-
-int pauseSignal = 0;
+#include "nps_ivy.h"

 void tstp_hdl(int n __attribute__((unused)))
 {
@@ -77,6 +41,7 @@ void tstp_hdl(int n __attribute__((unused)))
  }
 }

+
 void cont_hdl(int n __attribute__((unused)))
 {
  signal(SIGCONT, cont_hdl);
@@ -84,13 +49,20 @@ void cont_hdl(int n __attribute__((unused)))
  printf("Press <enter> to continue.\n");
 }

+
 double time_to_double(struct timeval *t)
 {
  return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
 }

-int main(int argc, char **argv)
+double ntime_to_double(struct timespec *t)
 {
+  return ((double)t->tv_sec + (double)(t->tv_nsec * 1e-9));
+}
+
+int nps_main_init(int argc, char **argv)
+{
+  pauseSignal = 0;

  if (!nps_main_parse_options(argc, argv)) { return 1; }

@@ -101,22 +73,6 @@ int main(int argc, char **argv)
   */
  setbuf(stdout, NULL);

-  nps_main_init();
-
-  signal(SIGCONT, cont_hdl);
-  signal(SIGTSTP, tstp_hdl);
-  printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor);
-
-  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
-  g_timeout_add(HOST_TIMEOUT_MS, nps_main_periodic, NULL);
-  g_main_loop_run(ml);
-
-  return 0;
-}
-
-
-static void nps_main_init(void)
-{

  nps_main.sim_time = 0.;
  nps_main.display_time = 0.;
@@ -125,67 +81,22 @@ static void nps_main_init(void)
  nps_main.real_initial_time = time_to_double(&t);
  nps_main.scaled_initial_time = time_to_double(&t);

-  nps_ivy_init(nps_main.ivy_bus);
  nps_fdm_init(SIM_DT);
  nps_atmosphere_init();
  nps_sensors_init(nps_main.sim_time);
  printf("Simulating with dt of %f\n", SIM_DT);

-  enum NpsRadioControlType rc_type;
-  char *rc_dev = NULL;
-  if (nps_main.js_dev) {
-    rc_type = JOYSTICK;
-    rc_dev = nps_main.js_dev;
-  } else if (nps_main.spektrum_dev) {
-    rc_type = SPEKTRUM;
-    rc_dev = nps_main.spektrum_dev;
-  } else {
-    rc_type = SCRIPT;
-  }
-  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
-
-  if (nps_main.fg_host)
-    nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset);
-
+  nps_radio_and_autopilot_init();

 #if DEBUG_NPS_TIME
  printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
 #endif

-}
+  signal(SIGCONT, cont_hdl);
+  signal(SIGTSTP, tstp_hdl);
+  printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor);

-
-
-static void nps_main_run_sim_step(void)
-{
-  //  printf("sim at %f\n", nps_main.sim_time);
-
-  nps_atmosphere_update(SIM_DT);
-
-  nps_autopilot_run_systime_step();
-
-  nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB);
-
-  nps_sensors_run_step(nps_main.sim_time);
-
-  nps_autopilot_run_step(nps_main.sim_time);
-
-}
-
-
-static void nps_main_display(void)
-{
-  //  printf("display at %f\n", nps_main.display_time);
-  nps_ivy_display();
-
-  if (nps_main.fg_host) {
-    if (nps_main.fg_fdm) {
-      nps_flightgear_send_fdm();
-    } else {
-      nps_flightgear_send();
-    }
-  }
-  nps_flightgear_receive();
+  return 0;
 }


@@ -217,84 +128,7 @@ void nps_set_time_factor(float time_factor)
 }


-static gboolean nps_main_periodic(gpointer data __attribute__((unused)))
-{
-  struct timeval tv_now;
-  double  host_time_now;
-
-  if (pauseSignal) {
-    char line[128];
-    double tf = 1.0;
-    double t1, t2, irt;
-
-    gettimeofday(&tv_now, NULL);
-    t1 = time_to_double(&tv_now);
-    /* unscale to initial real time*/
-    irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor;
-
-    printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
-           nps_main.host_time_factor);
-    fflush(stdout);
-    if (fgets(line, 127, stdin)) {
-      if ((sscanf(line, " %le ", &tf) == 1)) {
-        if (tf > 0 && tf < 1000) {
-          nps_main.host_time_factor = tf;
-        }
-      }
-      printf("Time factor is %f\n", nps_main.host_time_factor);
-    }
-    gettimeofday(&tv_now, NULL);
-    t2 = time_to_double(&tv_now);
-    /* add the pause to initial real time */
-    irt += t2 - t1;
-    nps_main.real_initial_time += t2 - t1;
-    /* convert to scaled initial real time */
-    nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor;
-    pauseSignal = 0;
-  }
-
-  gettimeofday(&tv_now, NULL);
-  host_time_now = time_to_double(&tv_now);
-  double host_time_elapsed = nps_main.host_time_factor * (host_time_now  - nps_main.scaled_initial_time);
-
-#if DEBUG_NPS_TIME
-  printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
-         nps_main.sim_time, nps_main.display_time);
-#endif
-
-  int cnt = 0;
-  static int prev_cnt = 0;
-  static int grow_cnt = 0;
-  while (nps_main.sim_time <= host_time_elapsed) {
-    nps_main_run_sim_step();
-    nps_main.sim_time += SIM_DT;
-    if (nps_main.display_time < (host_time_now - nps_main.real_initial_time)) {
-      nps_main_display();
-      nps_main.display_time += DISPLAY_DT;
-    }
-    cnt++;
-  }
-
-  /* Check to make sure the simulation doesn't get too far behind real time looping */
-  if (cnt > (prev_cnt)) {grow_cnt++;}
-  else { grow_cnt--;}
-  if (grow_cnt < 0) {grow_cnt = 0;}
-  prev_cnt = cnt;
-
-  if (grow_cnt > 10) {
-    printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
-  }
-
-#if DEBUG_NPS_TIME
-  printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
-#endif
-
-  return TRUE;
-
-}
-
-
-static bool nps_main_parse_options(int argc, char **argv)
+bool nps_main_parse_options(int argc, char **argv)
 {

  nps_main.fg_host = NULL;
@@ -391,3 +225,93 @@ static bool nps_main_parse_options(int argc, char **argv)
  }
  return TRUE;
 }
+
+
+void *nps_flight_gear_loop(void *data __attribute__((unused)))
+{
+  struct timespec requestStart;
+  struct timespec requestEnd;
+  struct timespec waitFor;
+  long int period_ns = DISPLAY_DT * 1000000000L; // thread period in nanoseconds
+  long int task_ns = 0; // time it took to finish the task in nanoseconds
+
+
+  nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset);
+
+  while (TRUE) {
+    clock_gettime(CLOCK_REALTIME, &requestStart);
+
+    pthread_mutex_lock(&fdm_mutex);
+    if (nps_main.fg_host) {
+      if (nps_main.fg_fdm) {
+        nps_flightgear_send_fdm();
+      } else {
+        nps_flightgear_send();
+      }
+    }
+    pthread_mutex_unlock(&fdm_mutex);
+
+    clock_gettime(CLOCK_REALTIME, &requestEnd);
+
+    // Calculate time it took
+    task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
+
+    // task took less than one period, sleep for the rest of time
+    if (task_ns < period_ns) {
+      waitFor.tv_sec = 0;
+      waitFor.tv_nsec = period_ns - task_ns;
+      nanosleep(&waitFor, NULL);
+    } else {
+      // task took longer than the period
+      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);
+    }
+  }
+
+  return(NULL);
+}
+
+
+
+void *nps_main_display(void *data __attribute__((unused)))
+{
+  struct timespec requestStart;
+  struct timespec requestEnd;
+  struct timespec waitFor;
+  long int period_ns = 3 * DISPLAY_DT * 1000000000L; // thread period in nanoseconds
+  long int task_ns = 0; // time it took to finish the task in nanoseconds
+
+  struct NpsFdm fdm_ivy;
+  struct NpsSensors sensors_ivy;
+
+  nps_ivy_init(nps_main.ivy_bus);
+
+  while (TRUE) {
+    clock_gettime(CLOCK_REALTIME, &requestStart);
+
+    pthread_mutex_lock(&fdm_mutex);
+    memcpy(&fdm_ivy, &fdm, sizeof(fdm));
+    memcpy(&sensors_ivy, &sensors, sizeof(sensors));
+    pthread_mutex_unlock(&fdm_mutex);
+
+    nps_ivy_display(&fdm_ivy, &sensors_ivy);
+
+    clock_gettime(CLOCK_REALTIME, &requestEnd);
+
+    // Calculate time it took
+    task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
+
+    // task took less than one period, sleep for the rest of time
+    if (task_ns < period_ns) {
+      waitFor.tv_sec = 0;
+      waitFor.tv_nsec = period_ns - task_ns;
+      nanosleep(&waitFor, NULL);
+    } else {
+      // task took longer than the period
+      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);
+    }
+
+  }
+  return(NULL);
+}
@@ -0,0 +1,334 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+#include "termios.h"
+#include <fcntl.h>
+#include <unistd.h>
+
+
+#include "paparazzi.h"
+#include "pprzlink/messages.h"
+#include "pprzlink/dl_protocol.h"
+#include "pprzlink/pprz_transport.h"
+#include "generated/airframe.h"
+
+/* Message id helpers */
+#define SenderIdOfPprzMsg(x) (x[0])
+#define IdOfPprzMsg(x) (x[1])
+
+#include "nps_main.h"
+#include "nps_sensors.h"
+#include "nps_atmosphere.h"
+#include "nps_autopilot.h"
+#include "nps_ivy.h"
+#include "nps_flightgear.h"
+#include "mcu_periph/sys_time.h"
+
+#include "nps_ins.h"
+
+void *nps_ins_data_loop(void *data __attribute__((unused)));
+void *nps_ap_data_loop(void *data __attribute__((unused)));
+
+pthread_t th_ins_data; // sends INS packets to the autopilot
+pthread_t th_ap_data; // receives commands from the autopilot
+
+#define NPS_MAX_MSG_SIZE 512
+
+int main(int argc, char **argv)
+{
+  nps_main_init(argc, argv);
+
+  if (nps_main.fg_host) {
+    pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL);
+  }
+  pthread_create(&th_display_ivy, NULL, nps_main_display, NULL);
+  pthread_create(&th_main_loop, NULL, nps_main_loop, NULL);
+  pthread_create(&th_ins_data, NULL, nps_ins_data_loop, NULL);
+  pthread_create(&th_ap_data, NULL, nps_ap_data_loop, NULL);
+  pthread_join(th_main_loop, NULL);
+
+  return 0;
+}
+
+void nps_radio_and_autopilot_init(void)
+{
+  enum NpsRadioControlType rc_type;
+  char *rc_dev = NULL;
+  if (nps_main.js_dev) {
+    rc_type = JOYSTICK;
+    rc_dev = nps_main.js_dev;
+  } else if (nps_main.spektrum_dev) {
+    rc_type = SPEKTRUM;
+    rc_dev = nps_main.spektrum_dev;
+  } else {
+    rc_type = SCRIPT;
+  }
+  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
+}
+
+void nps_update_launch_from_dl(uint8_t value)
+{
+  autopilot.launch = value;
+}
+
+void nps_main_run_sim_step(void)
+{
+  nps_atmosphere_update(SIM_DT);
+  nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB);
+}
+
+void *nps_ins_data_loop(void *data __attribute__((unused)))
+{
+  struct timespec requestStart;
+  struct timespec requestEnd;
+  struct timespec waitFor;
+  long int period_ns = (1. / INS_FREQUENCY) * 1000000000L; // thread period in nanoseconds
+  long int task_ns = 0; // time it took to finish the task in nanoseconds
+
+  nps_ins_init(); // initialize ins variables and pointers
+
+  // configure port
+  int fd = open(INS_DEV, O_WRONLY | O_NOCTTY | O_SYNC);//open(INS_DEV, O_RDWR | O_NOCTTY);
+  if (fd < 0) {
+    printf("INS THREAD: data loop error opening port %i\n", fd);
+    return(NULL);
+  }
+
+  struct termios new_settings;
+  tcgetattr(fd, &new_settings);
+  memset(&new_settings, 0, sizeof(new_settings));
+  new_settings.c_iflag = 0;
+  new_settings.c_cflag = 0;
+  new_settings.c_lflag = 0;
+  new_settings.c_cc[VMIN] = 0;
+  new_settings.c_cc[VTIME] = 0;
+  cfsetispeed(&new_settings, (speed_t)INS_BAUD);
+  cfsetospeed(&new_settings, (speed_t)INS_BAUD);
+  tcsetattr(fd, TCSANOW, &new_settings);
+
+  struct NpsFdm fdm_ins;
+
+  while (TRUE) {
+    // lock mutex
+    pthread_mutex_lock(&fdm_mutex);
+
+    // start timing
+    clock_gettime(CLOCK_REALTIME, &requestStart);
+
+    // make a copy of fdm struct to speed things up
+    memcpy(&fdm_ins, &fdm, sizeof(fdm));
+
+    // unlock mutex
+    pthread_mutex_unlock(&fdm_mutex);
+
+    // fetch data
+    nps_ins_fetch_data(&fdm_ins);
+
+    // send ins data here
+    static uint16_t idx;
+    idx = nps_ins_fill_buffer();
+
+    static int wlen;
+    wlen = write(fd, ins_buffer, idx);
+    if (wlen != idx) {
+      printf("INS THREAD: Warning - sent only %u bytes to the autopilot, instead of expected %u\n", wlen, idx);
+    }
+
+    clock_gettime(CLOCK_REALTIME, &requestEnd);
+
+    // Calculate time it took
+    task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
+
+    // task took less than one period, sleep for the rest of time
+    if (task_ns < period_ns) {
+      waitFor.tv_sec = 0;
+      waitFor.tv_nsec = period_ns - task_ns;
+      nanosleep(&waitFor, NULL);
+    } else {
+      // task took longer than the period
+      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);
+    }
+  }
+  return(NULL);
+}
+
+
+
+void *nps_ap_data_loop(void *data __attribute__((unused)))
+{
+  // configure port
+  int fd = open(AP_DEV, O_RDONLY | O_NOCTTY);
+  if (fd < 0) {
+    printf("AP data loop error opening port %i\n", fd);
+    return(NULL);
+  }
+
+  struct termios new_settings;
+  tcgetattr(fd, &new_settings);
+  memset(&new_settings, 0, sizeof(new_settings));
+  new_settings.c_iflag = 0;
+  new_settings.c_cflag = 0;
+  new_settings.c_lflag = 0;
+  new_settings.c_cc[VMIN] = 1;
+  new_settings.c_cc[VTIME] = 5;
+  cfsetispeed(&new_settings, (speed_t)AP_BAUD);
+  cfsetospeed(&new_settings, (speed_t)AP_BAUD);
+  tcsetattr(fd, TCSANOW, &new_settings);
+
+  int rdlen;
+  uint8_t buf[NPS_MAX_MSG_SIZE];
+  uint8_t cmd_len;
+  pprz_t  cmd_buf[NPS_COMMANDS_NB];
+
+  struct pprz_transport pprz_tp_logger;
+
+  while (TRUE) {
+    // receive messages from the autopilot
+    rdlen = read(fd, buf, sizeof(buf) - 1);
+
+    for (int i = 0; i < rdlen; i++) {
+      // parse data
+      parse_pprz(&pprz_tp_logger, buf[i]);
+
+      // if msg_available then read
+      if (pprz_tp_logger.trans_rx.msg_received) {
+        for (int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) {
+          buf[k] = pprz_tp_logger.trans_rx.payload[k];
+        }
+        //Parse message
+        uint8_t sender_id = SenderIdOfPprzMsg(buf);
+        uint8_t msg_id = IdOfPprzMsg(buf);
+
+        // parse telemetry messages coming from other AC
+        if (sender_id != AC_ID) {
+          switch (msg_id) {
+            default: {
+              break;
+            }
+          }
+        } else {
+          /* parse telemetry messages coming from the correct AC_ID */
+          switch (msg_id) {
+            case DL_COMMANDS:
+              // parse commands message
+              cmd_len = DL_COMMANDS_values_length(buf);
+              memcpy(&cmd_buf, DL_COMMANDS_values(buf), cmd_len * sizeof(int16_t));
+              pthread_mutex_lock(&fdm_mutex);
+              // update commands
+              for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
+                autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ;
+              }
+              // hack: invert pitch to fit most JSBSim models
+              autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ;
+              pthread_mutex_unlock(&fdm_mutex);
+              break;
+            case DL_MOTOR_MIXING:
+              // parse actuarors message
+              cmd_len = DL_MOTOR_MIXING_values_length(buf);
+              memcpy(&cmd_buf, DL_MOTOR_MIXING_values(buf), cmd_len * sizeof(int16_t));
+              pthread_mutex_lock(&fdm_mutex);
+              // update commands
+              for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
+                autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ;
+              }
+              pthread_mutex_unlock(&fdm_mutex);
+              break;
+            default:
+              break;
+          }
+        }
+        pprz_tp_logger.trans_rx.msg_received = false;
+      }
+    }
+  }
+  return(NULL);
+}
+
+
+
+
+void *nps_main_loop(void *data __attribute__((unused)))
+{
+  struct timespec requestStart;
+  struct timespec requestEnd;
+  struct timespec waitFor;
+  long int period_ns = SIM_DT * 1000000000L; // thread period in nanoseconds
+  long int task_ns = 0; // time it took to finish the task in nanoseconds
+
+  // check the sim time difference from the realtime
+  // fdm.time - simulation time
+  struct timespec startTime;
+  struct timespec realTime;
+  clock_gettime(CLOCK_REALTIME, &startTime);
+  double start_secs = ntime_to_double(&startTime);
+  double real_secs = 0;
+  double real_time = 0;
+  static int guard;
+
+  while (TRUE) {
+    clock_gettime(CLOCK_REALTIME, &requestStart);
+
+    pthread_mutex_lock(&fdm_mutex);
+
+    // check the current simulation time
+    clock_gettime(CLOCK_REALTIME, &realTime);
+    real_secs = ntime_to_double(&realTime);
+    real_time = real_secs - start_secs; // real time elapsed
+
+    guard = 0;
+    while ((real_time - fdm.time) > SIM_DT) {
+      nps_main_run_sim_step();
+      guard++;
+      if (guard > 2) {
+        //If we are too much behind, catch up incrementaly
+        break;
+      }
+    }
+    pthread_mutex_unlock(&fdm_mutex);
+
+    clock_gettime(CLOCK_REALTIME, &requestEnd);
+
+    // Calculate time it took
+    task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
+
+    // task took less than one period, sleep for the rest of time
+    if (task_ns < period_ns) {
+      waitFor.tv_sec = 0;
+      waitFor.tv_nsec = period_ns - task_ns;
+      nanosleep(&waitFor, NULL);
+    } else {
+      // task took longer than the period
+      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);
+    }
+  }
+  return(NULL);
+}
+
+
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012 The Paparazzi Team
+ * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/time.h>
+
+#include "nps_main.h"
+#include "nps_fdm.h"
+
+
+
+
+int main(int argc, char **argv)
+{
+  if (nps_main_init(argc, argv)) {
+    return 1;
+  }
+
+  if (nps_main.fg_host) {
+    pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL);
+  }
+  pthread_create(&th_display_ivy, NULL, nps_main_display, NULL);
+  pthread_create(&th_main_loop, NULL, nps_main_loop, NULL);
+  pthread_join(th_main_loop, NULL);
+
+  return 0;
+}
+
+
+void nps_update_launch_from_dl(uint8_t value __attribute__((unused))) {}
+
+
+void nps_radio_and_autopilot_init(void)
+{
+  enum NpsRadioControlType rc_type;
+  char *rc_dev = NULL;
+  if (nps_main.js_dev) {
+    rc_type = JOYSTICK;
+    rc_dev = nps_main.js_dev;
+  } else if (nps_main.spektrum_dev) {
+    rc_type = SPEKTRUM;
+    rc_dev = nps_main.spektrum_dev;
+  } else {
+    rc_type = SCRIPT;
+  }
+  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
+}
+
+
+void nps_main_run_sim_step(void)
+{
+  nps_atmosphere_update(SIM_DT);
+
+  nps_autopilot_run_systime_step();
+
+  nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB);
+
+  nps_sensors_run_step(nps_main.sim_time);
+
+  nps_autopilot_run_step(nps_main.sim_time);
+
+}
+
+
+void *nps_main_loop(void *data __attribute__((unused)))
+{
+  struct timespec requestStart;
+  struct timespec requestEnd;
+  struct timespec waitFor;
+  long int period_ns = HOST_TIMEOUT_MS * 1000000LL; // thread period in nanoseconds
+  long int task_ns = 0; // time it took to finish the task in nanoseconds
+
+  struct timeval tv_now;
+  double  host_time_now;
+
+  while (TRUE) {
+    if (pauseSignal) {
+      char line[128];
+      double tf = 1.0;
+      double t1, t2, irt;
+
+      gettimeofday(&tv_now, NULL);
+      t1 = time_to_double(&tv_now);
+      // unscale to initial real time
+      irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor;
+
+      printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
+             nps_main.host_time_factor);
+      fflush(stdout);
+      if (fgets(line, 127, stdin)) {
+        if ((sscanf(line, " %le ", &tf) == 1)) {
+          if (tf > 0 && tf < 1000) {
+            nps_main.host_time_factor = tf;
+          }
+        }
+        printf("Time factor is %f\n", nps_main.host_time_factor);
+      }
+      gettimeofday(&tv_now, NULL);
+      t2 = time_to_double(&tv_now);
+      // add the pause to initial real time
+      irt += t2 - t1;
+      nps_main.real_initial_time += t2 - t1;
+      // convert to scaled initial real time
+      nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor;
+      pauseSignal = 0;
+    }
+
+    clock_gettime(CLOCK_REALTIME, &requestStart); // init measurement (after the pause signal)
+
+    gettimeofday(&tv_now, NULL);
+    host_time_now = time_to_double(&tv_now);
+    double host_time_elapsed = nps_main.host_time_factor * (host_time_now  - nps_main.scaled_initial_time);
+
+#if DEBUG_NPS_TIME
+    printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
+           nps_main.sim_time, nps_main.display_time);
+#endif
+
+    int cnt = 0;
+    static int prev_cnt = 0;
+    static int grow_cnt = 0;
+    while (nps_main.sim_time <= host_time_elapsed) {
+      pthread_mutex_lock(&fdm_mutex);
+      nps_main_run_sim_step();
+      nps_main.sim_time += SIM_DT;
+      pthread_mutex_unlock(&fdm_mutex);
+      cnt++;
+    }
+
+    // Check to make sure the simulation doesn't get too far behind real time looping
+    if (cnt > (prev_cnt)) {grow_cnt++;}
+    else { grow_cnt--;}
+    if (grow_cnt < 0) {grow_cnt = 0;}
+    prev_cnt = cnt;
+
+    if (grow_cnt > 10) {
+      printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
+    }
+
+#if DEBUG_NPS_TIME
+    printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
+#endif
+
+    clock_gettime(CLOCK_REALTIME, &requestEnd); // end measurement
+
+    // Calculate time it took
+    task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
+
+    if (task_ns > 0) {
+      waitFor.tv_sec = 0;
+      waitFor.tv_nsec = period_ns - task_ns;
+      nanosleep(&waitFor, NULL);
+    } else {
+      // task took longer than the period
+      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);
+    }
+  }
+  return(NULL);
+}
@@ -48,6 +48,8 @@ void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char
      break;
    case SCRIPT:
      break;
+    default:
+      break;
  }

 }
@@ -16,6 +16,8 @@
 #define IUCLC 0
 #endif

+#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512)
+
 static int sp_fd;

 static gboolean on_serial_data_received(GIOChannel *source,
@@ -132,19 +134,12 @@ static void parse_data(char *buf, int len)
  }
 }

-
-#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512)
 static void handle_frame(void)
 {
-
-
  nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0) / -340.;
  nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1) + 340) / 680.;
  nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2) / -340.;
  nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3) / -340.;
  nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5) / 340.;
-
-
  //  printf("%f %f %f %f %f \n", nps_radio_control.roll, nps_radio_control.throttle, nps_radio_control.pitch, nps_radio_control.yaw, nps_radio_control.mode);
-
 }