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

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

2
ArduPlane/GCS_Mavlink.h
View File

@ -54,7 +54,7 @@ private:
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; 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_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_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; bool try_send_message(enum ap_message id) override;
void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) 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 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 return true if in a specific AUTO mission command
*/ */

30
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) 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 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 new_airspeed_cm = -1; // return to default airspeed
return true; return true;
} else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && } else if ((speed_target_ms >= aparm.airspeed_min.get()) &&
(cmd.content.speed.target_ms <= aparm.airspeed_max.get())) { (speed_target_ms <= aparm.airspeed_max.get())) {
new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes 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)cmd.content.speed.target_ms); gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)speed_target_ms);
return true; return true;
} }
break; break;
case 1: // Ground speed case 1: // Ground speed
gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms);
aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); aparm.min_gndspeed_cm.set(speed_target_ms * 100);
return true; return true;
} }
if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) { if (throttle_pct > 0 && throttle_pct <= 100) {
gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)cmd.content.speed.throttle_pct); gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)throttle_pct);
aparm.throttle_cruise.set(cmd.content.speed.throttle_pct); aparm.throttle_cruise.set(throttle_pct);
return true; return true;
} }