Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

don't fly

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@428 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-09-07 05:43:30 +00:00
parent a496ef2b17
commit 66a45523d6
2 changed files with 64 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Navigation/Navigation.cpp
View File

@ -1,7 +1,8 @@
#include "Navigation.h" #include "Navigation.h"
Navigation::Navigation(GPS *withGPS) : Navigation::Navigation(GPS *withGPS, Waypoints *withWP) :
_gps(withGPS), _gps(withGPS),
_wp(withWP),
_hold_course(-1) _hold_course(-1)
{ {
} }
@ -41,11 +42,38 @@ Navigation::update_gps()
} }
} }
void
Navigation::load_first_wp(void)
{
set_next_wp(_wp->get_waypoint_with_index(1));
}
void
Navigation::load_home(void)
{
set_home(_wp->get_waypoint_with_index(0));
}
void
Navigation::reload_wp(void)
{
set_next_wp(_wp->get_current_waypoint());
}
void
Navigation::load_wp_index(uint8_t i)
{
_wp->set_index(i);
set_next_wp(_wp->get_current_waypoint());
}
void void
Navigation::set_home(Waypoints::WP loc) Navigation::set_home(Waypoints::WP loc)
{ {
_wp->set_waypoint_with_index(loc, i);
home = loc; home = loc;
location = home; //location = home;
} }
void void
@ -71,6 +99,7 @@ Navigation::set_next_wp(Waypoints::WP loc)
// ---------------------------- // ----------------------------
reset_crosstrack(); reset_crosstrack();
} }
void void
Navigation::calc_long_scaling(int32_t lat) Navigation::calc_long_scaling(int32_t lat)
{ {
@ -81,7 +110,7 @@ Navigation::calc_long_scaling(int32_t lat)
} }
void void
Navigation::hold_course(int8_t b) Navigation::set_hold_course(int16_t b)
{ {
if(b) if(b)
_hold_course = bearing; _hold_course = bearing;
@ -89,6 +118,12 @@ Navigation::hold_course(int8_t b)
_hold_course = -1; _hold_course = -1;
} }
int16_t
Navigation::get_hold_course(void)
{
return _hold_course;
}
void void
Navigation::calc_distance_error() Navigation::calc_distance_error()
{ {

44
libraries/AP_Navigation/Navigation.h
View File

@ -8,43 +8,49 @@
class Navigation { class Navigation {
public: public:
Navigation(GPS *withGPS); Navigation(GPS *withGPS, Waypoints *withWP);
void update_gps(void); // called 50 Hz void update_gps(void); // called 50 Hz
void set_home(Waypoints::WP loc); void set_home(Waypoints::WP loc);
void set_next_wp(Waypoints::WP loc); void load_first_wp(void);
void hold_course(int8_t b); // 1 = hold a current course, 0 disables course hold void load_wp_index(uint8_t i);
long get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2); void load_home(void);
long get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2); 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
int32_t get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2);
int32_t get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2);
void update_crosstrack(void);
long bearing; // deg * 100 : 0 to 360 current desired bearing to navigate int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
long distance; // meters : distance plane to next waypoint int32_t distance; // meters : distance plane to next waypoint
long altitude_above_home; // meters * 100 relative to home position int32_t altitude_above_home; // meters * 100 relative to home position
long total_distance; // meters : distance between waypoints int32_t total_distance; // meters : distance between waypoints
long bearing_error; // deg * 100 : 18000 to -18000 int32_t bearing_error; // deg * 100 : 18000 to -18000
long altitude_error; // deg * 100 : 18000 to -18000 int32_t altitude_error; // deg * 100 : 18000 to -18000
int16_t loiter_sum; int16_t loiter_sum;
Waypoints::WP home, location, prev_wp, next_wp; Waypoints::WP home, location, prev_wp, next_wp;
private: private:
void calc_long_scaling(int32_t lat); void calc_int32_t_scaling(int32_t lat);
void calc_bearing_error(void); void calc_bearing_error(void);
void calc_altitude_error(void); void calc_altitude_error(void);
void calc_distance_error(void); void calc_distance_error(void);
void update_crosstrack(void);
void reset_crosstrack(void); void reset_crosstrack(void);
int32_t wrap_360(int32_t error); // utility int32_t wrap_360(int32_t error); // utility
int16_t _old_bearing; // used to track delta on the bearing int16_t _old_bearing; // used to track delta on the bearing
GPS *_gps; GPS *_gps;
long _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target Waypoints *_wp;
int32_t _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline
long _hold_course; // deg * 100 dir of plane int16_t _hold_course; // deg * 100 dir of plane
long _target_altitude; // used for int32_t _target_altitude; // used for
long _offset_altitude; // used for int32_t _offset_altitude; // used for
float _altitude_estimate; float _altitude_estimate;
float _scaleLongUp; // used to reverse longtitude scaling float _scaleLongUp; // used to reverse int32_ttitude scaling
float _scaleLongDown; // used to reverse longtitude scaling float _scaleLongDown; // used to reverse int32_ttitude scaling
int16_t _loiter_delta; int16_t _loiter_delta;
}; };