vehicle_local_position: use double for ref_lat and ref_lon instead of int32, fix related apps

This commit is contained in:
Anton Babushkin
2014-03-17 23:58:00 +04:00
parent 2f7303f2dd
commit c266124099
6 changed files with 12 additions and 12 deletions
@@ -341,8 +341,8 @@ void KalmanNav::updatePublications()
_localPos.xy_global = true; _localPos.xy_global = true;
_localPos.z_global = true; _localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp; _localPos.ref_timestamp = _pubTimeStamp;
_localPos.ref_lat = getLatDegE7(); _localPos.ref_lat = lat * M_RAD_TO_DEG;
_localPos.ref_lon = getLonDegE7(); _localPos.ref_lon = lon * M_RAD_TO_DEG;
_localPos.ref_alt = 0; _localPos.ref_alt = 0;
_localPos.landed = landed; _localPos.landed = landed;
+2 -2
View File
@@ -670,8 +670,8 @@ handle_message(mavlink_message_t *msg)
hil_local_pos.xy_global = true; hil_local_pos.xy_global = true;
hil_local_pos.z_global = true; hil_local_pos.z_global = true;
hil_local_pos.ref_timestamp = timestamp; hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat; hil_local_pos.ref_lat = hil_state.lat / 1e7d;
hil_local_pos.ref_lon = hil_state.lon; hil_local_pos.ref_lon = hil_state.lon / 1e7d;
hil_local_pos.ref_alt = alt0; hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed; hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
@@ -576,13 +576,13 @@ MulticopterPositionControl::task_main()
was_armed = _control_mode.flag_armed; was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled || if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled || _control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) { _control_mode.flag_control_velocity_enabled) {
update_ref();
_pos(0) = _local_pos.x; _pos(0) = _local_pos.x;
_pos(1) = _local_pos.y; _pos(1) = _local_pos.y;
_pos(2) = _local_pos.z; _pos(2) = _local_pos.z;
@@ -556,8 +556,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
double lon = gps.lon * 1e-7; double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3; float alt = gps.alt * 1e-3;
local_pos.ref_lat = gps.lat; local_pos.ref_lat = lat;
local_pos.ref_lon = gps.lon; local_pos.ref_lon = lon;
local_pos.ref_alt = alt + z_est[0]; local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t; local_pos.ref_timestamp = t;
+2 -2
View File
@@ -1085,8 +1085,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz; log_msg.body.log_LPOS.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
@@ -73,8 +73,8 @@ struct vehicle_local_position_s
bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */ uint64_t ref_timestamp; /**< Time when reference position was set */
int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ double ref_lat; /**< Reference point latitude in degrees */
int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ double ref_lon; /**< Reference point longitude in degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
bool landed; /**< true if vehicle is landed */ bool landed; /**< true if vehicle is landed */
/* Distance to surface */ /* Distance to surface */