Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use ahrs trig values

Browse Source
This commit is contained in:
Randy Mackay 2014-02-08 16:38:20 +09:00
parent 98d667c916
commit c9415a08f1
2 changed files with 5 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -77,9 +77,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
_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),
@ -201,8 +198,8 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
}
// rotate pilot input to lat/lon frame
target_vel_adj.x = (_pilot_vel_forward_cms*_cos_yaw - _pilot_vel_right_cms*_sin_yaw);
target_vel_adj.y = (_pilot_vel_forward_cms*_sin_yaw + _pilot_vel_right_cms*_cos_yaw);
target_vel_adj.x = (_pilot_vel_forward_cms*_ahrs->cos_yaw() - _pilot_vel_right_cms*_ahrs->sin_yaw());
target_vel_adj.y = (_pilot_vel_forward_cms*_ahrs->sin_yaw() + _pilot_vel_right_cms*_ahrs->cos_yaw());
// add desired change in velocity to current target velocit
_target_vel.x += target_vel_adj.x*nav_dt;
@ -665,11 +662,11 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw;
accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
accel_forward = accel_lat*_ahrs->cos_yaw() + accel_lon*_ahrs->sin_yaw();
accel_right = -accel_lat*_ahrs->sin_yaw() + accel_lon*_ahrs->cos_yaw();
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
_desired_roll = constrain_float(fast_atan(accel_right*_ahrs->cos_pitch()/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
}

10
libraries/AC_WPNav/AC_WPNav.h
View File

@ -118,13 +118,6 @@ public:
/// set_desired_alt - set desired altitude (in cm above home)
void set_desired_alt(float desired_alt) { _target.z = desired_alt; }
/// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
void set_cos_sin_yaw(float cos_yaw, float sin_yaw, float cos_pitch) {
_cos_yaw = cos_yaw;
_sin_yaw = sin_yaw;
_cos_pitch = cos_pitch;
}
/// set_althold_kP - pass in alt hold controller's P gain
void set_althold_kP(float kP) { if(kP>0.0f) _althold_kP = kP; }
@ -209,9 +202,6 @@ protected:
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _loiter_dt; // time difference since last loiter call
float _wpnav_dt; // time difference since last loiter call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw;
float _cos_pitch;
float _althold_kP; // alt hold's P gain
// output from controller