Cleanup: removed unused AP_Navigation
This commit is contained in:
Andrew Tridgell
parent
4ee969f439
commit
678c281f78
@ -1,249 +0,0 @@
|
||||
#include "Navigation.h"
|
||||
|
||||
Navigation::Navigation(GPS *withGPS, Waypoints *withWP) :
|
||||
_gps(withGPS),
|
||||
_wp(withWP),
|
||||
_hold_course(-1)
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::update_gps()
|
||||
{
|
||||
location.alt = _gps->altitude;
|
||||
location.lng = _gps->longitude;
|
||||
location.lat = _gps->latitude;
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
bearing = get_bearing(&location, &next_wp);
|
||||
|
||||
// waypoint distance from plane
|
||||
distance = get_distance(&location, &next_wp);
|
||||
|
||||
calc_bearing_error();
|
||||
calc_altitude_error();
|
||||
altitude_above_home = location.alt - home.alt;
|
||||
|
||||
// check if we have missed the WP
|
||||
_loiter_delta = (bearing - _old_bearing) / 100;
|
||||
|
||||
// reset the old value
|
||||
_old_bearing = bearing;
|
||||
|
||||
// wrap values
|
||||
if (_loiter_delta > 170) _loiter_delta -= 360;
|
||||
if (_loiter_delta < -170) _loiter_delta += 360;
|
||||
loiter_sum += abs(_loiter_delta);
|
||||
|
||||
if (distance <= 0) {
|
||||
distance = -1;
|
||||
Serial.print("MSG wp error ");
|
||||
Serial.println(distance, DEC);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::load_first_wp(void)
|
||||
{
|
||||
set_next_wp(_wp->get_waypoint_with_index(1));
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::load_home(void)
|
||||
{
|
||||
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
|
||||
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
|
||||
Navigation::hold_location()
|
||||
{
|
||||
// set_next_wp() XXX needs to be implemented
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::set_home(Waypoints::WP loc)
|
||||
{
|
||||
_wp->set_waypoint_with_index(loc, 0);
|
||||
home = loc;
|
||||
//location = home;
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::set_next_wp(Waypoints::WP loc)
|
||||
{
|
||||
prev_wp = next_wp;
|
||||
next_wp = loc;
|
||||
|
||||
if(_scaleLongDown == 0)
|
||||
calc_long_scaling(loc.lat);
|
||||
|
||||
total_distance = get_distance(&location, &next_wp);
|
||||
distance = total_distance;
|
||||
|
||||
bearing = get_bearing(&location, &next_wp);
|
||||
_old_bearing = bearing;
|
||||
|
||||
// clear loitering code
|
||||
_loiter_delta = 0;
|
||||
loiter_sum = 0;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::calc_long_scaling(int32_t lat)
|
||||
{
|
||||
// this is used to offset the shrinking longitude as we go towards the poles
|
||||
float rads = (fabs(lat) / T7) * 0.0174532925;
|
||||
_scaleLongDown = cos(rads);
|
||||
_scaleLongUp = 1.0f / cos(rads);
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::set_hold_course(int16_t b)
|
||||
{
|
||||
if(b)
|
||||
_hold_course = bearing;
|
||||
else
|
||||
_hold_course = -1;
|
||||
}
|
||||
|
||||
int16_t
|
||||
Navigation::get_hold_course(void)
|
||||
{
|
||||
return _hold_course;
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::calc_distance_error()
|
||||
{
|
||||
//distance_estimate += (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
|
||||
//distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance);
|
||||
//distance = max(distance_estimate,10);
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::calc_bearing_error(void)
|
||||
{
|
||||
if(_hold_course == -1) {
|
||||
bearing_error = wrap_180(bearing - _gps->ground_course);
|
||||
}else{
|
||||
bearing_error = _hold_course;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t
|
||||
Navigation::wrap_180(int32_t error)
|
||||
{
|
||||
if (error > 18000) error -= 36000;
|
||||
if (error < -18000) error += 36000;
|
||||
return error;
|
||||
}
|
||||
|
||||
int32_t
|
||||
Navigation::wrap_360(int32_t angle)
|
||||
{
|
||||
if (angle > 36000) angle -= 36000;
|
||||
if (angle < 0) angle += 36000;
|
||||
return angle;
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::set_bearing_error(int32_t error)
|
||||
{
|
||||
bearing_error = wrap_180(error);
|
||||
}
|
||||
|
||||
|
||||
/*****************************************
|
||||
* Altitude error with Airspeed correction
|
||||
*****************************************/
|
||||
void
|
||||
Navigation::calc_altitude_error(void)
|
||||
{
|
||||
// limit climb rates
|
||||
_target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20));
|
||||
if(prev_wp.alt > next_wp.alt) {
|
||||
_target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt);
|
||||
}else{
|
||||
_target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt);
|
||||
}
|
||||
altitude_error = _target_altitude - location.alt;
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::set_loiter_vector(int16_t v)
|
||||
{
|
||||
// _vector = constrain(v, -18000, 18000); XXX needs to be implemented
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::update_crosstrack(void)
|
||||
{
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
if (abs(bearing - _crosstrack_bearing) < 4500) { // If we are too far off or too close we don't do track following
|
||||
_crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance; // Meters we are off track line
|
||||
bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigation::reset_crosstrack(void)
|
||||
{
|
||||
_crosstrack_bearing = get_bearing(&location, &next_wp); // Used for track following
|
||||
}
|
||||
|
||||
long
|
||||
Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2)
|
||||
{
|
||||
if(loc1->lat == 0 || loc1->lng == 0)
|
||||
return -1;
|
||||
if(loc2->lat == 0 || loc2->lng == 0)
|
||||
return -1;
|
||||
if(_scaleLongDown == 0)
|
||||
calc_long_scaling(loc2->lat);
|
||||
float dlat = (float)(loc2->lat - loc1->lat);
|
||||
float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown;
|
||||
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
|
||||
}
|
||||
|
||||
long
|
||||
Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2)
|
||||
{
|
||||
if(loc1->lat == 0 || loc1->lng == 0)
|
||||
return -1;
|
||||
if(loc2->lat == 0 || loc2->lng == 0)
|
||||
return -1;
|
||||
if(_scaleLongDown == 0)
|
||||
calc_long_scaling(loc2->lat);
|
||||
long off_x = loc2->lng - loc1->lng;
|
||||
long off_y = (loc2->lat - loc1->lat) * _scaleLongUp;
|
||||
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
|
||||
if (bearing < 0)
|
||||
bearing += 36000;
|
||||
return bearing;
|
||||
}
|
||||
@ -1,80 +0,0 @@
|
||||
|
||||
#ifndef AP_NAVIGATION_h
|
||||
#define AP_NAVIGATION_h
|
||||
|
||||
#define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter)
|
||||
#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100
|
||||
#include <GPS.h> // ArduPilot GPS Library
|
||||
#include "Waypoints.h" // ArduPilot Waypoints Library
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#define T7 10000000
|
||||
|
||||
class Navigation {
|
||||
public:
|
||||
Navigation(GPS *withGPS, Waypoints *withWP);
|
||||
|
||||
void update_gps(void); // called 50 Hz
|
||||
void set_home(Waypoints::WP loc);
|
||||
void set_next_wp(Waypoints::WP loc);
|
||||
void load_first_wp(void);
|
||||
void load_wp_with_index(uint8_t i);
|
||||
void load_home(void);
|
||||
void return_to_home_with_alt(uint32_t alt);
|
||||
|
||||
void reload_wp(void);
|
||||
void load_wp_index(uint8_t i);
|
||||
void hold_location();
|
||||
void set_wp(Waypoints::WP loc);
|
||||
|
||||
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 set_loiter_vector(int16_t v);
|
||||
void update_crosstrack(void);
|
||||
|
||||
int32_t wrap_180(int32_t error); // utility
|
||||
int32_t wrap_360(int32_t angle); // utility
|
||||
|
||||
int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
|
||||
int32_t distance; // meters : distance plane to next waypoint
|
||||
int32_t altitude_above_home; // meters * 100 relative to home position
|
||||
int32_t total_distance; // meters : distance between waypoints
|
||||
int32_t bearing_error; // deg * 100 : 18000 to -18000
|
||||
int32_t altitude_error; // deg * 100 : 18000 to -18000
|
||||
|
||||
int16_t loiter_sum;
|
||||
Waypoints::WP home, location, prev_wp, next_wp;
|
||||
|
||||
private:
|
||||
void calc_int32_t_scaling(int32_t lat);
|
||||
void calc_bearing_error(void);
|
||||
void calc_altitude_error(void);
|
||||
void calc_distance_error(void);
|
||||
void calc_long_scaling(int32_t lat);
|
||||
void reset_crosstrack(void);
|
||||
|
||||
int16_t _old_bearing; // used to track delta on the bearing
|
||||
GPS * _gps;
|
||||
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
|
||||
int16_t _hold_course; // deg * 100 dir of plane
|
||||
int32_t _target_altitude; // used for
|
||||
int32_t _offset_altitude; // used for
|
||||
float _altitude_estimate;
|
||||
float _scaleLongUp; // used to reverse int32_ttitude scaling
|
||||
float _scaleLongDown; // used to reverse int32_ttitude scaling
|
||||
int16_t _loiter_delta;
|
||||
};
|
||||
|
||||
|
||||
#endif // AP_NAVIGATION_h
|
||||
@ -1,2 +0,0 @@
|
||||
BOARD = mega
|
||||
include ../../../../mk/Arduino.mk
|
||||
@ -1,152 +0,0 @@
|
||||
/*
|
||||
*
|
||||
* This test assumes you are at the LOWl demo Airport
|
||||
*
|
||||
*/
|
||||
|
||||
#include "Waypoints.h"
|
||||
#include "Navigation.h"
|
||||
#include "AP_GPS_IMU.h"
|
||||
#include "AP_RC.h"
|
||||
|
||||
|
||||
AP_GPS_IMU gps;
|
||||
Navigation nav((GPS *) &gps);
|
||||
AP_RC rc;
|
||||
|
||||
#define CH_ROLL 0
|
||||
#define CH_PITCH 1
|
||||
#define CH_THROTTLE 2
|
||||
#define CH_RUDDER 3
|
||||
|
||||
#define CH3_MIN 1100
|
||||
#define CH3_MAX 1900
|
||||
|
||||
#define REVERSE_RUDDER 1
|
||||
#define REVERSE_ROLL 1
|
||||
#define REVERSE_PITCH 1
|
||||
|
||||
int8_t did_init_home;
|
||||
int16_t servo_out[4];
|
||||
int16_t radio_trim[4] = {1500,1500,1100,1500};
|
||||
int16_t radio_in[4];
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.println("Navigation test");
|
||||
Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012};
|
||||
nav.set_next_wp(location_B);
|
||||
rc.init(radio_trim);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(20);
|
||||
gps.update();
|
||||
rc.read_pwm();
|
||||
for(int y=0; y<4; y++) {
|
||||
//rc.set_ch_pwm(y, rc.input[y]); // send to Servos
|
||||
radio_in[y] = rc.input[y];
|
||||
}
|
||||
|
||||
servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500;
|
||||
servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500;
|
||||
servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500;
|
||||
servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100;
|
||||
servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100);
|
||||
|
||||
output_Xplane();
|
||||
if(gps.new_data /* && gps.fix */ ) {
|
||||
if(did_init_home == 0) {
|
||||
did_init_home = 1;
|
||||
Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude};
|
||||
nav.set_home(wp);
|
||||
Serial.println("MSG init home");
|
||||
}else{
|
||||
nav.update_gps();
|
||||
}
|
||||
//print_loc();
|
||||
}
|
||||
}
|
||||
|
||||
void print_loc()
|
||||
{
|
||||
Serial.print("MSG GPS: ");
|
||||
Serial.print(nav.location.lat, DEC);
|
||||
Serial.print(", ");
|
||||
Serial.print(nav.location.lng, DEC);
|
||||
Serial.print(", ");
|
||||
Serial.print(nav.location.alt, DEC);
|
||||
Serial.print("\tDistance = ");
|
||||
Serial.print(nav.distance, DEC);
|
||||
Serial.print(" Bearing = ");
|
||||
Serial.print(nav.bearing/100, DEC);
|
||||
Serial.print(" Bearing err = ");
|
||||
Serial.print(nav.bearing_error/100, DEC);
|
||||
Serial.print(" alt err = ");
|
||||
Serial.print(nav.altitude_error/100, DEC);
|
||||
Serial.print(" Alt above home = ");
|
||||
Serial.println(nav.altitude_above_home/100, DEC);
|
||||
}
|
||||
|
||||
void print_pwm()
|
||||
{
|
||||
Serial.print("ch1 ");
|
||||
Serial.print(rc.input[CH_ROLL],DEC);
|
||||
Serial.print("\tch2: ");
|
||||
Serial.print(rc.input[CH_PITCH],DEC);
|
||||
Serial.print("\tch3 :");
|
||||
Serial.print(rc.input[CH_THROTTLE],DEC);
|
||||
Serial.print("\tch4 :");
|
||||
Serial.println(rc.input[CH_RUDDER],DEC);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
byte buf_len = 0;
|
||||
byte out_buffer[32];
|
||||
|
||||
void output_Xplane(void)
|
||||
{
|
||||
// output real-time sensor data
|
||||
Serial.print("AAA"); // Message preamble
|
||||
output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1
|
||||
output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3
|
||||
output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5
|
||||
output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7
|
||||
output_int((int)nav.distance); // 4 bytes 8,9
|
||||
output_int((int)nav.bearing_error); // 5 bytes 10,11
|
||||
output_int((int)nav.altitude_error); // 6 bytes 12,13
|
||||
output_int(0); // 7 bytes 14,15
|
||||
output_byte(1); // 8 bytes 16
|
||||
output_byte(1); // 9 bytes 17
|
||||
|
||||
// print out the buffer and checksum
|
||||
// ---------------------------------
|
||||
print_buffer();
|
||||
}
|
||||
|
||||
void output_int(int value)
|
||||
{
|
||||
//buf_len += 2;
|
||||
out_buffer[buf_len++] = value & 0xff;
|
||||
out_buffer[buf_len++] = (value >> 8) & 0xff;
|
||||
}
|
||||
void output_byte(byte value)
|
||||
{
|
||||
out_buffer[buf_len++] = value;
|
||||
}
|
||||
|
||||
void print_buffer()
|
||||
{
|
||||
byte ck_a = 0;
|
||||
byte ck_b = 0;
|
||||
for (byte i = 0; i < buf_len; i++) {
|
||||
Serial.print (out_buffer[i]);
|
||||
}
|
||||
Serial.print('\r');
|
||||
Serial.print('\n');
|
||||
buf_len = 0;
|
||||
}
|
||||
@ -1,2 +0,0 @@
|
||||
# this example does not build
|
||||
x
|
||||
@ -1,2 +0,0 @@
|
||||
BOARD = mega
|
||||
include ../../../../mk/Arduino.mk
|
||||
@ -1,28 +0,0 @@
|
||||
#include <Waypoints.h>
|
||||
#include <Navigation.h>
|
||||
#include <AP_GPS_IMU.h>
|
||||
|
||||
|
||||
AP_GPS_IMU gps;
|
||||
Navigation nav((GPS *) & gps);
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial.println("Navigation test");
|
||||
|
||||
Waypoints::WP location_A = {0, 0, 38.579633 * T7, -122.566265 * T7, 10000};
|
||||
Waypoints::WP location_B = {0, 0, 38.578743 * T7, -122.572782 * T7, 5000};
|
||||
|
||||
long distance = nav.get_distance(&location_A, &location_B);
|
||||
long bearing = nav.get_bearing(&location_A, &location_B);
|
||||
|
||||
Serial.print("\tDistance = ");
|
||||
Serial.print(distance, DEC);
|
||||
Serial.print(" Bearing = ");
|
||||
Serial.println(bearing, DEC);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user