Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Doug Weibel 2011-09-12 07:25:05 -06:00
commit 816bd59af6
10 changed files with 50 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/ArduCopter.pde
View File

@ -316,11 +316,13 @@ static bool did_ground_start = false; // have we ground started after first ar
// --------------------- // ---------------------
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate //static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static bool xtrack_enabled = false; //static bool xtrack_enabled = false;
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target //static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static long circle_angle = 0; static long circle_angle = 0;
static byte wp_control; // used to control - navgation or loiter static byte wp_control; // used to control - navgation or loiter
@ -349,7 +351,7 @@ static int airspeed; // m/s * 100
// Location Errors // Location Errors
// --------------- // ---------------
static long bearing_error; // deg * 100 : 0 to 36000 //static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude; static long old_altitude;
static long yaw_error; // how off are we pointed static long yaw_error; // how off are we pointed
@ -674,8 +676,8 @@ static void medium_loop()
// ------------------------------------------------------ // ------------------------------------------------------
navigate(); navigate();
// control mode specific updates to nav_bearing // control mode specific updates
// -------------------------------------------- // -----------------------------
update_navigation(); update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
@ -799,7 +801,7 @@ static void fifty_hz_loop()
#endif #endif
// use Yaw to find our bearing error // use Yaw to find our bearing error
calc_bearing_error(); //calc_bearing_error();
//if (throttle_slew < 0) //if (throttle_slew < 0)
// throttle_slew++; // throttle_slew++;
@ -1246,7 +1248,7 @@ static void update_navigation()
wp_control = WP_MODE; wp_control = WP_MODE;
}else{ }else{
set_mode(LOITER); set_mode(LOITER);
xtrack_enabled = false; //xtrack_enabled = false;
} }

2
ArduCopter/HIL_Xplane.pde
View File

@ -12,7 +12,7 @@ void HIL_XPLANE::output_HIL(void)
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9 output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11 //output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)altitude_error); // 6 bytes 12, 13 output_int((int)altitude_error); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15 output_int((int)energy_error); // 7 bytes 14,15
output_byte((int)g.waypoint_index); // 8 bytes 16 output_byte((int)g.waypoint_index); // 8 bytes 16

6
ArduCopter/Log.pde
View File

@ -537,9 +537,8 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)wp_distance); // 1 DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteByte(wp_verify_byte); // 2 DataFlash.WriteByte(wp_verify_byte); // 2
DataFlash.WriteInt((int)(target_bearing/100)); // 3 DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4 DataFlash.WriteInt((int)long_error); // 4
DataFlash.WriteInt((int)long_error); // 5 DataFlash.WriteInt((int)lat_error); // 5
DataFlash.WriteInt((int)lat_error); // 6
/* /*
@ -568,7 +567,6 @@ static void Log_Read_Nav_Tuning()
DataFlash.ReadInt(), // distance DataFlash.ReadInt(), // distance
DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadByte(), // wp_verify_byte
DataFlash.ReadInt(), // target_bearing DataFlash.ReadInt(), // target_bearing
DataFlash.ReadInt(), // nav_bearing
DataFlash.ReadInt(), // long_error DataFlash.ReadInt(), // long_error
DataFlash.ReadInt()); // lat_error DataFlash.ReadInt()); // lat_error

2
ArduCopter/Mavlink_Common.h
View File

@ -142,7 +142,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
chan, chan,
nav_roll / 1.0e2, nav_roll / 1.0e2,
nav_pitch / 1.0e2, nav_pitch / 1.0e2,
nav_bearing / 1.0e2, target_bearing / 1.0e2,
target_bearing / 1.0e2, target_bearing / 1.0e2,
wp_distance, wp_distance,
altitude_error / 1.0e2, altitude_error / 1.0e2,

3
ArduCopter/commands.pde
View File

@ -190,7 +190,6 @@ static void set_next_WP(struct Location *wp)
wp_totalDistance = get_distance(&current_loc, &next_WP); wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP); target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP // to check if we have missed the WP
// ---------------------------- // ----------------------------
@ -198,7 +197,7 @@ static void set_next_WP(struct Location *wp)
// set a new crosstrack bearing // set a new crosstrack bearing
// ---------------------------- // ----------------------------
crosstrack_bearing = target_bearing; // Used for track following //crosstrack_bearing = target_bearing; // Used for track following
gcs.print_current_waypoints(); gcs.print_current_waypoints();
} }

4
ArduCopter/config.h
View File

@ -482,10 +482,10 @@
# define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.1 // this # define NAV_I 0.12 // this
#endif #endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 16 // degrees # define NAV_IMAX 20 // degrees
#endif #endif
/* /*

35
ArduCopter/navigation.pde
View File

@ -34,11 +34,12 @@ static void navigate()
// nav_bearing will include xtrac correction // nav_bearing will include xtrac correction
// ----------------------------------------- // -----------------------------------------
//xtrack_enabled = false; //xtrack_enabled = false;
if(xtrack_enabled){
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); //if(xtrack_enabled){
}else{ // crosstrack_correction = get_crosstrack_correction();
nav_bearing = target_bearing; //}else {
} // crosstrack_correction = 0;
//}
} }
static bool check_missed_wp() static bool check_missed_wp()
@ -101,18 +102,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
} }
// find the rates: // find the rates:
float temp = radians((float)g_gps->ground_course/100.0);
// calc the cos of the error to tell how fast we are moving towards the target in cm // calc the cos of the error to tell how fast we are moving towards the target in cm
y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); y_actual_speed = (float)g_gps->ground_speed * cos(temp);
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
// calc the sin of the error to tell how fast we are moving laterally to the target in cm // calc the sin of the error to tell how fast we are moving laterally to the target in cm
x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); x_actual_speed = (float)g_gps->ground_speed * sin(temp);
x_rate_error = x_target_speed - x_actual_speed; x_rate_error = x_target_speed - x_actual_speed;
x_rate_error = constrain(x_rate_error, -250, 250); x_rate_error = constrain(x_rate_error, -600, 600);
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
} }
@ -128,11 +131,11 @@ static void calc_nav_pitch_roll()
} }
// ------------------------------ // ------------------------------
static void calc_bearing_error() /*static void calc_bearing_error()
{ {
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }*/
static long get_altitude_error() static long get_altitude_error()
{ {
@ -189,6 +192,7 @@ static long wrap_180(long error)
return error; return error;
} }
/*
static long get_crosstrack_correction(void) static long get_crosstrack_correction(void)
{ {
// Crosstrack Error // Crosstrack Error
@ -206,19 +210,20 @@ static long get_crosstrack_correction(void)
} }
return 0; return 0;
} }
*/
/*
static long cross_track_test() static long cross_track_test()
{ {
long temp = wrap_180(target_bearing - crosstrack_bearing); long temp = wrap_180(target_bearing - crosstrack_bearing);
return abs(temp); return abs(temp);
} }
*/
/*
static void reset_crosstrack() static void reset_crosstrack()
{ {
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
} }
*/
static long get_altitude_above_home(void) static long get_altitude_above_home(void)
{ {
// This is the altitude above the home location // This is the altitude above the home location

10
ArduCopter/system.pde
View File

@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands); MENU(main_menu, "ArduCopter 2.0.43 Beta", main_menu_commands);
#endif // CLI_ENABLED #endif // CLI_ENABLED
@ -362,7 +362,7 @@ static void set_mode(byte mode)
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction // most modes do not calculate crosstrack correction
xtrack_enabled = false; //xtrack_enabled = false;
reset_nav_I(); reset_nav_I();
switch(control_mode) switch(control_mode)
@ -411,7 +411,7 @@ static void set_mode(byte mode)
// do crosstrack correction // do crosstrack correction
// XXX move to flight commands // XXX move to flight commands
xtrack_enabled = true; //xtrack_enabled = true;
break; break;
case CIRCLE: case CIRCLE:
@ -437,7 +437,7 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO; throttle_mode = THROTTLE_AUTO;
xtrack_enabled = true; //xtrack_enabled = true;
init_throttle_cruise(); init_throttle_cruise();
next_WP = current_loc; next_WP = current_loc;
break; break;
@ -447,7 +447,7 @@ static void set_mode(byte mode)
roll_pitch_mode = RTL_RP; roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR; throttle_mode = RTL_THR;
xtrack_enabled = true; //xtrack_enabled = true;
init_throttle_cruise(); init_throttle_cruise();
do_RTL(); do_RTL();
break; break;

2
config.mk Normal file
View File

@ -0,0 +1,2 @@
BOARD=mega
PORT=/dev/null

7
libraries/AP_Common/Arduino.mk
View File

@ -259,7 +259,12 @@ SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS)
# make. # make.
# #
SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p'
LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) ifneq ($(findstring CYGWIN, $(SYSTYPE)),)
# Workaround a cygwin issue
LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR)))
else
LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR)))
endif
# #
# Find sketchbook libraries referenced by the sketch. # Find sketchbook libraries referenced by the sketch.