Minor updates in Vectornav driver and HITL

sw/airborne/subsystems/ins/ins_vectornav.c

@@ -122,6 +122,16 @@ void ins_vectornav_monitor(void)
     gps.fix = GPS_FIX_NONE;
   }
 
+  // Make gps pacc available in GPS page on GCS
+  static uint32_t last_pacc = 0;
+  // update only if pacc changes
+  if (last_pacc != gps.pacc) {
+    last_pacc = gps.pacc;
+    // we don't know the value of CNO, hence oscillate
+    // between 0 and 1 to not confuse the user
+    gps.svinfos[0].cno = (gps.svinfos[0].cno + 1) % 2;
+  }
+
   // update counter
   last_cnt = ins_vn.vn_packet.counter;
 
@@ -282,15 +292,10 @@ void ins_vectornav_propagate()
   SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
   stateSetPositionLla_i(&gps.lla_pos);
 
-  // ECEF position
-  struct LtpDef_f def;
-  ltp_def_from_lla_f(&def, &ins_vn.lla_pos);
-  struct EcefCoor_f ecef_vel;
-  ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vn_data.vel_ned);
-  ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel);
-  SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
-
   // ECEF velocity
+  // TODO: properly implement
+
+  // ECEF position
   gps.ecef_pos.x = stateGetPositionEcef_i()->x;
   gps.ecef_pos.y = stateGetPositionEcef_i()->y;
   gps.ecef_pos.z = stateGetPositionEcef_i()->z;

sw/simulator/nps/nps_ins_vectornav.c

@@ -26,6 +26,8 @@
 #include "nps_fdm.h"
 #include <time.h>
 #include <stdio.h>
+#include "nps_sensors.h"
+#include <stdlib.h>     /* srand, rand */
 
 /*
  * Vectornav info
@@ -113,6 +115,11 @@ static uint64_t vn_get_time_of_week(void)
   return tow;
 }
 
+/**
+ * Fetch data from FDM and store them into vectornav packet
+ * NOTE: some noise is being added, see Vectornav specifications
+ * for details about the precision: http://www.vectornav.com/products/vn-200/specifications
+ */
 void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
 {
   struct NpsFdm fdm_data;
@@ -133,9 +140,9 @@ void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
 
   //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
+  vn_data.Position[0] = DegOfRad(sensors.gps.lla_pos.lat);
+  vn_data.Position[1] = DegOfRad(sensors.gps.lla_pos.lon);
+  vn_data.Position[2] = sensors.gps.lla_pos.alt;
 
   //VelNed, float [m/s]
   //The estimated velocity in the North East Down (NED) frame, given in m/s.
@@ -155,13 +162,18 @@ void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
   vn_data.NumSats = 8; // random number
 
   //gps fix, uint8
+  // TODO: add warm-up time
   vn_data.Fix = 3; // 3D fix
 
   //posU, float[3]
-  // TODO
+  // TODO: use proper sensor simulation
+  vn_data.PosU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
+  vn_data.PosU[1] = 2.5+(((float)rand())/RAND_MAX)*0.1;
+  vn_data.PosU[2] = 2.5+(((float)rand())/RAND_MAX)*0.1;
 
   //velU, float
-  // TODO
+  // TODO: use proper sensor simulation
+  vn_data.VelU = 5.0+(((float)rand())/RAND_MAX)*0.1;
 
   //linear acceleration imu-body frame, float [m/s^2]
   vn_data.LinearAccelBody[0] = (float)fdm_data.ltp_ecef_vel.x;
@@ -169,7 +181,10 @@ void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
   vn_data.LinearAccelBody[2] = (float)fdm_data.ltp_ecef_vel.z;
 
   //YprU, float[3]
-  // TODO
+  // TODO: use proper sensor simulation
+  vn_data.YprU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1;
+  vn_data.YprU[1] = 0.5+(((float)rand())/RAND_MAX)*0.1;
+  vn_data.YprU[2] = 0.5+(((float)rand())/RAND_MAX)*0.1;
 
   //instatus, uint16
   vn_data.InsStatus = 0x02;

sw/simulator/nps/nps_main_hitl.c

@@ -99,7 +99,10 @@ void nps_update_launch_from_dl(uint8_t value)
 void nps_main_run_sim_step(void)
 {
   nps_atmosphere_update(SIM_DT);
+
   nps_fdm_run_step(nps_autopilot.launch, nps_autopilot.commands, NPS_COMMANDS_NB);
+
+  nps_sensors_run_step(nps_main.sim_time);
 }
 
 void *nps_ins_data_loop(void *data __attribute__((unused)))
@@ -312,6 +315,7 @@ void *nps_main_loop(void *data __attribute__((unused)))
     guard = 0;
     while ((real_time - fdm.time) > SIM_DT) {
       nps_main_run_sim_step();
+      nps_main.sim_time = fdm.time;
       guard++;
       if (guard > 2) {
         //If we are too much behind, catch up incrementaly

sw/simulator/nps/nps_sensor_gps.c

@@ -1,6 +1,6 @@
 #include "nps_sensor_gps.h"
 
-
+#include <stdio.h>
 #include "generated/airframe.h"
 #include "nps_fdm.h"
 #include "nps_random.h"
@@ -38,12 +38,10 @@ void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
 
 void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time)
 {
-
   if (time < gps->next_update) {
     return;
   }
 
-
   /*
    * simulate speed sensor
    */