Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add WPNAV_ACCEL_Z 

Allows configurable z-axis acceleration during missions
This commit is contained in:
Randy Mackay 2014-04-30 12:03:09 +09:00
parent 7d7a2aced7
commit 0103ae2eb0
2 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -61,6 +61,15 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @User: Standard
AP_GROUPINFO("ACCEL", 5, AC_WPNav, _wp_accel_cms, WPNAV_ACCELERATION),
// @Param: ACCEL_Z
// @DisplayName: Waypoint Vertical Acceleration
// @Description: Defines the vertical acceleration in cm/s/s used during missions
// @Units: cm/s/s
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT),
AP_GROUPEND
};
@ -323,6 +332,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.set_accel_z(_wp_accel_z_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
@ -552,11 +562,11 @@ void AC_WPNav::calculate_wp_leash_length()
_track_speed = _wp_speed_cms/pos_delta_unit_xy;
_track_leash_length = _pos_control.get_leash_xy()/pos_delta_unit_xy;
}else if(pos_delta_unit_xy == 0){
_track_accel = _pos_control.get_accel_z()/pos_delta_unit_z;
_track_accel = _wp_accel_z_cms/pos_delta_unit_z;
_track_speed = speed_z/pos_delta_unit_z;
_track_leash_length = leash_z/pos_delta_unit_z;
}else{
_track_accel = min(_pos_control.get_accel_z()/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_accel = min(_wp_accel_z_cms/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
_track_speed = min(speed_z/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
_track_leash_length = min(leash_z/pos_delta_unit_z, _pos_control.get_leash_xy()/pos_delta_unit_xy);
}

8
libraries/AC_WPNav/AC_WPNav.h
View File

@ -25,6 +25,8 @@
#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity
#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity
#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration betwen waypoints in cm/s/s
#define WPNAV_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
@ -100,6 +102,9 @@ public:
/// get_speed_down - returns target descent speed in cm/s during missions. Note: always positive
float get_speed_down() const { return _wp_speed_down_cms; }
/// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive
float get_accel_z() const { return _wp_accel_z_cms; }
/// get_wp_radius - access for waypoint radius in cm
float get_wp_radius() const { return _wp_radius_cm; }
@ -249,7 +254,8 @@ protected:
AP_Float _wp_speed_up_cms; // climb speed target in cm/s
AP_Float _wp_speed_down_cms; // descent speed target in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions
AP_Float _wp_accel_cms; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions
// loiter controller internal variables
uint32_t _loiter_last_update; // time of last update_loiter call