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

Tracker: rename calc_body_frame_target to convert_ef_to_bf and formatting fixes 

No functional change
This commit is contained in:
Randy Mackay 2016-07-28 11:31:48 +09:00
parent e174014477
commit 4d5f1f9a33
2 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
AntennaTracker/Tracker.h
View File

@ -207,6 +207,10 @@ private:
void gcs_retry_deferred(void);
void load_parameters(void);
void update_auto(void);
void calc_angle_error(float pitch, float yaw, bool direction_reversed);
void convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
bool get_ef_yaw_direction();
void update_manual(void);
void update_scan(void);
bool servo_test_set_servo(uint8_t servo_num, uint16_t pwm);
@ -258,10 +262,6 @@ private:
void start_logging();
void log_init(void);
bool should_log(uint32_t mask);
void calc_angle_error(float pitch, float yaw, bool direction_reversed);
void calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
bool get_ef_yaw_direction();
public:
void mavlink_snoop(const mavlink_message_t* msg);

18
AntennaTracker/control_auto.cpp
View File

@ -26,7 +26,7 @@ void Tracker::update_auto(void)
float bf_pitch;
float bf_yaw;
calc_body_frame_target(pitch, yaw, bf_pitch, bf_yaw);
convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
// only move servos if target is at least distance_min away
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) {
@ -34,6 +34,7 @@ void Tracker::update_auto(void)
update_yaw_servo(bf_yaw);
}
}
void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
// Pitch angle error in centidegrees
@ -58,15 +59,16 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
// earth frame to body frame angle error conversion
float bf_pitch_err;
float bf_yaw_err;
calc_body_frame_target(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
}
void Tracker::calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
void Tracker::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
{
// earth frame to body frame pitch and yaw conversion
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
}
bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
@ -76,12 +78,12 @@ bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& e
return false;
}
// convert earth frame angle or rates to body frame
ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw; //pitch
ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw; //yaw
ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw;
ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw;
return true;
}
// return value is true if takign the long road to the target, false if normal, shortest direction should be used
// return value is true if taking the long road to the target, false if normal, shortest direction should be used
bool Tracker::get_ef_yaw_direction()
{
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees