mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
a3c2851120
1) Explicit control of tracking loop period and damping which removes previous variation in period with speed and fixed damping ratio 2) Explicit control of track capture angle (now set to 45 degrees by default) 3) Removal of restriction on loiter radius being greater than L1 distance The circle(loiter) control is a L1 and PD hybrid utilising L1 for waypoint capture and PD control for circle tracking. Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au> Pair-Programmed-With: Andrew Tridgell <tridge@samba.org>
111 lines
3.0 KiB
C++
111 lines
3.0 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
/// @file AP_L1_Control.h
|
|
/// @brief L1 Control algorithm. This is a instance of an
|
|
/// AP_Navigation class
|
|
|
|
/*
|
|
* Originally written by Brandon Jones 2013
|
|
*
|
|
* Modified by Paul Riseborough 2013 to provide:
|
|
* - Explicit control over frequency and damping
|
|
* - Explicit control over track capture angle
|
|
* - Ability to use loiter radius smaller than L1 length
|
|
*/
|
|
|
|
#ifndef AP_L1_CONTROL_H
|
|
#define AP_L1_CONTROL_H
|
|
|
|
#include <AP_Math.h>
|
|
#include <AP_AHRS.h>
|
|
#include <AP_Param.h>
|
|
#include <AP_Navigation.h>
|
|
|
|
class AP_L1_Control : public AP_Navigation {
|
|
public:
|
|
AP_L1_Control(AP_AHRS *ahrs) :
|
|
_ahrs(ahrs)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
}
|
|
|
|
/* see AP_Navigation.h for the definitions and units of these
|
|
* functions */
|
|
int32_t nav_roll_cd(void);
|
|
|
|
// return the desired track heading angle(centi-degrees)
|
|
int32_t nav_bearing_cd(void);
|
|
|
|
// return the heading error angle (centi-degrees) +ve to left of track
|
|
int32_t bearing_error_cd(void);
|
|
|
|
float crosstrack_error(void);
|
|
|
|
int32_t target_bearing_cd(void);
|
|
float turn_distance(float wp_radius);
|
|
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
|
|
void update_loiter(const struct Location ¢er_WP, float radius, int8_t loiter_direction);
|
|
void update_heading_hold(int32_t navigation_heading_cd);
|
|
void update_level_flight(void);
|
|
bool reached_loiter_target(void);
|
|
|
|
// this supports the NAVl1_* user settable parameters
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
private:
|
|
// reference to the AHRS object
|
|
AP_AHRS *_ahrs;
|
|
|
|
struct Location _current_loc;
|
|
|
|
// lateral acceration in m/s required to fly to the
|
|
// L1 reference point (+ve to right)
|
|
float _latAccDem;
|
|
|
|
// L1 tracking distance in meters which is dynamically updated
|
|
float _L1_dist;
|
|
|
|
// Status which is true when the vehicle has started circling the WP
|
|
bool _WPcircle;
|
|
|
|
// bearing angle (radians) to L1 point
|
|
float _nav_bearing;
|
|
|
|
// bearing error angle (radians) +ve to left of track
|
|
float _bearing_error;
|
|
|
|
// crosstrack error in meters
|
|
float _crosstrack_error;
|
|
|
|
// target bearing in centi-degrees from last update
|
|
int32_t _target_bearing_cd;
|
|
|
|
// L1 tracking loop period (sec)
|
|
AP_Float _L1_period;
|
|
// L1 tracking loop damping ratio
|
|
AP_Float _L1_damping;
|
|
// L1 control loop enable
|
|
AP_Int8 _enable;
|
|
|
|
// Convert a 2D vector from latitude and longitude to planar
|
|
// coordinates based on a reference point
|
|
Vector2f _geo2planar(const Vector2f &ref, const Vector2f &wp);
|
|
|
|
//Calculate the cross product of two 2D vectors
|
|
float _cross2D(const Vector2f &v1, const Vector2f &v2);
|
|
|
|
//Calculate the dot product of two 2D vectors
|
|
float _dot2D(const Vector2f &v1, const Vector2f &v2);
|
|
|
|
// Convert a 2D vector from planar coordinates to latitude and
|
|
// longitude based on a reference point
|
|
Vector2f _planar2geo(const Vector2f &ref, const Vector2f &wp);
|
|
|
|
//Calculate the maximum of two floating point numbers
|
|
float _maxf(const float &num1, const float &num2);
|
|
|
|
};
|
|
|
|
|
|
#endif //AP_L1_CONTROL_H
|