From 37118920ed01858f8fd0d0f09aabdd87ff22284d Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Mon, 2 May 2016 20:23:08 -0700 Subject: [PATCH] Sub: Add upstream Copter changes. --- ArduSub/ArduSub.cpp | 15 ++------------- ArduSub/GCS_Mavlink.cpp | 1 + ArduSub/config.h | 4 ---- 3 files changed, 3 insertions(+), 17 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 7df9dbe1d8..98f5a07ab0 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -79,19 +79,8 @@ /* scheduler table for fast CPUs - all regular tasks apart from the fast_loop() - should be listed here, along with how often they should be called - (in 2.5ms units) and the maximum time they are expected to take (in - microseconds) - 1 = 400hz - 2 = 200hz - 4 = 100hz - 8 = 50hz - 20 = 20hz - 40 = 10hz - 133 = 3hz - 400 = 1hz - 4000 = 0.1hz - + should be listed here, along with how often they should be called (in hz) + and the maximum time they are expected to take (in microseconds) */ const AP_Scheduler::Task Sub::scheduler_tasks[] = { SCHED_TASK(rc_loop, 100, 130), diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 9eb74c38b8..079729ab84 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1353,6 +1353,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED sub.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); + result = MAV_RESULT_ACCEPTED; #endif break; diff --git a/ArduSub/config.h b/ArduSub/config.h index 0b885d0f3e..abeadfc984 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -171,10 +171,6 @@ #define FS_RADIO_TIMEOUT_MS 500 // RC Radio Failsafe triggers after 500 miliseconds with No RC Input #endif -#ifndef FS_CLOSE_TO_HOME_CM - # define FS_CLOSE_TO_HOME_CM 500 // if vehicle within 5m of home, vehicle will LAND instead of RTL during some failsafes -#endif - #ifndef PREARM_DISPLAY_PERIOD # define PREARM_DISPLAY_PERIOD 30 #endif