Browse Source

AP_Mount: params always use set method

apm_2208
Iampete1 3 years ago committed by Peter Hall
parent
commit
47105f0b03
  1. 6
      libraries/AP_Mount/AP_Mount.cpp
  2. 6
      libraries/AP_Mount/AP_Mount_Backend.cpp

6
libraries/AP_Mount/AP_Mount.cpp

@ -621,9 +621,9 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_lon @@ -621,9 +621,9 @@ MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_lon
return MAV_RESULT_FAILED;
}
_backends[_primary]->set_mode((MAV_MOUNT_MODE)packet.param1);
state[_primary]._stab_roll = packet.param2;
state[_primary]._stab_tilt = packet.param3;
state[_primary]._stab_pan = packet.param4;
state[_primary]._stab_roll.set(packet.param2);
state[_primary]._stab_tilt.set(packet.param3);
state[_primary]._stab_pan.set(packet.param4);
return MAV_RESULT_ACCEPTED;
}

6
libraries/AP_Mount/AP_Mount_Backend.cpp

@ -60,9 +60,9 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid) @@ -60,9 +60,9 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet)
{
set_mode((MAV_MOUNT_MODE)packet.mount_mode);
_state._stab_roll = packet.stab_roll;
_state._stab_tilt = packet.stab_pitch;
_state._stab_pan = packet.stab_yaw;
_state._stab_roll.set(packet.stab_roll);
_state._stab_tilt.set(packet.stab_pitch);
_state._stab_pan.set(packet.stab_yaw);
}
// process MOUNT_CONTROL messages received from GCS. deprecated.

Loading…
Cancel
Save