dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/navigator_new' into fw_autoland_att_tecs_navigator_termination_controlgroups

This commit is contained in:
Thomas Gubler 2013-12-27 11:29:38 +01:00
parent 80c1710434 effa62937f
commit ef15d6360a
8 changed files with 42 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/hott/messages.cpp
View File

@ -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;
}

4
src/lib/geo/geo.c
View File

@ -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++)

20
src/modules/commander/commander.cpp
View File

@ -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) {

6
src/modules/mavlink/orb_listener.c
View File

@ -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));
}
@ -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

31
src/modules/navigator/navigator_main.cpp
View File

@ -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,14 +947,13 @@ 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);
/* 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);
@ -1163,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);
@ -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
}

4
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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;
}

4
src/modules/sdlog2/sdlog2.c
View File

@ -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;

20
src/modules/uORB/topics/home_position.h
View File

@ -2,6 +2,7 @@
*
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#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 */
};
/**