aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: handle MAV_CMD_NAV_TAKEOFF via command_long and command_in

Browse Source
This commit is contained in:
Peter Barker 2023-10-18 17:30:32 +11:00 committed by Peter Barker
parent 21eaa08333
commit ca3b5a860a
2 changed files with 23 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/GCS_Mavlink.cpp
View File

@ -770,6 +770,10 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
case MAV_CMD_DO_MOTOR_TEST:
return handle_MAV_CMD_DO_MOTOR_TEST(packet);
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_VTOL_TAKEOFF:
return handle_MAV_CMD_NAV_TAKEOFF(packet);
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
return handle_MAV_CMD_DO_PARACHUTE(packet);
@ -847,31 +851,37 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t
}
#endif
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet)
{
switch(packet.command) {
if (packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
return MAV_RESULT_DENIED; // meaning some parameters are bad
}
case MAV_CMD_NAV_VTOL_TAKEOFF:
case MAV_CMD_NAV_TAKEOFF: {
// param3 : horizontal navigation by pilot acceptable
// param4 : yaw angle (not supported)
// param5 : latitude (not supported)
// param6 : longitude (not supported)
// param7 : altitude [metres]
float takeoff_alt = packet.param7 * 100; // Convert m to cm
float takeoff_alt = packet.z * 100; // Convert m to cm
if (!copter.flightmode->do_user_takeoff(takeoff_alt, is_zero(packet.param3))) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
default:
return GCS_MAVLINK::handle_command_long_packet(packet, msg);
}
}
bool GCS_MAVLINK_Copter::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
{
if (packet_command == MAV_CMD_NAV_TAKEOFF ||
packet_command == MAV_CMD_NAV_VTOL_TAKEOFF) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
return true;
}
return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command);
}
MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet)
{
// param1 : target angle [0-360]

4
ArduCopter/GCS_Mavlink.h
View File

@ -38,7 +38,6 @@ protected:
MAV_RESULT handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
#endif
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t &packet);
@ -109,7 +108,10 @@ private:
MAV_RESULT handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet);
#endif
bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override;
MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet);
MAV_RESULT handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet);
#if AP_WINCH_ENABLED
MAV_RESULT handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet);