Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Merge pull request #1150 from paparazzi/nps_datalink

Browse Source 
[NPS] add possibility to simulate loss of datalink/downlink

close #934
This commit is contained in:
Gautier Hattenberger
2015-03-27 13:19:38 +01:00
parent 7eea0aadd2 bdd7ceb43e
commit 01534f24ab
23 changed files with 206 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/nps.xml
+2
View File
@@ -7,6 +7,8 @@
<dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="bypass_ahrs" values="No|Yes"/>
<dl_setting var="nps_bypass_ins" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="bypass_ins" values="No|Yes"/>
<dl_setting var="gps_has_fix" min="0" step="1" max="1" module="subsystems/gps/gps_sim_nps" shortname="gps_fix" values="No|Yes"/>
<dl_setting var="autopilot.datalink_enabled" min="0" step="1" max="1" module="nps/nps_autopilot" shortname="datalink" values="OFF|ON"/>
<dl_setting var="ivy_tp.ivy_dl_enabled" min="0" step="1" max="1" module="subsystems/datalink/ivy_transport" shortname="downlink" values="OFF|ON"/>
<dl_setting var="nps_electrical.supply_voltage" min="0" step="0.1" max="24" module="nps/nps_electrical" shortname="bat_voltage" unit="V"/>
<dl_setting var="nps_atmosphere.wind_speed" min="0" step="0.1" max="25" module="nps/nps_atmosphere" shortname="wind_speed" unit="m/s"/>
<dl_setting var="nps_atmosphere.wind_dir" min="0" step="1" max="360" module="nps/nps_atmosphere" shortname="wind_dir" unit="rad" alt_unit="deg"/>
sw/simulator/nps/nps_autopilot.h
+1
View File
@@ -27,6 +27,7 @@
struct NpsAutopilot {
double commands[NPS_COMMANDS_NB];
bool_t launch;
bool_t datalink_enabled;
};
extern struct NpsAutopilot autopilot;
sw/simulator/nps/nps_autopilot_fixedwing.c
+8
View File
@@ -52,6 +52,9 @@
// for launch
#include "firmwares/fixedwing/autopilot.h"
// for datalink_time hack
#include "subsystems/datalink/datalink.h"
struct NpsAutopilot autopilot;
bool_t nps_bypass_ahrs;
bool_t nps_bypass_ins;
@@ -72,6 +75,7 @@ bool_t nps_bypass_ins;
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
autopilot.launch = FALSE;
autopilot.datalink_enabled = TRUE;
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_electrical_init();
@@ -169,6 +173,10 @@ PRINT_CONFIG_VAR(COMMAND_YAW)
if (!launch)
autopilot.commands[COMMAND_THROTTLE] = 0;
// hack to reset datalink_time, since we don't use actual dl_parse_msg
if (autopilot.datalink_enabled) {
datalink_time = 0;
}
}
void sim_overwrite_ahrs(void) {
sw/simulator/nps/nps_autopilot_rotorcraft.c
+8
View File
@@ -42,6 +42,9 @@
#include "messages.h"
#include "subsystems/datalink/downlink.h"
// for datalink_time hack
#include "subsystems/datalink/datalink.h"
struct NpsAutopilot autopilot;
bool_t nps_bypass_ahrs;
bool_t nps_bypass_ins;
@@ -60,6 +63,7 @@ bool_t nps_bypass_ins;
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
autopilot.launch = TRUE;
autopilot.datalink_enabled = TRUE;
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_electrical_init();
@@ -134,6 +138,10 @@ void nps_autopilot_run_step(double time) {
for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
// hack to reset datalink_time, since we don't use actual dl_parse_msg
if (autopilot.datalink_enabled) {
datalink_time = 0;
}
}
sw/simulator/nps/nps_ivy_common.c
+19
View File
@@ -87,6 +87,13 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
if (atoi(argv[1]) != AC_ID)
return;
/* HACK:
* we actually don't want to allow changing settings if datalink is disabled,
* but since we currently change this variable via settings we have to allow it
*/
//if (!autopilot.datalink_enabled)
// return;
uint8_t index = atoi(argv[2]);
float value = atof(argv[3]);
DlSetting(index, value);
@@ -99,6 +106,8 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (atoi(argv[1]) != AC_ID)
return;
if (!autopilot.datalink_enabled)
return;
uint8_t index = atoi(argv[2]);
float value = settings_get_value(index);
@@ -109,6 +118,9 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[] __attribute__ ((unused))) {
if (!autopilot.datalink_enabled)
return;
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
@@ -117,6 +129,8 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (atoi(argv[2]) != AC_ID)
return;
if (!autopilot.datalink_enabled)
return;
int block = atoi(argv[1]);
nav_goto_block(block);
@@ -127,6 +141,9 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (!autopilot.datalink_enabled)
return;
uint8_t throttle_mode = atoi(argv[2]);
int8_t roll = atoi(argv[3]);
int8_t pitch = atoi(argv[4]);
@@ -139,6 +156,8 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]){
if (atoi(argv[1]) != AC_ID)
return;
if (!autopilot.datalink_enabled)
return;
uint8_t mode = atoi(argv[2]);
uint8_t throttle = atoi(argv[3]);
sw/simulator/nps/nps_ivy_fixedwing.c
+3
View File
@@ -8,6 +8,7 @@
#include "math/pprz_algebra_double.h"
#include "subsystems/ins.h"
#include "subsystems/navigation/common_nav.h"
#include "nps_autopilot.h"
/* fixedwing specific Datalink Ivy functions */
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
@@ -33,6 +34,8 @@ void nps_ivy_init(char* ivy_bus) {
void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
sw/simulator/nps/nps_ivy_mission_commands.c
+30
View File
@@ -93,6 +93,9 @@ void nps_ivy_mission_commands_init(void) {
static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -109,6 +112,9 @@ static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -125,6 +131,9 @@ static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -141,6 +150,9 @@ static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -157,6 +169,9 @@ static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -173,6 +188,9 @@ static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -189,6 +207,9 @@ static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -206,6 +227,9 @@ static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
uint8_t i = 0;
float dummy;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -223,6 +247,8 @@ static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
dl_buffer[3] = (uint8_t)(atoi(argv[2])); //mission_id
@@ -233,6 +259,8 @@ static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
@@ -242,6 +270,8 @@ static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)),
static void on_DL_END_MISSION(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id
sw/simulator/nps/nps_ivy_rotorcraft.c
+3
View File
@@ -41,6 +41,9 @@ void nps_ivy_init(char* ivy_bus) {
static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]) {
if (!autopilot.datalink_enabled)
return;
if (atoi(argv[2]) == AC_ID) {
uint8_t wp_id = atoi(argv[1]);
sw/simulator/nps/nps_sensor_accel.c
+9 -8
View File
@@ -7,27 +7,28 @@
#include NPS_SENSORS_PARAMS
#include "math/pprz_algebra_int.h"
void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time)
void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
{
FLOAT_VECT3_ZERO(accel->value);
accel->min = NPS_ACCEL_MIN;
accel->max = NPS_ACCEL_MAX;
FLOAT_MAT33_DIAG(accel->sensitivity,
NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ);
NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ);
VECT3_ASSIGN(accel->neutral,
NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z);
NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z);
VECT3_ASSIGN(accel->noise_std_dev,
NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z);
NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z);
VECT3_ASSIGN(accel->bias,
NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z);
NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z);
accel->next_update = time;
accel->data_available = FALSE;
}
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu)
void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu)
{
if (time < accel->next_update)
if (time < accel->next_update) {
return;
}
/* transform to imu frame */
struct DoubleVect3 accelero_imu;
@@ -44,7 +45,7 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct
/* white noise */
double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
/* scale */
struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ};
struct DoubleVect3 gain = {accel->sensitivity.m[0], accel->sensitivity.m[4], accel->sensitivity.m[8]};
VECT3_EW_MUL(accelero_error, accelero_error, gain);
/* add error */
VECT3_ADD(accel->value, accelero_error);
sw/simulator/nps/nps_sensor_accel.h
+2 -2
View File
@@ -19,7 +19,7 @@ struct NpsSensorAccel {
};
extern void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time);
extern void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu);
extern void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time);
extern void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu);
#endif /* NPS_SENSOR_ACCEL_H */
sw/simulator/nps/nps_sensor_baro.c
+11 -6
View File
@@ -8,24 +8,29 @@
#include "nps_random.h"
#include NPS_SENSORS_PARAMS
#ifndef NPS_BARO_NOISE_STD_DEV
#define NPS_BARO_NOISE_STD_DEV 0.
#endif
void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time) {
void nps_sensor_baro_init(struct NpsSensorBaro *baro, double time)
{
baro->value = 0.;
baro->noise_std_dev = NPS_BARO_NOISE_STD_DEV;
baro->next_update = time;
baro->data_available = FALSE;
}
void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {
if (time < baro->next_update)
void nps_sensor_baro_run_step(struct NpsSensorBaro *baro, double time)
{
if (time < baro->next_update) {
return;
}
/* pressure in Pascal */
baro->value = pprz_isa_pressure_of_altitude(fdm.hmsl);
/* add noise with std dev Pascal */
baro->value += get_gaussian_noise() * NPS_BARO_NOISE_STD_DEV;
baro->value += get_gaussian_noise() * baro->noise_std_dev;
baro->next_update += NPS_BARO_DT;
baro->data_available = TRUE;
sw/simulator/nps/nps_sensor_baro.h
+3 -2
View File
@@ -8,12 +8,13 @@
struct NpsSensorBaro {
double value; ///< pressure in Pascal
double noise_std_dev; ///< noise standard deviation
double next_update;
bool_t data_available;
};
extern void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time);
extern void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time);
extern void nps_sensor_baro_init(struct NpsSensorBaro *baro, double time);
extern void nps_sensor_baro_run_step(struct NpsSensorBaro *baro, double time);
#endif /* NPS_SENSOR_BARO_H */
sw/simulator/nps/nps_sensor_gps.c
+13 -10
View File
@@ -7,22 +7,23 @@
#include "nps_sensors_utils.h"
#include NPS_SENSORS_PARAMS
void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) {
void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
{
FLOAT_VECT3_ZERO(gps->ecef_pos);
FLOAT_VECT3_ZERO(gps->ecef_vel);
gps->hmsl = 0.0;
gps->pos_latency = NPS_GPS_POS_LATENCY;
gps->speed_latency = NPS_GPS_SPEED_LATENCY;
VECT3_ASSIGN(gps->pos_noise_std_dev,
NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV);
NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV);
VECT3_ASSIGN(gps->speed_noise_std_dev,
NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV);
NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV);
VECT3_ASSIGN(gps->pos_bias_initial,
NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z);
NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z);
VECT3_ASSIGN(gps->pos_bias_random_walk_std_dev,
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X,
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y,
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z);
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X,
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y,
NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z);
FLOAT_VECT3_ZERO(gps->pos_bias_random_walk_value);
gps->next_update = time;
gps->data_available = FALSE;
@@ -35,10 +36,12 @@ void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) {
*
*/
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time)
{
if (time < gps->next_update)
if (time < gps->next_update) {
return;
}
/*
@@ -79,7 +82,7 @@ void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
*/
/* convert current ecef reading to lla */
struct LlaCoor_d cur_lla_reading;
lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d*) &cur_pos_reading);
lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d *) &cur_pos_reading);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);
sw/simulator/nps/nps_sensor_gps.h
+6 -6
View File
@@ -22,16 +22,16 @@ struct NpsSensorGps {
struct DoubleVect3 pos_bias_random_walk_value;
double pos_latency;
double speed_latency;
GSList* hmsl_history;
GSList* pos_history;
GSList* lla_history;
GSList* speed_history;
GSList *hmsl_history;
GSList *pos_history;
GSList *lla_history;
GSList *speed_history;
double next_update;
bool_t data_available;
};
extern void nps_sensor_gps_init(struct NpsSensorGps* gps, double time);
extern void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time);
extern void nps_sensor_gps_init(struct NpsSensorGps *gps, double time);
extern void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time);
#endif /* NPS_SENSOR_GPS_H */
sw/simulator/nps/nps_sensor_gyro.c
+17 -17
View File
@@ -6,36 +6,39 @@
#include "math/pprz_algebra_int.h"
#include "nps_random.h"
void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
{
FLOAT_VECT3_ZERO(gyro->value);
gyro->min = NPS_GYRO_MIN;
gyro->max = NPS_GYRO_MAX;
FLOAT_MAT33_DIAG(gyro->sensitivity,
NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
VECT3_ASSIGN(gyro->neutral,
NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
VECT3_ASSIGN(gyro->noise_std_dev,
NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
VECT3_ASSIGN(gyro->bias_initial,
NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
VECT3_ASSIGN(gyro->bias_random_walk_std_dev,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
FLOAT_VECT3_ZERO(gyro->bias_random_walk_value);
gyro->next_update = time;
gyro->data_available = FALSE;
}
void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) {
void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
{
if (time < gyro->next_update)
if (time < gyro->next_update) {
return;
}
/* transform body rates to IMU frame */
struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel);
struct DoubleVect3 *rate_body = (struct DoubleVect3 *)(&fdm.body_inertial_rotvel);
struct DoubleVect3 rate_imu;
MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body);
/* compute gyros readings */
MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
VECT3_ADD(gyro->value, gyro->neutral);
@@ -44,10 +47,10 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
VECT3_COPY(gyro_error, gyro->bias_initial);
double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev);
double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev,
NPS_GYRO_DT, 5.);
NPS_GYRO_DT, 5.);
VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR};
struct DoubleVect3 gain = {gyro->sensitivity.m[0], gyro->sensitivity.m[4], gyro->sensitivity.m[8]};
VECT3_EW_MUL(gyro_error, gyro_error, gain);
VECT3_ADD(gyro->value, gyro_error);
@@ -60,6 +63,3 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
gyro->next_update += NPS_GYRO_DT;
gyro->data_available = TRUE;
}
sw/simulator/nps/nps_sensor_gyro.h
+2 -2
View File
@@ -21,8 +21,8 @@ struct NpsSensorGyro {
};
extern void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time);
extern void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu);
extern void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time);
extern void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu);
#endif /* NPS_SENSOR_GYRO_H */
sw/simulator/nps/nps_sensor_mag.c
+12 -9
View File
@@ -5,27 +5,30 @@
#include NPS_SENSORS_PARAMS
#include "math/pprz_algebra_int.h"
void nps_sensor_mag_init(struct NpsSensorMag* mag, double time) {
void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
{
VECT3_ASSIGN(mag->value, 0., 0., 0.);
mag->min = NPS_MAG_MIN;
mag->max = NPS_MAG_MAX;
FLOAT_MAT33_DIAG(mag->sensitivity,
NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ);
NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ);
VECT3_ASSIGN(mag->neutral,
NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z);
NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z);
VECT3_ASSIGN(mag->noise_std_dev,
NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z);
NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z);
struct DoubleEulers imu_to_sensor_eulers =
{ NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI };
{ NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI };
DOUBLE_RMAT_OF_EULERS(mag->imu_to_sensor_rmat, imu_to_sensor_eulers);
mag->next_update = time;
mag->data_available = FALSE;
}
void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu) {
void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
{
if (time < mag->next_update)
if (time < mag->next_update) {
return;
}
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
@@ -33,11 +36,11 @@ void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct Doubl
/* transform to imu frame */
struct DoubleVect3 h_imu;
MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body );
MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body);
/* transform to sensor frame */
struct DoubleVect3 h_sensor;
MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu );
MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu);
/* compute magnetometer reading */
MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
sw/simulator/nps/nps_sensor_mag.h
+2 -2
View File
@@ -19,7 +19,7 @@ struct NpsSensorMag {
};
extern void nps_sensor_mag_init(struct NpsSensorMag* mag, double time);
extern void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu);
extern void nps_sensor_mag_init(struct NpsSensorMag *mag, double time);
extern void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu);
#endif /* NPS_SENSOR_MAG_H */
sw/simulator/nps/nps_sensor_sonar.c
+10 -5
View File
@@ -50,22 +50,27 @@
#endif
void nps_sensor_sonar_init(struct NpsSensorSonar* sonar, double time) {
void nps_sensor_sonar_init(struct NpsSensorSonar *sonar, double time)
{
sonar->value = 0.;
sonar->offset = NPS_SONAR_OFFSET;
sonar->noise_std_dev = NPS_SONAR_NOISE_STD_DEV;
sonar->next_update = time;
sonar->data_available = FALSE;
}
void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time) {
void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time)
{
if (time < sonar->next_update)
if (time < sonar->next_update) {
return;
}
/* agl in meters */
sonar->value = fdm.agl + NPS_SONAR_OFFSET;
sonar->value = fdm.agl + sonar->offset;
/* add noise with std dev meters */
sonar->value += get_gaussian_noise() * NPS_SONAR_NOISE_STD_DEV;
sonar->value += get_gaussian_noise() * sonar->noise_std_dev;
sonar->next_update += NPS_SONAR_DT;
sonar->data_available = TRUE;
sw/simulator/nps/nps_sensor_sonar.h
+7 -5
View File
@@ -35,13 +35,15 @@
#include "std.h"
struct NpsSensorSonar {
double value; ///< sonar reading in meters
double next_update;
bool_t data_available;
double value; ///< sonar reading in meters
double offset; ///< offset in meters
double noise_std_dev; ///< noise standard deviation
double next_update;
bool_t data_available;
};
extern void nps_sensor_sonar_init(struct NpsSensorSonar* sonar, double time);
extern void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time);
extern void nps_sensor_sonar_init(struct NpsSensorSonar *sonar, double time);
extern void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time);
#endif /* NPS_SENSOR_SONAR_H */
sw/simulator/nps/nps_sensors.c
+15 -8
View File
@@ -5,10 +5,11 @@
struct NpsSensors sensors;
void nps_sensors_init(double time) {
void nps_sensors_init(double time)
{
struct DoubleEulers body_to_imu_eulers =
{ NPS_BODY_TO_IMU_PHI, NPS_BODY_TO_IMU_THETA, NPS_BODY_TO_IMU_PSI };
{ NPS_BODY_TO_IMU_PHI, NPS_BODY_TO_IMU_THETA, NPS_BODY_TO_IMU_PSI };
DOUBLE_RMAT_OF_EULERS(sensors.body_to_imu_rmat, body_to_imu_eulers);
nps_sensor_gyro_init(&sensors.gyro, time);
@@ -20,7 +21,8 @@ void nps_sensors_init(double time) {
}
void nps_sensors_run_step(double time) {
void nps_sensors_run_step(double time)
{
nps_sensor_gyro_run_step(&sensors.gyro, time, &sensors.body_to_imu_rmat);
nps_sensor_accel_run_step(&sensors.accel, time, &sensors.body_to_imu_rmat);
nps_sensor_mag_run_step(&sensors.mag, time, &sensors.body_to_imu_rmat);
@@ -30,7 +32,8 @@ void nps_sensors_run_step(double time) {
}
bool_t nps_sensors_gyro_available(void) {
bool_t nps_sensors_gyro_available(void)
{
if (sensors.gyro.data_available) {
sensors.gyro.data_available = FALSE;
return TRUE;
@@ -38,7 +41,8 @@ bool_t nps_sensors_gyro_available(void) {
return FALSE;
}
bool_t nps_sensors_mag_available(void) {
bool_t nps_sensors_mag_available(void)
{
if (sensors.mag.data_available) {
sensors.mag.data_available = FALSE;
return TRUE;
@@ -46,7 +50,8 @@ bool_t nps_sensors_mag_available(void) {
return FALSE;
}
bool_t nps_sensors_baro_available(void) {
bool_t nps_sensors_baro_available(void)
{
if (sensors.baro.data_available) {
sensors.baro.data_available = FALSE;
return TRUE;
@@ -54,7 +59,8 @@ bool_t nps_sensors_baro_available(void) {
return FALSE;
}
bool_t nps_sensors_gps_available(void) {
bool_t nps_sensors_gps_available(void)
{
if (sensors.gps.data_available) {
sensors.gps.data_available = FALSE;
return TRUE;
@@ -62,7 +68,8 @@ bool_t nps_sensors_gps_available(void) {
return FALSE;
}
bool_t nps_sensors_sonar_available(void) {
bool_t nps_sensors_sonar_available(void)
{
if (sensors.sonar.data_available) {
sensors.sonar.data_available = FALSE;
return TRUE;
sw/simulator/nps/nps_sensors_utils.c
+19 -16
View File
@@ -3,44 +3,47 @@
//#include <string.h>
#include "math/pprz_algebra.h"
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) {
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
{
/* add new reading */
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
struct BoozDatedSensor *cur_read = g_new(struct BoozDatedSensor, 1);
cur_read->time = time;
cur_read->value = (struct DoubleVect3*) g_memdup(cur_reading, sizeof(struct DoubleVect3));
cur_read->value = (struct DoubleVect3 *) g_memdup(cur_reading, sizeof(struct DoubleVect3));
*history = g_slist_prepend(*history, cur_read);
/* remove old readings */
GSList* last = g_slist_last(*history);
while (last && ((struct BoozDatedSensor*)last->data)->time < time - latency) {
GSList *last = g_slist_last(*history);
while (last && ((struct BoozDatedSensor *)last->data)->time < time - latency) {
*history = g_slist_remove_link(*history, last);
g_free(((struct BoozDatedSensor*)last->data)->value);
g_free((struct BoozDatedSensor*)last->data);
g_free(((struct BoozDatedSensor *)last->data)->value);
g_free((struct BoozDatedSensor *)last->data);
g_slist_free_1(last);
last = g_slist_last(*history);
}
/* update sensor */
//g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
VECT3_COPY(*((struct DoubleVect3*)sensor_reading), *((struct BoozDatedSensor*)last->data)->value);
VECT3_COPY(*((struct DoubleVect3 *)sensor_reading), *((struct BoozDatedSensor *)last->data)->value);
}
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) {
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency,
gpointer sensor_reading)
{
/* add new reading */
struct BoozDatedSensor_Single* cur_read = g_new(struct BoozDatedSensor_Single, 1);
struct BoozDatedSensor_Single *cur_read = g_new(struct BoozDatedSensor_Single, 1);
cur_read->time = time;
cur_read->value = (double*) g_memdup(cur_reading, sizeof(double));
cur_read->value = (double *) g_memdup(cur_reading, sizeof(double));
*history = g_slist_prepend(*history, cur_read);
/* remove old readings */
GSList* last = g_slist_last(*history);
while (last && ((struct BoozDatedSensor_Single*)last->data)->time < time - latency) {
GSList *last = g_slist_last(*history);
while (last && ((struct BoozDatedSensor_Single *)last->data)->time < time - latency) {
*history = g_slist_remove_link(*history, last);
g_free(((struct BoozDatedSensor_Single*)last->data)->value);
g_free((struct BoozDatedSensor_Single*)last->data);
g_free(((struct BoozDatedSensor_Single *)last->data)->value);
g_free((struct BoozDatedSensor_Single *)last->data);
g_slist_free_1(last);
last = g_slist_last(*history);
}
/* update sensor */
//g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
*((double*)sensor_reading) = *(((struct BoozDatedSensor_Single*)last->data)->value);
*((double *)sensor_reading) = *(((struct BoozDatedSensor_Single *)last->data)->value);
}
sw/simulator/nps/nps_sensors_utils.h
+4 -4
View File
@@ -5,21 +5,21 @@
#include "math/pprz_algebra_double.h"
struct BoozDatedSensor {
struct DoubleVect3* value;
struct DoubleVect3 *value;
double time;
};
struct BoozDatedSensor_Single {
double* value;
double *value;
double time;
};
/* cur_reading and sensor_reading must be of a type that can be cast to DoubleVect3* */
extern void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history,
double latency, gpointer sensor_reading);
double latency, gpointer sensor_reading);
/* ...and the same for single double values */
extern void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history,
double latency, gpointer sensor_reading);
double latency, gpointer sensor_reading);
#endif /* NPS_SENSORS_UTILS_H */