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

Copter: Move some common functions to AP_Math (NFC)

This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-12-03 22:07:39 +01:00 committed by Randy Mackay
parent 1a512609ac
commit bd027e4fcb
5 changed files with 5 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.cpp
View File

@ -80,7 +80,7 @@ float Copter::get_roi_yaw()
roi_yaw_counter++;
if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0;
yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP);
yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
}
return yaw_look_at_WP_bearing;

2
ArduCopter/Copter.h
View File

@ -1038,8 +1038,6 @@ private:
Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm);
float pv_alt_above_home(float alt_above_origin_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
float pv_distance_to_home_cm(const Vector3f &destination);
void default_dead_zones();
void init_rc_in();

2
ArduCopter/control_guided.cpp
View File

@ -757,7 +757,7 @@ bool Copter::guided_limit_check()
// check if we have gone beyond horizontal limit
if (guided_limit.horiz_max_cm > 0.0f) {
float horiz_move = pv_get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
float horiz_move = get_horizontal_distance_cm(guided_limit.start_pos, curr_pos);
if (horiz_move > guided_limit.horiz_max_cm) {
return true;
}

4
ArduCopter/navigation.cpp
View File

@ -83,8 +83,8 @@ void Copter::calc_home_distance_and_bearing()
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
home_distance = get_horizontal_distance_cm(curr, home);
home_bearing = get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);

18
ArduCopter/position_vector.cpp
View File

@ -29,25 +29,9 @@ float Copter::pv_alt_above_home(float alt_above_origin_cm)
return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
}
// pv_get_bearing_cd - return bearing in centi-degrees between two positions
float Copter::pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
{
float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
if (bearing < 0) {
bearing += 36000;
}
return bearing;
}
// pv_get_horizontal_distance_cm - return distance between two positions in cm
float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
{
return norm(destination.x-origin.x,destination.y-origin.y);
}
// returns distance between a destination and home in cm
float Copter::pv_distance_to_home_cm(const Vector3f &destination)
{
Vector3f home = pv_location_to_vector(ahrs.get_home());
return pv_get_horizontal_distance_cm(home, destination);
return get_horizontal_distance_cm(home, destination);
}