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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@444 f9c3cf11-9bcb-44bc-f272-b75c42450872

This commit is contained in:
jasonshort 2010-09-09 06:35:21 +00:00
parent 63ae72648e
commit dff31e7dde
2 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Navigation/Navigation.cpp
View File

@ -51,7 +51,15 @@ Navigation::load_first_wp(void)
void
Navigation::load_home(void)
{
set_home(_wp->get_waypoint_with_index(0));
home = _wp->get_waypoint_with_index(0);
}
void
Navigation::return_to_home_with_alt(uint32_t alt)
{
Waypoints::WP loc = _wp->get_waypoint_with_index(0);
loc.alt += alt;
set_next_wp(loc);
}
void
@ -66,11 +74,17 @@ Navigation::load_wp_index(uint8_t i)
_wp->set_index(i);
set_next_wp(_wp->get_current_waypoint());
}
void
Navigation::hold_location()
{
set_next_wp()
}
void
Navigation::set_home(Waypoints::WP loc)
{
Waypoints::WP loc
_wp->set_waypoint_with_index(loc, i);
home = loc;
//location = home;
@ -150,6 +164,13 @@ Navigation::wrap_360(int32_t error)
return error;
}
void
Navigation::set_bearing_error(int32_t error)
{
bearing_error = wrap_360(error);
}
/*****************************************
* Altitude error with Airspeed correction
*****************************************/
@ -166,6 +187,12 @@ Navigation::calc_altitude_error(void)
altitude_error = _target_altitude - location.alt;
}
void
Navigation::set_loiter_vector(int16_t v)
{
_vector = constrain(v, -18000, 18000);
}
void
Navigation::update_crosstrack(void)
{

12
libraries/AP_Navigation/Navigation.h
View File

@ -13,14 +13,20 @@ public:
void update_gps(void); // called 50 Hz
void set_home(Waypoints::WP loc);
void load_first_wp(void);
void load_wp_index(uint8_t i);
void load_wp_with_index(uint8_t i);
void load_home(void);
void return_to_home(void);
void reload_wp(void);
void set_wp(Waypoints::WP loc);
void set_hold_course(int16_t b); // 1 = hold a current course, 0 disables course hold
int16_t get_hold_course(void); // 1 = hold a current course, 0 disables course hold
void set_hold_course(int16_t b); // -1 deisables, 0-36000 enables
int16_t get_hold_course(void);
int32_t get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2);
int32_t get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2);
void set_bearing_error(int32_t error);
void update_crosstrack(void);
int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate