From 32c7aea2a6a0c355d2cae362884e40e4f3573311 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:07:45 +0100 Subject: [PATCH 1/2] Home position: use double for lat/lon and float for altitude, set home position to global position instead of GPS position once we have a fix --- src/drivers/hott/messages.cpp | 4 ++-- src/modules/commander/commander.cpp | 20 +++++++------------- src/modules/mavlink/orb_listener.c | 2 +- src/modules/navigator/navigator_main.cpp | 21 ++++++++++----------- src/modules/uORB/topics/home_position.h | 20 +++++++++----------- 5 files changed, 29 insertions(+), 38 deletions(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index bb8d45beab..90a7440159 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), _home_sub, &home); - _home_lat = ((double)(home.lat))*1e-7d; - _home_lon = ((double)(home.lon))*1e-7d; + _home_lat = home.lat; + _home_lon = home.lon; _home_position_set = true; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fc1d2c191..442f16a58f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1048,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[]) if (!home_position_set && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed + && global_position.valid) { /* copy position data to uORB home message, store it locally as well */ - // TODO use global position estimate - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; + home.lat = (double)global_position.lat / 1e7d; + home.lon = (double)global_position.lon / 1e7d; + home.altitude = (float)global_position.alt / 1e3f; - double home_lat_d = home.lat * 1e-7; - double home_lon_d = home.lon * 1e-7; - warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index de902f3da0..96888f06a0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -657,7 +657,7 @@ l_home(const struct listener *l) orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7ac885b48..c6fe93e9e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -953,8 +953,7 @@ Navigator::start_loiter() get_loiter_item(&_mission_item_triplet.current); - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + float global_min_alt = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { @@ -1080,9 +1079,9 @@ Navigator::start_rtl() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; @@ -1104,9 +1103,9 @@ Navigator::start_rtl_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; get_loiter_item(&_mission_item_triplet.current); @@ -1319,9 +1318,9 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) { if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0fb..1cf37e29c0 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -2,6 +2,7 @@ * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +35,10 @@ /** * @file home_position.h - * Definition of the GPS home position uORB topic. + * Definition of the home position uORB topic. * * @author Lorenz Meier + * @author Julian Oes */ #ifndef TOPIC_HOME_POSITION_H_ @@ -55,16 +57,12 @@ */ struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + + bool altitude_is_relative; + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float altitude; /**< Altitude in meters */ }; /** From effa62937fbaab0e446a95ac1ac9447a2895594e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:14:08 +0100 Subject: [PATCH 2/2] Prevent some warnings for lat/lon double conversions --- src/lib/geo/geo.c | 4 ++-- src/modules/mavlink/orb_listener.c | 4 ++-- src/modules/navigator/navigator_main.cpp | 10 +++++----- .../position_estimator_inav_main.c | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 85b17f9ae7..f64bfb41a4 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -513,8 +513,8 @@ __EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, c unsigned int i, j, vertices = fence->count; bool c = false; - double lat = vehicle->lat / 1e7; - double lon = vehicle->lon / 1e7; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; // skip vertex 0 (return point) for (i = 0, j = vertices - 1; i < vertices; j = i++) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 96888f06a0..17978615f0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -416,8 +416,8 @@ l_global_position_setpoint(const struct listener *l) if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, - (int32_t)(triplet.current.lat * 1e7f), - (int32_t)(triplet.current.lon * 1e7f), + (int32_t)(triplet.current.lat * 1e7d), + (int32_t)(triplet.current.lon * 1e7d), (int32_t)(triplet.current.altitude * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6fe93e9e1..7be9229c9b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -723,7 +723,7 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", @@ -947,8 +947,8 @@ Navigator::start_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; _mission_item_triplet.current.yaw = 0.0f; get_loiter_item(&_mission_item_triplet.current); @@ -1162,11 +1162,11 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); - // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt); + // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); // warnx("Dist: %4.4f", dist); 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 3084b6d928..5bf0fba30e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -614,8 +614,8 @@ 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); - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7d); + global_pos.lon = (int32_t)(est_lon * 1e7d); global_pos.time_gps_usec = gps.time_gps_usec; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5dc325a05e..9f634d9e4e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1153,8 +1153,8 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7); + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;