diff --git a/conf/flight_plans/rotorcraft_basic_geofence.xml b/conf/flight_plans/rotorcraft_basic_geofence.xml
index 92c8940437..230152a457 100644
--- a/conf/flight_plans/rotorcraft_basic_geofence.xml
+++ b/conf/flight_plans/rotorcraft_basic_geofence.xml
@@ -47,7 +47,7 @@
-
+
diff --git a/conf/modules/ins_vectornav.xml b/conf/modules/ins_vectornav.xml
index aaee09f62f..fe932612ae 100644
--- a/conf/modules/ins_vectornav.xml
+++ b/conf/modules/ins_vectornav.xml
@@ -24,6 +24,8 @@
+
+
diff --git a/conf/simulator/nps/nps_sensors_params_vectornav.h b/conf/simulator/nps/nps_sensors_params_vectornav.h
new file mode 100644
index 0000000000..eda949f471
--- /dev/null
+++ b/conf/simulator/nps/nps_sensors_params_vectornav.h
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2012 Felix Ruess
+ *
+ * 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_SENSORS_PARAMS_H
+#define NPS_SENSORS_PARAMS_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+
+#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
+#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
+#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
+
+/*
+ * Accelerometer
+ */
+/* ADXL345 configured to +-16g with 13bit resolution */
+#define NPS_ACCEL_MIN -4095
+#define NPS_ACCEL_MAX 4095
+/* ms-2 */
+/* aka 2^10/ACCEL_X_SENS */
+#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS))
+#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS))
+#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS))
+
+#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
+/* m2s-4 */
+#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2
+/* ms-2 */
+#define NPS_ACCEL_BIAS_X 0
+#define NPS_ACCEL_BIAS_Y 0
+#define NPS_ACCEL_BIAS_Z 0
+/* s */
+#define NPS_ACCEL_DT (1./512.)
+
+
+
+/*
+ * Gyrometer
+ */
+/* IMU-3000 has 16 bit resolution */
+#define NPS_GYRO_MIN -32767
+#define NPS_GYRO_MAX 32767
+
+/* 2^12/GYRO_X_SENS */
+#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS))
+#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS))
+#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS))
+
+#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
+#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
+#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
+
+#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
+
+#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
+
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
+/* s */
+#define NPS_GYRO_DT (1./512.)
+
+
+
+/*
+ * Magnetometer
+ */
+ /* HMC5843 has 12 bit resolution */
+#define NPS_MAG_MIN -2047
+#define NPS_MAG_MAX 2047
+
+#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
+#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
+#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
+
+#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS))
+#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS))
+#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS))
+
+#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
+#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
+#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
+
+#define NPS_MAG_NOISE_STD_DEV_X 2e-3
+#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
+#define NPS_MAG_NOISE_STD_DEV_Z 2e-3
+
+#define NPS_MAG_DT (1./100.)
+
+
+/*
+ * Barometer (pressure and std dev in Pascal)
+ */
+#define NPS_BARO_DT (1./50.)
+#define NPS_BARO_NOISE_STD_DEV 2
+
+/*
+ * GPS
+ */
+
+#ifndef GPS_PERFECT
+#define GPS_PERFECT 1
+#endif
+
+#if GPS_PERFECT
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV 0.
+#define NPS_GPS_SPEED_LATENCY 0.
+#define NPS_GPS_POS_NOISE_STD_DEV 0.001
+#define NPS_GPS_POS_BIAS_INITIAL_X 0.
+#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
+#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
+#define NPS_GPS_POS_LATENCY 0.
+
+#else
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5
+#define NPS_GPS_SPEED_LATENCY 0.2
+#define NPS_GPS_POS_NOISE_STD_DEV 2
+#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
+#define NPS_GPS_POS_LATENCY 0.2
+
+#endif /* GPS_PERFECT */
+
+#define NPS_GPS_DT (1./512.)
+
+#endif /* NPS_SENSORS_PARAMS_H */
diff --git a/sw/airborne/subsystems/ins/ins_vectornav.c b/sw/airborne/subsystems/ins/ins_vectornav.c
index 2483e1bfc6..91287129af 100644
--- a/sw/airborne/subsystems/ins/ins_vectornav.c
+++ b/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;
diff --git a/sw/simulator/nps/nps_ins_vectornav.c b/sw/simulator/nps/nps_ins_vectornav.c
index 1f83969e00..fa15786a5a 100644
--- a/sw/simulator/nps/nps_ins_vectornav.c
+++ b/sw/simulator/nps/nps_ins_vectornav.c
@@ -26,6 +26,8 @@
#include "nps_fdm.h"
#include
#include
+#include "nps_sensors.h"
+#include /* 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;
diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c
index a5041408db..a50f79dc96 100644
--- a/sw/simulator/nps/nps_main_hitl.c
+++ b/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
diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c
index cd10957c65..8a2ed0e6f8 100644
--- a/sw/simulator/nps/nps_sensor_gps.c
+++ b/sw/simulator/nps/nps_sensor_gps.c
@@ -1,6 +1,6 @@
#include "nps_sensor_gps.h"
-
+#include
#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
*/