diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-17 22:20:41 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-17 22:20:41 +0400 |
commit | 3d5f52678fa093a248d824828fcafe12ac2f8f15 (patch) | |
tree | b95401fd45205dac6c8311de68ad0cdb101364a2 /src/modules | |
parent | 2284a7e985b174dab4b3c1666d9f019d9479a230 (diff) | |
download | px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.tar.gz px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.tar.bz2 px4-firmware-3d5f52678fa093a248d824828fcafe12ac2f8f15.zip |
Use updated map_projection_XXX functions in apps
Diffstat (limited to 'src/modules')
4 files changed, 14 insertions, 11 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 668bac5d9..a4d5560c7 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -97,6 +97,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + memset(&ref, 0, sizeof(ref)); + F.zero(); G.zero(); V.zero(); @@ -247,11 +249,7 @@ void KalmanNav::update() lat0 = lat; lon0 = lon; alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); + map_projection_init(&ref, lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", @@ -327,7 +325,7 @@ void KalmanNav::updatePublications() float x; float y; bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); + map_projection_project(&ref, lat, lon, &x, &y); _localPos.timestamp = _pubTimeStamp; _localPos.xy_valid = true; _localPos.z_valid = true; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8..5021b9927 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -49,6 +49,7 @@ #include <controllib/block/BlockParam.hpp> #include <controllib/uorb/UOrbSubscription.hpp> #include <controllib/uorb/UOrbPublication.hpp> +#include <lib/geo/geo.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> @@ -164,6 +165,7 @@ protected: // parameters float alt; /**< altitude, meters */ double lat0, lon0; /**< reference latitude and longitude */ + struct map_projection_reference_s ref; /**< local projection reference */ float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1dbe56495..624740237 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -132,6 +132,7 @@ static orb_advert_t telemetry_status_pub = -1; static int32_t lat0 = 0; static int32_t lon0 = 0; static double alt0 = 0; +struct map_projection_reference_s hil_ref; static void handle_message(mavlink_message_t *msg) @@ -653,7 +654,7 @@ handle_message(mavlink_message_t *msg) bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? double lat = hil_state.lat*1e-7; double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); + map_projection_project(&hil_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -679,7 +680,7 @@ handle_message(mavlink_message_t *msg) lat0 = hil_state.lat; lon0 = hil_state.lon; alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); + map_projection_init(&hil_ref, hil_state.lat, hil_state.lon); } /* Calculate Rotation Matrix */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index a14354138..eddf6e94e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -206,6 +206,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + struct map_projection_reference_s ref; + memset(&ref, 0, sizeof(ref)); uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -560,7 +562,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = t; /* initialize projection */ - map_projection_init(lat, lon); + map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } @@ -569,7 +571,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; @@ -836,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; |