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

nps: add hmsl to gps

Browse Source
This commit is contained in:
Felix Ruess
2010-06-20 23:41:10 +00:00
parent a53205da24
commit 8e76ae4cf1
6 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_gps.h
+2 -2
View File
@@ -69,8 +69,8 @@ static inline void booz_gps_feed_value() {
booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100. + NAV_HMSL0; booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100.;
booz_gps_state.hmsl = sensors.gps.lla_pos.alt * 100.; booz_gps_state.hmsl = sensors.gps.hmsl * 100.;
booz_gps_state.fix = BOOZ2_GPS_FIX_3D; booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
booz_gps_available = TRUE; booz_gps_available = TRUE;
} }
sw/airborne/math/pprz_geodetic_int.h
+2 -2
View File
@@ -15,8 +15,8 @@ struct EcefCoor_i {
int32_t z; int32_t z;
}; };
/* lon, lat in radians */ /* lon, lat in radians*1e7 */
/* alt in meters */ /* alt in centimeters */
struct LlaCoor_i { struct LlaCoor_i {
int32_t lon; int32_t lon;
int32_t lat; int32_t lat;
sw/simulator/nps/nps_fdm.h
+1
View File
@@ -25,6 +25,7 @@ struct NpsFdm {
struct EcefCoor_d ecef_pos; struct EcefCoor_d ecef_pos;
struct NedCoor_d ltpprz_pos; struct NedCoor_d ltpprz_pos;
struct LlaCoor_d lla_pos; struct LlaCoor_d lla_pos;
double hmsl;
/* velocity and acceleration wrt inertial frame expressed in ecef frame */ /* velocity and acceleration wrt inertial frame expressed in ecef frame */
// struct EcefCoor_d ecef_inertial_vel; // struct EcefCoor_d ecef_inertial_vel;
sw/simulator/nps/nps_fdm_jsbsim.c
+3 -2
View File
@@ -82,6 +82,7 @@ static void fetch_state(void) {
* position * position
*/ */
jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation()); jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation());
fdm.hmsl = propagate->GetAltitudeASLmeters();
/* /*
* linear speed and accelerations * linear speed and accelerations
@@ -247,9 +248,9 @@ static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate) {
void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGLocation* jsb_location) { void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGLocation* jsb_location) {
fdm_lla->lat = jsb_location->GetLatitude(); fdm_lla->lat = jsb_location->GetGeodLatitudeRad();
fdm_lla->lon = jsb_location->GetLongitude(); fdm_lla->lon = jsb_location->GetLongitude();
fdm_lla->alt = MetersOfFeet(jsb_location->GetGeodAltitude()); fdm_lla->alt = MetersOfFeet(jsb_location->GetGeodeticAltitude());
// printf("%f\n", jsb_location->GetGeodAltitude()); // printf("%f\n", jsb_location->GetGeodAltitude());
} }
#endif #endif
sw/simulator/nps/nps_sensor_gps.c
+3
View File
@@ -10,6 +10,7 @@
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_pos);
FLOAT_VECT3_ZERO(gps->ecef_vel); FLOAT_VECT3_ZERO(gps->ecef_vel);
gps->hmsl = 0.0;
gps->pos_latency = NPS_GPS_POS_LATENCY; gps->pos_latency = NPS_GPS_POS_LATENCY;
gps->speed_latency = NPS_GPS_SPEED_LATENCY; gps->speed_latency = NPS_GPS_SPEED_LATENCY;
VECT3_ASSIGN(gps->pos_noise_std_dev, VECT3_ASSIGN(gps->pos_noise_std_dev,
@@ -83,6 +84,8 @@ void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
/* store that for later and retrieve a previously stored data */ /* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos); UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);
double cur_hmsl_reading = fdm.hmsl;
UpdateSensorLatency(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl);
gps->next_update += NPS_GPS_DT; gps->next_update += NPS_GPS_DT;
gps->data_available = TRUE; gps->data_available = TRUE;
sw/simulator/nps/nps_sensor_gps.h
+4 -2
View File
@@ -14,6 +14,7 @@ struct NpsSensorGps {
struct EcefCoor_d ecef_pos; struct EcefCoor_d ecef_pos;
struct EcefCoor_d ecef_vel; struct EcefCoor_d ecef_vel;
struct LlaCoor_d lla_pos; struct LlaCoor_d lla_pos;
double hmsl;
struct DoubleVect3 pos_noise_std_dev; struct DoubleVect3 pos_noise_std_dev;
struct DoubleVect3 speed_noise_std_dev; struct DoubleVect3 speed_noise_std_dev;
struct DoubleVect3 pos_bias_initial; struct DoubleVect3 pos_bias_initial;
@@ -21,11 +22,12 @@ struct NpsSensorGps {
struct DoubleVect3 pos_bias_random_walk_value; struct DoubleVect3 pos_bias_random_walk_value;
double pos_latency; double pos_latency;
double speed_latency; double speed_latency;
GSList* hmsl_history;
GSList* pos_history; GSList* pos_history;
GSList* lla_history; GSList* lla_history;
GSList* speed_history; GSList* speed_history;
double next_update; double next_update;
bool_t data_available; bool_t data_available;
}; };