[nps] sim_overwrite_ins updates

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -89,7 +89,7 @@
 #include "led.h"
 
 #ifdef USE_NPS
-#include "nps_autopilot_fixedwing.h"
+#include "nps_autopilot.h"
 #endif
 
 /* Default trim commands for roll, pitch and yaw */

sw/airborne/firmwares/rotorcraft/main.c

@@ -73,7 +73,7 @@
 #include "firmwares/rotorcraft/main.h"
 
 #ifdef SITL
-#include "nps_autopilot_rotorcraft.h"
+#include "nps_autopilot.h"
 #endif
 
 #include "generated/modules.h"
@@ -309,9 +309,6 @@ static inline void on_gyro_event( void ) {
     if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
     ins_propagate();
-#ifdef SITL
-    if (nps_bypass_ins) sim_overwrite_ins();
-#endif
   }
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_imu_available();

sw/airborne/subsystems/ins/ins_int.c

@@ -45,8 +45,9 @@
 #include "subsystems/ins/hf_float.h"
 #endif
 
-#ifdef SITL
-#include "nps_fdm.h"
+#if defined SITL && USE_NPS
+//#include "nps_fdm.h"
+#include "nps_autopilot.h"
 #include <stdio.h>
 #endif
 
@@ -357,6 +358,11 @@ static void ins_ned_to_state(void) {
   stateSetPositionNed_i(&ins_impl.ltp_pos);
   stateSetSpeedNed_i(&ins_impl.ltp_speed);
   stateSetAccelNed_i(&ins_impl.ltp_accel);
+
+#if defined SITL && USE_NPS
+  if (nps_bypass_ins)
+    sim_overwrite_ins();
+#endif
 }
 
 

sw/simulator/nps/nps_autopilot.h

@@ -11,10 +11,15 @@ struct NpsAutopilot {
 
 extern struct NpsAutopilot autopilot;
 
+extern bool_t nps_bypass_ahrs;
+extern bool_t nps_bypass_ins;
+extern void sim_overwrite_ahrs(void);
+extern void sim_overwrite_ins(void);
+
 extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev);
 extern void nps_autopilot_run_step(double time);
 extern void nps_autopilot_run_systime_step(void);
 
+
 #endif /* NPS_AUTOPILOT_H */
 
-

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -20,7 +20,7 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "nps_autopilot_fixedwing.h"
+#include "nps_autopilot.h"
 
 #ifdef FBW
 #include "firmwares/fixedwing/main_fbw.h"
@@ -50,14 +50,19 @@
 #include "subsystems/commands.h"
 
 
-
 struct NpsAutopilot autopilot;
 bool_t nps_bypass_ahrs;
+bool_t nps_bypass_ins;
 
 #ifndef NPS_BYPASS_AHRS
 #define NPS_BYPASS_AHRS FALSE
 #endif
 
+#ifndef NPS_BYPASS_INS
+#define NPS_BYPASS_INS FALSE
+#endif
+
+
 #if !defined (FBW) || !defined (AP)
 #error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
 #endif
@@ -66,6 +71,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
 
   nps_radio_control_init(type_rc, num_rc_script, rc_dev);
   nps_bypass_ahrs = NPS_BYPASS_AHRS;
+  nps_bypass_ins = NPS_BYPASS_INS;
 
   Fbw(init);
   Ap(init);
@@ -108,8 +114,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
  }
 
   if (nps_sensors_baro_available()) {
-    /** @todo feed baro values */
-    //baro_feed_value(sensors.baro.value);
+    baro_feed_value(sensors.baro.value);
     Fbw(event_task);
     Ap(event_task);
   }
@@ -124,6 +129,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
     sim_overwrite_ahrs();
   }
 
+  if (nps_bypass_ins) {
+    sim_overwrite_ins();
+  }
+
   Fbw(handle_periodic_tasks);
   Ap(handle_periodic_tasks);
 
@@ -144,3 +153,19 @@ void sim_overwrite_ahrs(void) {
   stateSetBodyRates_f(&rates_f);
 
 }
+
+void sim_overwrite_ins(void) {
+
+  struct NedCoor_f ltp_pos;
+  VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
+  stateSetPositionNed_f(&ltp_pos);
+
+  struct NedCoor_f ltp_speed;
+  VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
+  stateSetSpeedNed_f(&ltp_speed);
+
+  struct NedCoor_f ltp_accel;
+  VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
+  stateSetAccelNed_f(&ltp_accel);
+
+}

sw/simulator/nps/nps_autopilot_fixedwing.h

@@ -1,10 +0,0 @@
-#ifndef NPS_AUTOPILOT_FIXEDWING_H
-#define NPS_AUTOPILOT_FIXEDWING_H
-
-#include "nps_autopilot.h"
-
-
-extern bool_t nps_bypass_ahrs;
-extern void sim_overwrite_ahrs(void);
-
-#endif /* NPS_AUTOPILOT_FIXEDWING_H */

sw/simulator/nps/nps_autopilot_rotorcraft.c

@@ -19,7 +19,7 @@
  * Boston, MA 02111-1307, USA.
  */
 
-#include "nps_autopilot_rotorcraft.h"
+#include "nps_autopilot.h"
 
 #include "firmwares/rotorcraft/main.h"
 #include "nps_sensors.h"

sw/simulator/nps/nps_autopilot_rotorcraft.h

@@ -1,12 +0,0 @@
-#ifndef NPS_AUTOPILOT_ROTORCRAFT_H
-#define NPS_AUTOPILOT_ROTORCRAFT_H
-
-#include "nps_autopilot.h"
-
-
-extern bool_t nps_bypass_ahrs;
-extern bool_t nps_bypass_ins;
-extern void sim_overwrite_ahrs(void);
-extern void sim_overwrite_ins(void);
-
-#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */