Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Use double for lat/lon in vehicle_global_position topic, use filed names lat, lon, alt, vel_n, vel_e, vel_d for global positions

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-24 00:06:10 +01:00
parent b3d98e4a19
commit 58792c5ca6
16 changed files with 76 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/frsky_data.c
+3 -3
View File
@@ -230,11 +230,11 @@ void frsky_send_frame2(int uart)
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f);
lat = frsky_format_gps(abs(global_pos.lat));
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f);
lon = frsky_format_gps(abs(global_pos.lon));
lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy)
speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
* 25.0f / 46.0f;
alt = global_pos.alt;
sec = tm_gps->tm_sec;
src/examples/fixedwing_control/main.c
+1 -5
View File
@@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
* Calculate heading error of current position to desired position
*/
/*
* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
* so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
*/
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+3 -4
View File
@@ -318,10 +318,9 @@ void KalmanNav::updatePublications()
_pos.lat = getLatDegE7();
_pos.lon = getLonDegE7();
_pos.alt = float(alt);
_pos.relative_alt = float(alt); // TODO, make relative
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
_pos.vel_n = vN;
_pos.vel_e = vE;
_pos.vel_d = vD;
_pos.yaw = psi;
// local position publication
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+3 -3
View File
@@ -414,9 +414,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
vel(0) = global_pos.vx;
vel(1) = global_pos.vy;
vel(2) = global_pos.vz;
vel(0) = global_pos.vel_n;
vel(1) = global_pos.vel_e;
vel(2) = global_pos.vel_d;
}
}
src/modules/commander/commander.cpp
+5 -5
View File
@@ -1009,12 +1009,12 @@ int commander_thread_main(int argc, char *argv[])
/* copy position data to uORB home message, store it locally as well */
home.lat = (double)global_position.lat / 1e7d;
home.lon = (double)global_position.lon / 1e7d;
home.altitude = (float)global_position.alt;
home.lat = global_position.lat;
home.lon = global_position.lon;
home.alt = global_position.alt;
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);
warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
src/modules/fixedwing_backside/fixedwing.cpp
+4 -4
View File
@@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update()
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vel_n * _pos.vel_n +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
_pos.vel_d * _pos.vel_d));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
@@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update()
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vel_n * _pos.vel_n +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
_pos.vel_d * _pos.vel_d));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+1 -1
View File
@@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
src/modules/fw_att_control/fw_att_control_main.cpp
+3 -3
View File
@@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main()
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz;
speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz;
speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz;
speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
warnx("Did not get a valid R\n");
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+1 -1
View File
@@ -1262,7 +1262,7 @@ FixedwingPositionControl::task_main()
vehicle_airspeed_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
/*
src/modules/mavlink/mavlink_receiver.cpp
+3 -3
View File
@@ -638,9 +638,9 @@ handle_message(mavlink_message_t *msg)
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
src/modules/mavlink/orb_listener.c
+10 -11
View File
@@ -67,6 +67,7 @@ extern bool gcs_link;
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct home_position_s home;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
@@ -247,10 +248,10 @@ l_vehicle_attitude(const struct listener *l)
hrt_abstime t = hrt_absolute_time();
if (t >= last_sent_vfr + 100000) {
last_sent_vfr = t;
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
}
/* send quaternion values if it exists */
@@ -380,13 +381,13 @@ l_global_position(const struct listener *l)
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
global_pos.timestamp / 1000,
global_pos.lat,
global_pos.lon,
global_pos.lat * 1e7,
global_pos.lon * 1e7,
global_pos.alt * 1000.0f,
global_pos.relative_alt * 1000.0f,
global_pos.vx * 100.0f,
global_pos.vy * 100.0f,
global_pos.vz * 100.0f,
(global_pos.alt - home.alt) * 1000.0f,
global_pos.vel_n * 100.0f,
global_pos.vel_e * 100.0f,
global_pos.vel_d * 100.0f,
_wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
@@ -657,11 +658,9 @@ l_optical_flow(const struct listener *l)
void
l_home(const struct listener *l)
{
struct home_position_s home;
orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
}
void
src/modules/navigator/navigator_main.cpp
+20 -20
View File
@@ -833,11 +833,11 @@ 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 / 1e7d, _global_pos.lat / 1e7d);
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
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",
(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
(double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
}
if (_fence_valid) {
@@ -964,11 +964,11 @@ Navigator::start_loiter()
if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
_reset_loiter_pos = false;
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl) {
@@ -1063,8 +1063,8 @@ Navigator::set_mission_item()
/* set current setpoint to takeoff */
_pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = takeoff_alt_amsl;
_pos_sp_triplet.current.yaw = NAN;
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
@@ -1111,7 +1111,7 @@ Navigator::start_rtl()
{
_do_takeoff = false;
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
@@ -1133,15 +1133,15 @@ Navigator::set_rtl_item()
case RTL_STATE_CLIMB: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
float climb_alt = _home_pos.alt + _parameters.rtl_alt;
if (_vstatus.condition_landed) {
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
}
_mission_item_valid = true;
_mission_item.lat = (double)_global_pos.lat / 1e7d;
_mission_item.lon = (double)_global_pos.lon / 1e7d;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
@@ -1158,7 +1158,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt);
break;
}
case RTL_STATE_RETURN: {
@@ -1194,7 +1194,7 @@ Navigator::set_rtl_item()
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.altitude + _parameters.land_alt;
_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
@@ -1220,7 +1220,7 @@ Navigator::set_rtl_item()
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.altitude;
_mission_item.altitude = _home_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
@@ -1256,11 +1256,11 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
/* set home position for RTL item */
sp->lat = _home_pos.lat;
sp->lon = _home_pos.lon;
sp->alt = _home_pos.altitude + _parameters.rtl_alt;
sp->alt = _home_pos.alt + _parameters.rtl_alt;
} else {
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude;
sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
}
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
@@ -1325,10 +1325,10 @@ Navigator::check_mission_item_reached()
/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
float wp_alt_amsl = _mission_item.altitude;
if (_mission_item.altitude_is_relative)
wp_alt_amsl += _home_pos.altitude;
wp_alt_amsl += _home_pos.alt;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
&dist_xy, &dist_z);
if (_do_takeoff) {
src/modules/position_estimator_inav/position_estimator_inav_main.c
+5 -9
View File
@@ -840,19 +840,15 @@ 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 * 1e7d);
global_pos.lon = (int32_t)(est_lon * 1e7d);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.time_gps_usec = gps.time_gps_usec;
}
/* set valid values even if position is not valid */
if (local_pos.v_xy_valid) {
global_pos.vx = local_pos.vx;
global_pos.vy = local_pos.vy;
}
if (local_pos.z_valid) {
global_pos.relative_alt = -local_pos.z;
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
}
if (local_pos.z_global) {
@@ -860,7 +856,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (local_pos.v_z_valid) {
global_pos.vz = local_pos.vz;
global_pos.vel_d = local_pos.vz;
}
global_pos.yaw = local_pos.yaw;
src/modules/sdlog2/sdlog2.c
+5 -5
View File
@@ -1252,12 +1252,12 @@ int sdlog2_thread_main(int argc, char *argv[])
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat;
log_msg.body.log_GPOS.lon = buf.global_pos.lon;
log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
src/modules/uORB/topics/home_position.h
+1 -1
View File
@@ -62,7 +62,7 @@ struct home_position_s
//bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float altitude; /**< Altitude in meters */
float alt; /**< Altitude in meters */
};
/**
src/modules/uORB/topics/vehicle_global_position.h
+8 -9
View File
@@ -54,7 +54,7 @@
/**
* Fused global position in WGS84.
*
* This struct contains the system's believ about its position. It is not the raw GPS
* This struct contains global position estimation. It is not the raw GPS
* measurement (@see vehicle_gps_position). This topic is usually published by the position
* estimator, which will take more sources of information into account than just GPS,
* e.g. control inputs of the vehicle in a Kalman-filter implementation.
@@ -65,14 +65,13 @@ struct vehicle_global_position_s
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
float alt; /**< Altitude in meters */
float relative_alt; /**< Altitude above home position in meters, */
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
float yaw; /**< Compass heading in radians -PI..+PI. */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
};
/**