From d76e7d210bf551b148e739d5dc06afc703b8486d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Jul 2018 13:08:22 +1000 Subject: [PATCH] Plane: handle command_long in GCS base class --- ArduPlane/GCS_Mavlink.cpp | 539 ++++++++++++++++++-------------------- ArduPlane/GCS_Mavlink.h | 1 + 2 files changed, 249 insertions(+), 291 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f8fe9ff078..2a0ab53497 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -758,6 +758,254 @@ bool GCS_MAVLINK_Plane::should_disable_overrides_on_reboot() const return (plane.quadplane.enable != 0); } + +MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_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 + // or a companion computer: + if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) { + // failed + return MAV_RESULT_FAILED; + } + + AP_Mission::Mission_Command cmd; + if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) == MAV_MISSION_ACCEPTED) { + plane.do_change_speed(cmd); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + case MAV_CMD_NAV_LOITER_UNLIM: + plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + plane.set_mode(RTL, MODE_REASON_GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_NAV_TAKEOFF: { + // user takeoff only works with quadplane code for now + // param7 : altitude [metres] + float takeoff_alt = packet.param7; + if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + } + +#if MOUNT == ENABLED + // Sets the region of interest (ROI) for the camera + case MAV_CMD_DO_SET_ROI: + // sanity check location + if (!check_latlng(packet.param5, packet.param6)) { + return MAV_RESULT_FAILED; + } + Location roi_loc; + roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); + roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); + roi_loc.alt = (int32_t)(packet.param7 * 100.0f); + if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { + // switch off the camera tracking if enabled + if (plane.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + plane.camera_mount.set_mode_to_default(); + } + } else { + // send the command to the camera mount + plane.camera_mount.set_roi_target(roi_loc); + } + return MAV_RESULT_ACCEPTED; +#endif + +#if MOUNT == ENABLED + case MAV_CMD_DO_MOUNT_CONTROL: + plane.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); + return MAV_RESULT_ACCEPTED; +#endif + + case MAV_CMD_MISSION_START: + plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND); + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_COMPONENT_ARM_DISARM: + if (is_equal(packet.param1,1.0f)) { + // run pre_arm_checks and arm_checks and display failures + const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value); + if (plane.arm_motors(AP_Arming::MAVLINK, do_arming_checks)) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + } else if (is_zero(packet.param1)) { + if (plane.disarm_motors()) { + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + } + return MAV_RESULT_UNSUPPORTED; + + case MAV_CMD_DO_LAND_START: + // attempt to switch to next DO_LAND_START command in the mission + if (plane.mission.jump_to_landing_sequence()) { + plane.set_mode(AUTO, MODE_REASON_UNKNOWN); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; + + case MAV_CMD_DO_GO_AROUND: + if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + // Initiate an aborted landing. This will trigger a pitch-up and + // climb-out to a safe altitude holding heading then one of the + // following actions will occur, check for in this order: + // - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission, + // increment mission index to execute it + // - else if DO_LAND_START is available, jump to it + // - else decrement the mission index to repeat the landing approach + + if (!is_zero(packet.param1)) { + plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100; + } + if (plane.landing.request_go_around()) { + plane.auto_state.next_wp_crosstrack = false; + return MAV_RESULT_ACCEPTED; + } + } + return MAV_RESULT_FAILED; + + case MAV_CMD_DO_FENCE_ENABLE: + if (!plane.geofence_present()) { + gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured"); + return MAV_RESULT_FAILED; + } + switch((uint16_t)packet.param1) { + case 0: + if (! plane.geofence_set_enabled(false, GCS_TOGGLED)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + case 1: + if (! plane.geofence_set_enabled(true, GCS_TOGGLED)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + case 2: //disable fence floor only + if (! plane.geofence_set_floor_enabled(false)) { + return MAV_RESULT_FAILED; + } + gcs().send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); + return MAV_RESULT_ACCEPTED; + default: + break; + } + return MAV_RESULT_FAILED; + + case MAV_CMD_DO_SET_HOME: { + // param1 : use current (1=use current location, 0=use specified location) + // param5 : latitude + // param6 : longitude + // param7 : altitude (absolute) + if (is_equal(packet.param1,1.0f)) { + plane.set_home_persistently(AP::gps().location()); + AP::ahrs().lock_home(); + return MAV_RESULT_ACCEPTED; + } else { + // ensure param1 is zero + if (!is_zero(packet.param1)) { + return MAV_RESULT_FAILED; + } + if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { + // don't allow the 0,0 position + return MAV_RESULT_FAILED; + } + // sanity check location + if (!check_latlng(packet.param5,packet.param6)) { + return MAV_RESULT_FAILED; + } + Location new_home_loc {}; + new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); + new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); + new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); + plane.set_home(new_home_loc); + AP::ahrs().lock_home(); + return MAV_RESULT_ACCEPTED; + } + break; + } + + case MAV_CMD_DO_AUTOTUNE_ENABLE: + // param1 : enable/disable + plane.autotune_enable(!is_zero(packet.param1)); + return MAV_RESULT_ACCEPTED; + +#if PARACHUTE == ENABLED + case MAV_CMD_DO_PARACHUTE: + // configure or release parachute + switch ((uint16_t)packet.param1) { + case PARACHUTE_DISABLE: + plane.parachute.enabled(false); + return MAV_RESULT_ACCEPTED; + case PARACHUTE_ENABLE: + plane.parachute.enabled(true); + return MAV_RESULT_ACCEPTED; + case PARACHUTE_RELEASE: + // treat as a manual release which performs some additional check of altitude + if (plane.parachute.released()) { + gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released"); + return MAV_RESULT_FAILED; + } + if (!plane.parachute.enabled()) { + gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled"); + return MAV_RESULT_FAILED; + } + if (!plane.parachute_manual_release()) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + default: + break; + } + return MAV_RESULT_FAILED; +#endif + + case MAV_CMD_DO_MOTOR_TEST: + // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) + // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) + // param3 : throttle (range depends upon param2) + // param4 : timeout (in seconds) + // param5 : motor count (number of motors to test in sequence) + return plane.quadplane.mavlink_motor_test_start(chan, + (uint8_t)packet.param1, + (uint8_t)packet.param2, + (uint16_t)packet.param3, + packet.param4, + (uint8_t)packet.param5); + + case MAV_CMD_DO_VTOL_TRANSITION: + if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_DO_ENGINE_CONTROL: + if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + case MAV_CMD_ACCELCAL_VEHICLE_POS: + if (!plane.ins.get_acal()->gcs_vehicle_position(packet.param1)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; + + default: + return GCS_MAVLINK::handle_command_long_packet(packet); + } +} + void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { @@ -889,297 +1137,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_COMMAND_LONG: - { - // decode - mavlink_command_long_t packet; - mavlink_msg_command_long_decode(msg, &packet); - - MAV_RESULT result = MAV_RESULT_UNSUPPORTED; - - // do command - - 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 - // or a companion computer: - result = MAV_RESULT_FAILED; - if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) { - // failed - break; - } - - AP_Mission::Mission_Command cmd; - if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) - == MAV_MISSION_ACCEPTED) { - plane.do_change_speed(cmd); - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(RTL, MODE_REASON_GCS_COMMAND); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_NAV_TAKEOFF: { - // user takeoff only works with quadplane code for now - // param7 : altitude [metres] - float takeoff_alt = packet.param7; - if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - break; - } - -#if MOUNT == ENABLED - // Sets the region of interest (ROI) for the camera - case MAV_CMD_DO_SET_ROI: - // sanity check location - if (!check_latlng(packet.param5, packet.param6)) { - break; - } - Location roi_loc; - roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); - roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); - roi_loc.alt = (int32_t)(packet.param7 * 100.0f); - if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { - // switch off the camera tracking if enabled - if (plane.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - plane.camera_mount.set_mode_to_default(); - } - } else { - // send the command to the camera mount - plane.camera_mount.set_roi_target(roi_loc); - } - result = MAV_RESULT_ACCEPTED; - break; -#endif - - case MAV_CMD_DO_MOUNT_CONTROL: -#if MOUNT == ENABLED - plane.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); - result = MAV_RESULT_ACCEPTED; -#endif - break; - - case MAV_CMD_MISSION_START: - plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND); - result = MAV_RESULT_ACCEPTED; - break; - - case MAV_CMD_COMPONENT_ARM_DISARM: - if (is_equal(packet.param1,1.0f)) { - // run pre_arm_checks and arm_checks and display failures - const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value); - if (plane.arm_motors(AP_Arming::MAVLINK, do_arming_checks)) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } else if (is_zero(packet.param1)) { - if (plane.disarm_motors()) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - } else { - result = MAV_RESULT_UNSUPPORTED; - } - break; - - case MAV_CMD_DO_LAND_START: - result = MAV_RESULT_FAILED; - - // attempt to switch to next DO_LAND_START command in the mission - if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(AUTO, MODE_REASON_UNKNOWN); - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_GO_AROUND: - result = MAV_RESULT_FAILED; - - if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { - // Initiate an aborted landing. This will trigger a pitch-up and - // climb-out to a safe altitude holding heading then one of the - // following actions will occur, check for in this order: - // - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission, - // increment mission index to execute it - // - else if DO_LAND_START is available, jump to it - // - else decrement the mission index to repeat the landing approach - - if (!is_zero(packet.param1)) { - plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100; - } - if (plane.landing.request_go_around()) { - plane.auto_state.next_wp_crosstrack = false; - result = MAV_RESULT_ACCEPTED; - } - } - break; - - case MAV_CMD_DO_FENCE_ENABLE: - result = MAV_RESULT_ACCEPTED; - - if (!plane.geofence_present()) { - gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured"); - result = MAV_RESULT_FAILED; - } else { - switch((uint16_t)packet.param1) { - case 0: - if (! plane.geofence_set_enabled(false, GCS_TOGGLED)) { - result = MAV_RESULT_FAILED; - } - break; - case 1: - if (! plane.geofence_set_enabled(true, GCS_TOGGLED)) { - result = MAV_RESULT_FAILED; - } - break; - case 2: //disable fence floor only - if (! plane.geofence_set_floor_enabled(false)) { - result = MAV_RESULT_FAILED; - } else { - gcs().send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); - } - break; - default: - result = MAV_RESULT_FAILED; - break; - } - } - break; - - case MAV_CMD_DO_SET_HOME: { - // param1 : use current (1=use current location, 0=use specified location) - // param5 : latitude - // param6 : longitude - // param7 : altitude (absolute) - result = MAV_RESULT_FAILED; // assume failure - if (is_equal(packet.param1,1.0f)) { - plane.set_home_persistently(AP::gps().location()); - AP::ahrs().lock_home(); - result = MAV_RESULT_ACCEPTED; - } else { - // ensure param1 is zero - if (!is_zero(packet.param1)) { - break; - } - if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { - // don't allow the 0,0 position - break; - } - // sanity check location - if (!check_latlng(packet.param5,packet.param6)) { - break; - } - Location new_home_loc {}; - new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); - new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); - new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); - plane.set_home(new_home_loc); - AP::ahrs().lock_home(); - result = MAV_RESULT_ACCEPTED; - } - break; - } - - case MAV_CMD_DO_AUTOTUNE_ENABLE: - // param1 : enable/disable - plane.autotune_enable(!is_zero(packet.param1)); - break; - -#if PARACHUTE == ENABLED - case MAV_CMD_DO_PARACHUTE: - // configure or release parachute - result = MAV_RESULT_ACCEPTED; - switch ((uint16_t)packet.param1) { - case PARACHUTE_DISABLE: - plane.parachute.enabled(false); - break; - case PARACHUTE_ENABLE: - plane.parachute.enabled(true); - break; - case PARACHUTE_RELEASE: - // treat as a manual release which performs some additional check of altitude - if (plane.parachute.released()) { - gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released"); - result = MAV_RESULT_FAILED; - } else if (!plane.parachute.enabled()) { - gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled"); - result = MAV_RESULT_FAILED; - } else { - if (!plane.parachute_manual_release()) { - result = MAV_RESULT_FAILED; - } - } - break; - default: - result = MAV_RESULT_FAILED; - break; - } - break; -#endif - - case MAV_CMD_DO_MOTOR_TEST: - // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) - // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) - // param3 : throttle (range depends upon param2) - // param4 : timeout (in seconds) - // param5 : motor count (number of motors to test in sequence) - result = plane.quadplane.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4, (uint8_t)packet.param5); - break; - - case MAV_CMD_DO_VTOL_TRANSITION: - if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { - result = MAV_RESULT_FAILED; - } else { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_ENGINE_CONTROL: - if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { - result = MAV_RESULT_FAILED; - } else { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_ACCELCAL_VEHICLE_POS: - result = MAV_RESULT_FAILED; - - if (plane.ins.get_acal()->gcs_vehicle_position(packet.param1)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - default: - result = handle_command_long_message(packet); - break; - } - - mavlink_msg_command_ack_send_buf( - msg, - chan, - packet.command, - result); - - break; - } - #if GEOFENCE_ENABLED == ENABLED // receive a fence point from GCS and store in EEPROM case MAVLINK_MSG_ID_FENCE_POINT: { diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index b004e8d79d..f089ffd3c8 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -31,6 +31,7 @@ protected: MAV_RESULT handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; + MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; void send_position_target_global_int() override;