Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 09:33:59 -03:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Move some common functions to AP_Math (NFC)

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-12-03 22:08:08 +01:00 committed by Randy Mackay
parent bd027e4fcb
commit 9f6342a1d1
6 changed files with 4 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Attitude.cpp
View File

@ -71,7 +71,7 @@ float Sub::get_roi_yaw()
roi_yaw_counter++; roi_yaw_counter++;
if (roi_yaw_counter >= 4) { if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0; 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; return yaw_look_at_WP_bearing;

2
ArduSub/Sub.h
View File

@ -628,8 +628,6 @@ private:
void motors_output(); void motors_output();
Vector3f pv_location_to_vector(const Location& loc); Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm); float pv_alt_above_origin(float alt_above_home_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
void init_rc_in(); void init_rc_in();
void init_rc_out(); void init_rc_out();
void enable_motor_output(); void enable_motor_output();

2
ArduSub/control_auto.cpp
View File

@ -599,7 +599,7 @@ float Sub::get_auto_heading(void)
case AUTO_YAW_CORRECT_XTRACK: { case AUTO_YAW_CORRECT_XTRACK: {
// TODO return current yaw if not in appropriate mode // TODO return current yaw if not in appropriate mode
// Bearing of current track (centidegrees) // Bearing of current track (centidegrees)
float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination());
// Bearing from current position towards intermediate position target (centidegrees) // Bearing from current position towards intermediate position target (centidegrees)
float desired_angle = wp_nav.get_loiter_bearing_to_target(); float desired_angle = wp_nav.get_loiter_bearing_to_target();

2
ArduSub/control_guided.cpp
View File

@ -557,7 +557,7 @@ bool Sub::guided_limit_check()
// check if we have gone beyond horizontal limit // check if we have gone beyond horizontal limit
if (guided_limit.horiz_max_cm > 0.0f) { 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) { if (horiz_move > guided_limit.horiz_max_cm) {
return true; return true;
} }

2
ArduSub/fence.cpp
View File

@ -13,7 +13,7 @@ void Sub::fence_check()
Vector3f home = pv_location_to_vector(ahrs.get_home()); Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position(); Vector3f curr = inertial_nav.get_position();
int32_t home_distance = pv_get_horizontal_distance_cm(curr, home); int32_t home_distance = get_horizontal_distance_cm(curr, home);
// give fence library our current distance from home in meters // give fence library our current distance from home in meters
fence.set_home_distance(home_distance*0.01f); fence.set_home_distance(home_distance*0.01f);

15
ArduSub/position_vector.cpp
View File

@ -22,18 +22,3 @@ float Sub::pv_alt_above_origin(float alt_above_home_cm)
return alt_above_home_cm + (ahrs.get_home().alt - origin.alt); return alt_above_home_cm + (ahrs.get_home().alt - origin.alt);
} }
// pv_get_bearing_cd - return bearing in centi-degrees between two positions
float Sub::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 Sub::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
{
return norm(destination.x-origin.x,destination.y-origin.y);
}