mirror of https://github.com/ArduPilot/ardupilot
71 lines
2.8 KiB
C
71 lines
2.8 KiB
C
|
#pragma once
|
||
|
|
|
||
|
|
#include <AP_Common/AP_Common.h>
|
||
|
|
#include <APM_Control/AR_AttitudeControl.h>
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Pivot Turn controller for skid-steering rovers and boats
|
||
|
|
*
|
||
|
|
* How-to-Use:
|
||
|
|
* 1. call "enable(true)" once for skid-steering vehicles to enable this controller
|
||
|
|
* 2. before vehicle starts towards a waypoint call "check_activation" and provide the earth-frame heading to the waypoint
|
||
|
|
* this may change the controller's internal state to "active"
|
||
|
|
* 3. on each main loop iteration call "active()" to see if this controller thinks it is controllering the vehicle
|
||
|
|
* 4. call "get_turn_rate_rads()" to retrieve the desired turn rate towards the next waypoint
|
||
|
|
* 5. pass above turn rate into the rate controller, set speed controller's speed to zero
|
||
|
|
* 6. this controller's "active" state will change to false once it has completed the pivot turn
|
||
|
|
*/
|
||
|
|
|
||
|
|
class AR_PivotTurn {
|
||
|
|
public:
|
||
|
|
|
||
|
|
// constructor
|
||
|
|
AR_PivotTurn(AR_AttitudeControl& atc);
|
||
|
|
|
||
|
|
// enable or disable pivot controller
|
||
|
|
void enable(bool enable_pivot);
|
||
|
|
|
||
|
|
// true if this controller is controlling vehicle
|
||
|
|
bool active() const;
|
||
|
|
|
||
|
|
// checks if pivot turns should be activated or deactivated
|
||
|
|
// force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error
|
||
|
|
void check_activation(float desired_heading_deg, bool force_active = false);
|
||
|
|
|
||
|
|
// check if pivot turn would be activated given an expected change in yaw in degrees
|
||
|
|
// note this does not actually active the pivot turn. To activate use the check_activation method
|
||
|
|
bool would_activate(float yaw_change_deg) const WARN_IF_UNUSED;
|
||
|
|
|
||
|
|
// forcibly deactivate this controller
|
||
|
|
void deactivate() { _active = false; };
|
||
|
|
|
||
|
|
// get turn rate (in rad/sec)
|
||
|
|
// desired heading should be the heading towards the next waypoint in degrees
|
||
|
|
// dt should be the time since the last call in seconds
|
||
|
|
float get_turn_rate_rads(float desired_heading_deg, float dt);
|
||
|
|
|
||
|
|
// accessors for parameter values
|
||
|
|
float get_rate_max() const { return _rate_max; }
|
||
|
|
|
||
|
|
// parameter var table
|
||
|
|
static const struct AP_Param::GroupInfo var_info[];
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
// return post-turn delay duration in milliseconds
|
||
|
|
uint32_t get_delay_duration_ms() const;
|
||
|
|
|
||
|
|
// parameters
|
||
|
|
AP_Int16 _angle; // minimum angle error (in degrees) that leads to pivot turn
|
||
|
|
AP_Int16 _rate_max; // maximum turn rate (in degrees) during pivot turn
|
||
|
|
AP_Float _delay; // waiting time (in seconds) after pivot turn completes
|
||
|
|
|
||
|
|
// references
|
||
|
|
AR_AttitudeControl& _atc; // rover attitude control library
|
||
|
|
|
||
|
|
// local variables
|
||
|
|
bool _enabled; // true if vehicle can pivot
|
||
|
|
bool _active; // true if vehicle is currently pivoting
|
||
|
|
uint32_t _delay_start_ms; // system time when post-turn delay started
|
||
|
|
};
|