Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

nps getting there

Browse Source
This commit is contained in:
Antoine Drouin
2009-06-24 10:43:06 +00:00
parent ce771d1ce0
commit 59e526a779
13 changed files with 174 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/booz2_simulator_nps.makefile
+5
View File
@@ -26,10 +26,15 @@ sim.CFLAGS += -DJSBSIM_ROOT_DIR=\"/home/violato/enac/programs/JSBSim/\"
sim.srcs = $(SIMDIR)/nps_main.c \
$(SIMDIR)/nps_fdm_jsbsim.c \
$(SIMDIR)/nps_random.c \
$(SIMDIR)/booz_r250.c \
$(SIMDIR)/booz_randlcg.c \
$(SIMDIR)/nps_sensors.c \
$(SIMDIR)/nps_sensor_gyro.c \
$(SIMDIR)/nps_sensor_accel.c \
$(SIMDIR)/nps_sensor_mag.c \
$(SIMDIR)/nps_sensor_baro.c \
$(SIMDIR)/nps_sensor_gps.c \
$(SIMDIR)/nps_autopilot.c \
sim.srcs += $(SRC_BOOZ)/booz_trig_int.c
conf/simulator/nps_sensors_params_booz2_a1.h
+5 -9
View File
@@ -126,10 +126,10 @@
*/
/* m */
/* aka 2^8/INS_BARO_SENS */
#define BSM_BARO_QNH 900.
#define BSM_BARO_SENSITIVITY 17.066667
#define BSM_BARO_DT (1./100.)
#define BSM_BARO_NOISE_STD_DEV 5.e-2
#define NPS_BARO_QNH 900.
#define NPS_BARO_SENSITIVITY 17.066667
#define NPS_BARO_DT (1./100.)
#define NPS_BARO_NOISE_STD_DEV 5.e-2
/*
* GPS
@@ -163,10 +163,6 @@
#endif /* GPS_PERFECT */
#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
#define BSM_GPS_DT (1./4.)
#define NPS_GPS_DT (1./4.)
#endif /* NPS_SENSORS_PARAMS_H */
sw/simulator/nps_autopilot.c
+12 -1
View File
@@ -2,6 +2,8 @@
struct NpsAutopilot autopilot;
#include "nps_sensors.h"
void nps_autopilot_init(void) {
/* Just for testing fdm */
@@ -12,4 +14,13 @@ void nps_autopilot_init(void) {
autopilot.commands[3] = 0.5;
}
void nps_autopilot_run_step(void) {}
void nps_autopilot_run_step(void) {
if (nps_sensors_gyro_available()) {
// booz2_imu_feed_data();
// booz2_main_event();
}
}
sw/simulator/nps_fdm.h
+2
View File
@@ -15,6 +15,8 @@ struct NpsFdm {
struct EcefCoor_d ecef_vel;
struct DoubleVect3 body_accel;
struct NedCoor_d ltp_pos;
struct DoubleQuat ltp_to_body_quat;
struct DoubleRates body_rate;
struct DoubleRates body_rate_dot;
sw/simulator/nps_main.c
+5 -4
View File
@@ -66,20 +66,21 @@ static void nps_main_init(void) {
static void nps_main_run_sim_step(void) {
printf("sim at %f\n", nps_main.sim_time);
// printf("sim at %f\n", nps_main.sim_time);
nps_fdm_run_step(autopilot.commands);
// nps_sensors_run_step();
nps_sensors_run_step(nps_main.sim_time);
// nps_autopilot_run_step();
nps_autopilot_run_step();
}
static void nps_main_display(void) {
printf("display at %f\n", nps_main.display_time);
// printf("display at %f\n", nps_main.display_time);
nps_ivy_display();
}
sw/simulator/nps_sensor_accel.c
+16 -2
View File
@@ -2,8 +2,10 @@
#include "airframe.h"
#include "nps_fdm.h"
#include "nps_random.h"
#include NPS_SENSORS_PARAMS
void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
VECT3_ASSIGN(accel->value, 0., 0., 0.);
accel->resolution = NPS_ACCEL_RESOLUTION;
@@ -19,6 +21,7 @@ void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
accel->data_available = FALSE;
}
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) {
if (time < accel->next_update)
@@ -39,8 +42,19 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
/* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
/* FIXME: ADD error reading */
/* Compute sensor error */
struct DoubleVect3 accelero_error;
/* constant bias */
VECT3_COPY(accelero_error, accel->bias);
/* white noise */
double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
/* scale */
accelero_error.x *= MAT33_ELMT(accel->sensitivity, 0, 0);
accelero_error.y *= MAT33_ELMT(accel->sensitivity, 1, 1);
accelero_error.z *= MAT33_ELMT(accel->sensitivity, 2, 2);
/* add error */
VECT3_ADD(accel->value, accelero_error);
/* round signal to account for adc discretisation */
DOUBLE_VECT3_ROUND(accel->value);
/* saturate */
sw/simulator/nps_sensor_baro.c
+31
View File
@@ -0,0 +1,31 @@
#include "nps_sensor_baro.h"
#include "airframe.h"
#include "nps_fdm.h"
#include "nps_random.h"
#include NPS_SENSORS_PARAMS
void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time) {
baro->value = 0.;
baro->next_update = time;
baro->data_available = FALSE;
}
void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {
if (time < baro->next_update)
return;
if (time < 12.5)
baro->value = 840;
else {
double z = fdm.ltp_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV;
double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY;
baro_reading = rint(baro_reading);
baro->value = baro_reading;
}
baro->next_update += NPS_BARO_DT;
baro->data_available = TRUE;
}
sw/simulator/nps_sensor_baro.h
+19
View File
@@ -0,0 +1,19 @@
#ifndef NPS_SENSOR_BARO_H
#define NPS_SENSOR_BARO_H
#include "pprz_algebra.h"
#include "pprz_algebra_double.h"
#include "pprz_algebra_float.h"
#include "std.h"
struct NpsSensorBaro {
double value;
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);
#endif /* NPS_SENSOR_BARO_H */
sw/simulator/nps_sensor_gps.c
+28
View File
@@ -0,0 +1,28 @@
#include "nps_sensor_gps.h"
#include "airframe.h"
#include "nps_fdm.h"
#include "nps_random.h"
#include NPS_SENSORS_PARAMS
void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) {
FLOAT_VECT3_ZERO(gps->ecef_pos);
FLOAT_VECT3_ZERO(gps->ecef_vel);
gps->next_update = time;
gps->data_available = FALSE;
}
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
if (time < gps->next_update)
return;
VECT3_COPY(gps->ecef_pos, fdm.ecef_pos);
VECT3_COPY(gps->ecef_vel, fdm.ecef_vel);
gps->next_update += NPS_GPS_DT;
gps->data_available = TRUE;
}
sw/simulator/nps_sensor_gps.h
+22
View File
@@ -0,0 +1,22 @@
#ifndef NPS_SENSOR_GPS_H
#define NPS_SENSOR_GPS_H
#include "pprz_algebra.h"
#include "pprz_algebra_double.h"
#include "pprz_algebra_float.h"
#include "pprz_geodetic_double.h"
#include "std.h"
struct NpsSensorGps {
struct EcefCoor_d ecef_pos;
struct EcefCoor_d ecef_vel;
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);
#endif /* NPS_SENSOR_GPS_H */
sw/simulator/nps_sensor_mag.h
+1 -1
View File
@@ -21,4 +21,4 @@ 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);
#endif /* NPS_SENSOR_ACCEL_H */
#endif /* NPS_SENSOR_MAG_H */
sw/simulator/nps_sensors.c
+21
View File
@@ -14,13 +14,18 @@ void nps_sensors_init(double time) {
nps_sensor_gyro_init(&sensors.gyro, time);
nps_sensor_accel_init(&sensors.accel, time);
nps_sensor_mag_init(&sensors.mag, time);
nps_sensor_baro_init(&sensors.baro, time);
nps_sensor_gps_init(&sensors.gps, 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);
nps_sensor_baro_run_step(&sensors.baro, time);
nps_sensor_gps_run_step(&sensors.gps, time);
}
@@ -39,3 +44,19 @@ bool_t nps_sensors_mag_available(void) {
}
return FALSE;
}
bool_t nps_sensors_baro_available(void) {
if (sensors.baro.data_available) {
sensors.baro.data_available = FALSE;
return TRUE;
}
return FALSE;
}
bool_t nps_sensors_gps_available(void) {
if (sensors.gps.data_available) {
sensors.gps.data_available = FALSE;
return TRUE;
}
return FALSE;
}
sw/simulator/nps_sensors.h
+7 -5
View File
@@ -5,8 +5,8 @@
#include "nps_sensor_gyro.h"
#include "nps_sensor_accel.h"
#include "nps_sensor_mag.h"
//#include "nps_sensor_baro.h"
//#include "nps_sensor_gps.h"
#include "nps_sensor_baro.h"
#include "nps_sensor_gps.h"
struct NpsSensors {
@@ -14,9 +14,8 @@ struct NpsSensors {
struct NpsSensorGyro gyro;
struct NpsSensorAccel accel;
struct NpsSensorMag mag;
// struct NpsSensorBaro baro;
// struct NpsSensorGps gps;
struct NpsSensorBaro baro;
struct NpsSensorGps gps;
};
extern struct NpsSensors sensors;
@@ -26,5 +25,8 @@ extern void nps_sensors_run_step(double time);
extern bool_t nps_sensors_gyro_available();
extern bool_t nps_sensors_mag_available();
extern bool_t nps_sensors_baro_available();
extern bool_t nps_sensors_gps_available();
#endif /* NPS_SENSORS_H */