Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: handle DO_CHANGE_SPEED as COMMAND_INT

Browse Source
This commit is contained in:
Peter Barker 2023-08-30 07:41:10 +10:00 committed by Andrew Tridgell
parent 2d4b952f47
commit 445f1fa272
4 changed files with 32 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/GCS_Mavlink.cpp
View File

@ -981,16 +981,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
return MAV_RESULT_ACCEPTED;
#endif
case MAV_CMD_DO_CHANGE_SPEED:
return handle_command_DO_CHANGE_SPEED(packet);
default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}
}
MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
{
switch(packet.command) {
case MAV_CMD_DO_CHANGE_SPEED: {
// if we're in failsafe modes (e.g., RTL, LOITER) or in pilot
// controlled modes (e.g., MANUAL, TRAINING)
// this command should be ignored since it comes in from GCS
@ -1001,15 +1001,15 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
return MAV_RESULT_FAILED;
}
AP_Mission::Mission_Command cmd;
if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) != MAV_MISSION_ACCEPTED) {
return MAV_RESULT_DENIED;
}
if (plane.do_change_speed(cmd)) {
if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) {
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
}
MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
switch(packet.command) {
case MAV_CMD_NAV_LOITER_UNLIM:
plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND);

2
ArduPlane/GCS_Mavlink.h
View File

@ -54,7 +54,7 @@ private:
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet);
bool try_send_message(enum ap_message id) override;
void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;

1
ArduPlane/Plane.h
View File

@ -945,6 +945,7 @@ private:
bool is_land_command(uint16_t cmd) const;
bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct);
/*
return true if in a specific AUTO mission command
*/

32
ArduPlane/commands_logic.cpp
View File

@ -946,28 +946,36 @@ void Plane::do_loiter_at_location()
bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.content.speed.speed_type)
{
return do_change_speed(
(uint8_t)cmd.content.speed.speed_type,
cmd.content.speed.target_ms,
cmd.content.speed.throttle_pct
);
}
bool Plane::do_change_speed(uint8_t speedtype, float speed_target_ms, float throttle_pct)
{
switch (speedtype) {
case 0: // Airspeed
if (is_equal(cmd.content.speed.target_ms, -2.0f)) {
if (is_equal(speed_target_ms, -2.0f)) {
new_airspeed_cm = -1; // return to default airspeed
return true;
} else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) &&
(cmd.content.speed.target_ms <= aparm.airspeed_max.get())) {
new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes
gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms);
} else if ((speed_target_ms >= aparm.airspeed_min.get()) &&
(speed_target_ms <= aparm.airspeed_max.get())) {
new_airspeed_cm = speed_target_ms * 100; //new airspeed target for AUTO or GUIDED modes
gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)speed_target_ms);
return true;
}
break;
case 1: // Ground speed
gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms);
aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100);
gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms);
aparm.min_gndspeed_cm.set(speed_target_ms * 100);
return true;
}
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct);
aparm.throttle_cruise.set(cmd.content.speed.throttle_pct);
if (throttle_pct > 0 && throttle_pct <= 100) {
gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)throttle_pct);
aparm.throttle_cruise.set(throttle_pct);
return true;
}