diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 95c51284d9..364f81d7c2 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -57,6 +57,18 @@ void AP_Mount_Alexmos::update() } break; + case MAV_MOUNT_MODE_HOME_LOCATION: + // constantly update the home location: + if (!AP::ahrs().home_is_set()) { + break; + } + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false)) { + control_axis(_angle_ef_target_rad, false); + } + break; + case MAV_MOUNT_MODE_SYSID_TARGET: if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 96fc49d21a..483abf6337 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -85,6 +85,13 @@ void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_ break; } + case MAV_MOUNT_MODE_HOME_LOCATION: { + // set the target gps location + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + break; + } + default: // do nothing break; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 4564173b37..319eedcb35 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -64,6 +64,18 @@ void AP_Mount_SToRM32::update() } break; + case MAV_MOUNT_MODE_HOME_LOCATION: + // constantly update the home location: + if (!AP::ahrs().home_is_set()) { + break; + } + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { + resend_now = true; + } + break; + case MAV_MOUNT_MODE_SYSID_TARGET: if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) { resend_now = true; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index d67530e1c8..a51b2a22a5 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -80,6 +80,18 @@ void AP_Mount_SToRM32_serial::update() } break; + case MAV_MOUNT_MODE_HOME_LOCATION: + // constantly update the home location: + if (!AP::ahrs().home_is_set()) { + break; + } + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { + resend_now = true; + } + break; + case MAV_MOUNT_MODE_SYSID_TARGET: if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index f34253aa97..cbee4c1f15 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -76,6 +76,18 @@ void AP_Mount_Servo::update() break; } + case MAV_MOUNT_MODE_HOME_LOCATION: + // constantly update the home location: + if (!AP::ahrs().home_is_set()) { + break; + } + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) { + stabilize(); + } + break; + case MAV_MOUNT_MODE_SYSID_TARGET: if (calc_angle_to_sysid_target(_angle_ef_target_rad, _flags.tilt_control, diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 1c8e6eb88f..f532f1e600 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -69,13 +69,22 @@ void AP_Mount_SoloGimbal::update() // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: _gimbal.set_lockedToBody(false); - if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { + UNUSED_RESULT(calc_angle_to_roi_target(_angle_ef_target_rad, true, true)); + break; + + case MAV_MOUNT_MODE_HOME_LOCATION: + // constantly update the home location: + if (!AP::ahrs().home_is_set()) { + break; } + _state._roi_target = AP::ahrs().get_home(); + _state._roi_target_set = true; + _gimbal.set_lockedToBody(false); + UNUSED_RESULT(calc_angle_to_roi_target(_angle_ef_target_rad, true, true)); break; case MAV_MOUNT_MODE_SYSID_TARGET: - if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) { - } + UNUSED_RESULT(calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)); break; default: