Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: Support for pointing mount to circle center

Browse Source
This commit is contained in:
Asif Khan 2023-05-07 13:11:48 +05:30 committed by Randy Mackay
parent 74b23adfac
commit 27d96eb64e
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/mode_circle.cpp
View File

@ -1,4 +1,5 @@
#include "Copter.h" #include "Copter.h"
#include <AP_Mount/AP_Mount.h>
#if MODE_CIRCLE_ENABLED == ENABLED #if MODE_CIRCLE_ENABLED == ENABLED
@ -20,6 +21,18 @@ bool ModeCircle::init(bool ignore_checks)
// initialise circle controller including setting the circle center based on vehicle speed // initialise circle controller including setting the circle center based on vehicle speed
copter.circle_nav->init(); copter.circle_nav->init();
#if HAL_MOUNT_ENABLED
AP_Mount *s = AP_Mount::get_singleton();
// Check if the CIRCLE_OPTIONS parameter have roi_at_center
if (copter.circle_nav->roi_at_center()) {
Vector3p loc = copter.circle_nav->get_center();
loc.z = 0;
Location circle_center(loc, Location::AltFrame::ABOVE_TERRAIN);
s->set_roi_target(circle_center);
}
#endif
// set auto yaw circle mode // set auto yaw circle mode
auto_yaw.set_mode(AutoYaw::Mode::CIRCLE); auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);

4
libraries/AC_WPNav/AC_Circle.h
View File

@ -89,6 +89,9 @@ public:
/// true if pilot control of radius and turn rate is enabled /// true if pilot control of radius and turn rate is enabled
bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; } bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; }
/// true if mount roi is at circle center
bool roi_at_center() const { return (_options.get() & CircleOptions::ROI_AT_CENTER) != 0; }
/// provide rangefinder based terrain offset /// provide rangefinder based terrain offset
/// terrain offset is the terrain's height above the EKF origin /// terrain offset is the terrain's height above the EKF origin
void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;}
@ -136,6 +139,7 @@ private:
MANUAL_CONTROL = 1U << 0, MANUAL_CONTROL = 1U << 0,
FACE_DIRECTION_OF_TRAVEL = 1U << 1, FACE_DIRECTION_OF_TRAVEL = 1U << 1,
INIT_AT_CENTER = 1U << 2, // true then the circle center will be the current location, false and the center will be 1 radius ahead INIT_AT_CENTER = 1U << 2, // true then the circle center will be the current location, false and the center will be 1 radius ahead
ROI_AT_CENTER = 1U << 3, // true when the mount roi is at circle center
}; };
// parameters // parameters