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

Copter: convert fn from body-frame to NE

This commit is contained in:
Randy Mackay 2015-08-11 04:25:20 -07:00
parent 181373cf69
commit 76e66be9cb
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/Attitude.cpp
View File

@ -308,3 +308,12 @@ void Copter::update_poscon_alt_max()
// pass limit to pos controller
pos_control.set_alt_max(alt_limit_cm);
}
// rotate vector from vehicle's perspective to North-East frame
void Copter::rotate_body_frame_to_NE(float &x, float &y)
{
float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
x = ne_x;
y = ne_y;
}

1
ArduCopter/Copter.h
View File

@ -563,6 +563,7 @@ private:
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle);
void update_poscon_alt_max();
void rotate_body_frame_to_NE(float &x, float &y);
void gcs_send_heartbeat(void);
void gcs_send_deferred(void);
void send_heartbeat(mavlink_channel_t chan);