diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 60c0818e6339d..d07e7591fa163 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -591,6 +591,11 @@ class Copter : public AP_Vehicle { bool standby_active; +#if HAL_MOUNT_ENABLED + // true if the last time we checked the flightmode was landing: + bool was_landing; +#endif + static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5d8fc5a9e40bb..71e5c16f944af 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -428,6 +428,16 @@ void Copter::update_flight_mode() attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation flightmode->run(); + +#if HAL_MOUNT_ENABLED + { + const bool is_landing = flightmode->is_landing(); + if (is_landing && !was_landing) { + camera_mount.vehicle_has_started_to_land(); + } + was_landing = is_landing; + } +#endif } // exit_mode - high level call to organise cleanup as a flight mode is exited diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 97792527db53d..ab17687cdc68a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11512,6 +11512,38 @@ def PILOT_THR_BHV(self): self.wait_disarmed() + def MountRetractOnLanding(self): + '''test mount retracts on landing when asked to''' + self.context_push() + self.setup_servo_mount() + self.set_parameter("MNT1_OPTIONS", 2) + self.reboot_sitl() # to handle MNT_TYPE changing + + self.takeoff(10, mode='GUIDED') + + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + + angle = -10 + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=angle, # pitch angle in degrees + p2=0, # yaw angle in degrees + p3=0, # pitch rate in degrees (NaN to ignore) + p4=0, # yaw rate in degrees (NaN to ignore) + p5=0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame) + p6=0, # unused + p7=0, # gimbal id + ) + self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + + self.change_mode('LAND') + self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) + + self.wait_disarmed() + + self.context_pop() + self.reboot_sitl() + def CameraLogMessages(self): '''ensure Camera log messages are good''' self.set_parameter("RC12_OPTION", 9) # CameraTrigger @@ -12231,6 +12263,7 @@ def tests2b(self): # this block currently around 9.5mins here self.ScriptMountPOI, self.ScriptCopterPosOffsets, self.MountSolo, + self.MountRetractOnLanding, self.FlyMissionTwice, self.FlyMissionTwiceWithReset, self.MissionIndexValidity, diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index fe7f29f7eac3c..fbed50b693269 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -743,6 +743,19 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_ backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } +// called by a vehicle to indicate the vehicle has moved into a +// landing phase. For example, a Copter moving into LAND mode +// should call this method. +void AP_Mount::vehicle_has_started_to_land() +{ + // each instance writes log + for (uint8_t instance=0; instancevehicle_has_started_to_land(); + } + } +} + #if HAL_LOGGING_ENABLED // write mount log packet for all backends void AP_Mount::write_log() diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307f..9ab18d566e6bd 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -290,6 +290,11 @@ class AP_Mount // enable/disable rangefinder. Returns true on success bool set_rangefinder_enable(uint8_t instance, bool enable); + // called by a vehicle to indicate the vehicle has moved into a + // landing phase. For example, a Copter moving into LAND mode + // should call this method. + void vehicle_has_started_to_land(); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9d064633b1916..924e0ad7de766 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -930,4 +930,14 @@ void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str) _last_warning_ms = now_ms; } +// called by a vehicle to indicate the vehicle has moved into a +// landing phase. For example, a Copter moving into LAND mode +// should call this method. +void AP_Mount_Backend::vehicle_has_started_to_land() +{ + if (option_set(Options::RETRACT_ON_LANDING)) { + UNUSED_RESULT(set_mode(MAV_MOUNT_MODE_RETRACT)); + } +} + #endif // HAL_MOUNT_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f450b..6bfa1b5ec98a6 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -221,6 +221,11 @@ class AP_Mount_Backend // enable/disable rangefinder. Returns true on success virtual bool set_rangefinder_enable(bool enable) { return false; } + // called by a vehicle to indicate the vehicle has moved into a + // landing phase. For example, a Copter moving into LAND mode + // should call this method. + void vehicle_has_started_to_land(); + protected: enum class MountTargetType { @@ -249,6 +254,7 @@ class AP_Mount_Backend // options parameter bitmask handling enum class Options : uint8_t { RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode + RETRACT_ON_LANDING = (1U << 1), // Retract the gimbal when vehicle enters landing mode }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b41c4..75316e8a1bee4 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -168,7 +168,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @Param: _OPTIONS // @DisplayName: Mount options // @Description: Mount options bitmask - // @Bitmask: 0:RC lock state from previous mode + // @Bitmask: 0:RC lock state from previous mode, 1:Retract mount on land start // @User: Standard AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0),