Browse Source

AP_Mount: correct status_msg to conform to MAVLink specification

master
Jonathan Challinger 10 years ago committed by Randy Mackay
parent
commit
7b0e806db1
  1. 2
      libraries/AP_Mount/AP_Mount_Alexmos.cpp
  2. 2
      libraries/AP_Mount/AP_Mount_SToRM32.cpp
  3. 2
      libraries/AP_Mount/AP_Mount_Servo.cpp

2
libraries/AP_Mount/AP_Mount_Alexmos.cpp

@ -77,7 +77,7 @@ void AP_Mount_Alexmos::set_mode(enum MAV_MOUNT_MODE mode) @@ -77,7 +77,7 @@ void AP_Mount_Alexmos::set_mode(enum MAV_MOUNT_MODE mode)
void AP_Mount_Alexmos::status_msg(mavlink_channel_t chan)
{
get_angles();
mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.x*100, _current_angle.y*100, _current_angle.z*100);
mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100);
}
/*

2
libraries/AP_Mount/AP_Mount_SToRM32.cpp

@ -110,7 +110,7 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode) @@ -110,7 +110,7 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
void AP_Mount_SToRM32::status_msg(mavlink_channel_t chan)
{
// return target angles as gimbal's actual attitude. To-Do: retrieve actual gimbal attitude and send these instead
mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.z)*100);
mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);
}
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message

2
libraries/AP_Mount/AP_Mount_Servo.cpp

@ -117,7 +117,7 @@ void AP_Mount_Servo::check_servo_map() @@ -117,7 +117,7 @@ void AP_Mount_Servo::check_servo_map()
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_Servo::status_msg(mavlink_channel_t chan)
{
mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.x*100, _angle_bf_output_deg.y*100, _angle_bf_output_deg.z*100);
mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100);
}
// stabilize - stabilizes the mount relative to the Earth's frame

Loading…
Cancel
Save