Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: support changing vertical speed in DO_CHANGE_SPEED

Browse Source
This commit is contained in:
chobitsfan 2018-10-09 19:46:37 +08:00 committed by Randy Mackay
parent 7fa7c8f403
commit d94663d5af
2 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/GCS_Mavlink.cpp
View File

@ -755,7 +755,13 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
// param3 : unused
// param4 : unused
if (packet.param2 > 0.0f) {
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
if (packet.param1 > 2.9f) { // 3 = speed down
copter.wp_nav->set_speed_z(packet.param2 * 100.0f, copter.wp_nav->get_speed_up());
} else if (packet.param1 > 1.9f) { // 2 = speed up
copter.wp_nav->set_speed_z(copter.wp_nav->get_speed_down(), packet.param2 * 100.0f);
} else {
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
}
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;

8
ArduCopter/mode_auto.cpp
View File

@ -1316,7 +1316,13 @@ void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
void Copter::ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (cmd.content.speed.target_ms > 0) {
copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
if (cmd.content.speed.speed_type == 2) {
copter.wp_nav->set_speed_z(copter.wp_nav->get_speed_down(), cmd.content.speed.target_ms * 100.0f);
} else if (cmd.content.speed.speed_type == 3) {
copter.wp_nav->set_speed_z(cmd.content.speed.target_ms * 100.0f, copter.wp_nav->get_speed_up());
} else {
copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
}
}
}