Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: fix do-change-speed received during takeoff

Browse Source
This commit is contained in:
Randy Mackay 2023-11-28 20:36:08 +09:00
parent f1b404f889
commit 97ab37875e
2 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/mode.h
View File

@ -714,6 +714,13 @@ private:
float climb_rate; // climb rate in m/s. provided by mission command
uint32_t start_ms; // system time that nav attitude time command was received (used for timeout)
} nav_attitude_time;
// desired speeds
struct {
float xy; // desired speed horizontally in m/s. 0 if unset
float up; // desired speed upwards in m/s. 0 if unset
float down; // desired speed downwards in m/s. 0 if unset
} desired_speed_override;
};
#if AUTOTUNE_ENABLED == ENABLED

20
ArduCopter/mode_auto.cpp
View File

@ -41,6 +41,9 @@ bool ModeAuto::init(bool ignore_checks)
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// initialise desired speed overrides
desired_speed_override = {0, 0, 0};
// set flag to start mission
waiting_to_start = true;
@ -370,7 +373,16 @@ bool ModeAuto::wp_start(const Location& dest_loc)
stopping_point = takeoff_complete_pos.tofloat();
}
}
wp_nav->wp_and_spline_init(0, stopping_point);
float des_speed_xy_cm = is_positive(desired_speed_override.xy) ? (desired_speed_override.xy * 100) : 0;
wp_nav->wp_and_spline_init(des_speed_xy_cm, stopping_point);
// override speeds up and down if necessary
if (is_positive(desired_speed_override.up)) {
wp_nav->set_speed_up(desired_speed_override.up * 100.0);
}
if (is_positive(desired_speed_override.down)) {
wp_nav->set_speed_down(desired_speed_override.down * 100.0);
}
}
if (!wp_nav->set_wp_destination_loc(dest_loc)) {
@ -585,18 +597,21 @@ bool ModeAuto::use_pilot_yaw(void) const
bool ModeAuto::set_speed_xy(float speed_xy_cms)
{
copter.wp_nav->set_speed_xy(speed_xy_cms);
desired_speed_override.xy = speed_xy_cms * 0.01;
return true;
}
bool ModeAuto::set_speed_up(float speed_up_cms)
{
copter.wp_nav->set_speed_up(speed_up_cms);
desired_speed_override.up = speed_up_cms * 0.01;
return true;
}
bool ModeAuto::set_speed_down(float speed_down_cms)
{
copter.wp_nav->set_speed_down(speed_down_cms);
desired_speed_override.down = speed_down_cms * 0.01;
return true;
}
@ -1855,10 +1870,13 @@ void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
if (cmd.content.speed.target_ms > 0) {
if (cmd.content.speed.speed_type == 2) {
copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.up = cmd.content.speed.target_ms;
} else if (cmd.content.speed.speed_type == 3) {
copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.down = cmd.content.speed.target_ms;
} else {
copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
desired_speed_override.xy = cmd.content.speed.target_ms;
}
}
}