// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const 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),
_pid_rate_lon(pid_rate_lon),
_loiter_last_update(0),
_wpnav_last_update(0),
_althold_kP(WPNAV_ALT_HOLD_P),
_desired_roll(0),
_desired_pitch(0),
_target(0,0,0),
_pilot_vel_forward_cms(0),
_pilot_vel_right_cms(0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0),
_track_speed(0),
_track_leash_length(0),
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
{
AP_Param::setup_object_defaults(this, var_info);
// initialise leash lengths
calculate_wp_leash_length(true);
calculate_loiter_leash_length();
}
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
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),
_pid_rate_lon(pid_rate_lon),
_loiter_last_update(0),
_wpnav_last_update(0),
_cos_yaw(1.0),
_sin_yaw(0.0),
_cos_pitch(1.0),
_althold_kP(WPNAV_ALT_HOLD_P),
_desired_roll(0),
_desired_pitch(0),
_target(0,0,0),
_pilot_vel_forward_cms(0),
_pilot_vel_right_cms(0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0),
_track_speed(0),
_track_leash_length(0),
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
{
AP_Param::setup_object_defaults(this, var_info);
// calculate loiter leash
calculate_loiter_leash_length();
}
