aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WPNav: initialise desired roll, pitch

Browse Source 
bug-fix to resolve 1/10th second twitch when loiter or waypoint
controller are first engaged
This commit is contained in:
Randy Mackay 2013-05-09 18:32:02 +09:00
parent d0fb332e9f
commit ed5ddfd9db
3 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -738,7 +738,7 @@ static AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps);
// Waypoint navigation object
// To-Do: move inertial nav up or other navigation variables down here
////////////////////////////////////////////////////////////////////////////////
static AC_WPNav wp_nav(&inertial_nav, &g.pi_loiter_lat, &g.pi_loiter_lon, &g.pid_loiter_rate_lat, &g.pid_loiter_rate_lon);
static AC_WPNav wp_nav(&inertial_nav, &ahrs, &g.pi_loiter_lat, &g.pi_loiter_lon, &g.pid_loiter_rate_lat, &g.pid_loiter_rate_lon);
////////////////////////////////////////////////////////////////////////////////
// Performance monitoring

11
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -59,8 +59,9 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
_inav(inav),
_ahrs(ahrs),
_pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
@ -134,6 +135,10 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
project_stopping_point(position, velocity, target);
_target.x = target.x;
_target.y = target.y;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
@ -293,6 +298,10 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
// default waypoint back to slow
_flags.fast_waypoint = false;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
}
/// advance_target_along_track - move target location along track from origin to destination

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -35,7 +35,7 @@ class AC_WPNav
public:
/// Constructor
AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
///
/// simple loiter controller
@ -175,8 +175,9 @@ protected:
/// set climb param to true if track climbs vertically, false if descending
void calculate_wp_leash_length(bool climb);
// pointers to inertial nav library
// pointers to inertial nav and ahrs libraries
AP_InertialNav* _inav;
AP_AHRS* _ahrs;
// pointers to pid controllers
APM_PI* _pid_pos_lat;