Browse Source

Support relative altitude for offboard global position setpoints

Previously the coordinate_frame flag for setpoint_target_global_int message was not handled, resulting in the vehicle to not handle altitude correctly. This commit handles the relative altitude correctly.
release/1.12
Jaeyoung-Lim 4 years ago committed by Lorenz Meier
parent
commit
b3257c0bf2
  1. 11
      src/modules/mavlink/mavlink_receiver.cpp

11
src/modules/mavlink/mavlink_receiver.cpp

@ -1085,8 +1085,17 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t @@ -1085,8 +1085,17 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
pos_sp_triplet.current.position_valid = false;
} else {
float target_altitude;
if (set_position_target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
target_altitude = set_position_target_global_int.alt + local_pos.ref_alt;
} else {
target_altitude = set_position_target_global_int.alt; //MAV_FRAME_GLOBAL_INT
}
globallocalconverter_tolocal(set_position_target_global_int.lat_int / 1e7,
set_position_target_global_int.lon_int / 1e7, set_position_target_global_int.alt,
set_position_target_global_int.lon_int / 1e7, target_altitude,
&pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z);
pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed();
pos_sp_triplet.current.position_valid = true;

Loading…
Cancel
Save