mirror of https://github.com/ArduPilot/ardupilot
Copter: rename roi_WP to just roi
A Vector3f is not a waypoint. This should really be "Point Of Interest" rather than "Region Of Interest"
This commit is contained in:
parent
ba8b3e2415
commit
948b90ed97
|
@ -33,13 +33,13 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
|
|||
* yaw controllers
|
||||
*************************************************************/
|
||||
|
||||
// roi_yaw - returns heading towards location held in roi_WP
|
||||
// roi_yaw - returns heading towards location held in roi
|
||||
float Copter::Mode::AutoYaw::roi_yaw()
|
||||
{
|
||||
roi_yaw_counter++;
|
||||
if (roi_yaw_counter >= 4) {
|
||||
roi_yaw_counter = 0;
|
||||
_roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi_WP);
|
||||
_roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi);
|
||||
}
|
||||
|
||||
return _roi_yaw;
|
||||
|
|
|
@ -14,7 +14,7 @@
|
|||
enum autopilot_yaw_mode {
|
||||
AUTO_YAW_HOLD = 0, // pilot controls the heading
|
||||
AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
|
||||
AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted)
|
||||
AUTO_YAW_ROI = 2, // point towards a location held in roi (no pilot input accepted)
|
||||
AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted)
|
||||
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
|
||||
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
|
||||
|
|
|
@ -48,7 +48,7 @@ public:
|
|||
uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
|
||||
|
||||
// Yaw will point at this location if mode is set to AUTO_YAW_ROI
|
||||
Vector3f roi_WP;
|
||||
Vector3f roi;
|
||||
|
||||
// bearing from current location to the ROI
|
||||
float _roi_yaw;
|
||||
|
|
|
@ -1989,7 +1989,7 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
|
|||
#if MOUNT == ENABLED
|
||||
// check if mount type requires us to rotate the quad
|
||||
if(!copter.camera_mount.has_pan_control()) {
|
||||
roi_WP = copter.pv_location_to_vector(roi_location);
|
||||
roi = copter.pv_location_to_vector(roi_location);
|
||||
auto_yaw.set_mode(AUTO_YAW_ROI);
|
||||
}
|
||||
// send the command to the camera mount
|
||||
|
@ -2003,7 +2003,7 @@ void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
|
|||
// 4: point at a target given a target id (can't be implemented)
|
||||
#else
|
||||
// if we have no camera mount aim the quad at the location
|
||||
roi_WP = pv_location_to_vector(roi_location);
|
||||
roi = pv_location_to_vector(roi_location);
|
||||
auto_yaw.set_mode(AUTO_YAW_ROI);
|
||||
#endif // MOUNT == ENABLED
|
||||
}
|
||||
|
@ -2022,7 +2022,7 @@ float Copter::Mode::AutoYaw::yaw()
|
|||
switch(_mode) {
|
||||
|
||||
case AUTO_YAW_ROI:
|
||||
// point towards a location held in roi_WP
|
||||
// point towards a location held in roi
|
||||
return roi_yaw();
|
||||
|
||||
case AUTO_YAW_FIXED:
|
||||
|
|
Loading…
Reference in New Issue