Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 29 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.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.alt = sensors.gps.lla_pos.alt * 100. + NAV_HMSL0;
booz_gps_state.hmsl = sensors.gps.lla_pos.alt * 100.;
booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100.;
booz_gps_state.hmsl = sensors.gps.hmsl * 100.;
booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
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;
};
/* lon, lat in radians */
/* alt in meters */
/* lon, lat in radians*1e7 */
/* alt in centimeters */
struct LlaCoor_i {
int32_t lon;
int32_t lat;
sw/simulator/nps/nps_fdm.h
+1
View File
@@ -25,6 +25,7 @@ struct NpsFdm {
struct EcefCoor_d ecef_pos;
struct NedCoor_d ltpprz_pos;
struct LlaCoor_d lla_pos;
double hmsl;
/* velocity and acceleration wrt inertial frame expressed in ecef frame */
// 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
*/
jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation());
fdm.hmsl = propagate->GetAltitudeASLmeters();
/*
* 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) {
fdm_lla->lat = jsb_location->GetLatitude();
fdm_lla->lat = jsb_location->GetGeodLatitudeRad();
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());
}
#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) {
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,
@@ -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 */
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->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_vel;
struct LlaCoor_d lla_pos;
double hmsl;
struct DoubleVect3 pos_noise_std_dev;
struct DoubleVect3 speed_noise_std_dev;
struct DoubleVect3 pos_bias_initial;
@@ -21,11 +22,12 @@ 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;
double next_update;
bool_t data_available;
double next_update;
bool_t data_available;
};