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 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 bool xtrack_enabled = false;
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
//static bool xtrack_enabled = false;
//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 long circle_angle = 0;
static byte wp_control; // used to control - navgation or loiter
@ -349,7 +351,7 @@ static int airspeed; // m/s * 100
// 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 old_altitude;
static long yaw_error; // how off are we pointed
@ -674,8 +676,8 @@ static void medium_loop()
// ------------------------------------------------------
navigate();
// control mode specific updates to nav_bearing
// --------------------------------------------
// control mode specific updates
// -----------------------------
update_navigation();
if (g.log_bitmask & MASK_LOG_NTUN)
@ -799,7 +801,7 @@ static void fifty_hz_loop()
#endif
// use Yaw to find our bearing error
calc_bearing_error();
//calc_bearing_error();
//if (throttle_slew < 0)
// throttle_slew++;
@ -1246,7 +1248,7 @@ static void update_navigation()
wp_control = WP_MODE;
}else{
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_4.servo_out)); // 3 bytes 6, 7
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)energy_error); // 7 bytes 14,15
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.WriteByte(wp_verify_byte); // 2
DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4
DataFlash.WriteInt((int)long_error); // 5
DataFlash.WriteInt((int)lat_error); // 6
DataFlash.WriteInt((int)long_error); // 4
DataFlash.WriteInt((int)lat_error); // 5
/*
@ -568,7 +567,6 @@ static void Log_Read_Nav_Tuning()
DataFlash.ReadInt(), // distance
DataFlash.ReadByte(), // wp_verify_byte
DataFlash.ReadInt(), // target_bearing
DataFlash.ReadInt(), // nav_bearing
DataFlash.ReadInt(), // long_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,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
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_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// 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
// ----------------------------
crosstrack_bearing = target_bearing; // Used for track following
//crosstrack_bearing = target_bearing; // Used for track following
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
#endif
#ifndef NAV_I
# define NAV_I 0.1 // this
# define NAV_I 0.12 // this
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 16 // degrees
# define NAV_IMAX 20 // degrees
#endif
/*

35
ArduCopter/navigation.pde
View File

@ -34,11 +34,12 @@ static void navigate()
// nav_bearing will include xtrac correction
// -----------------------------------------
//xtrack_enabled = false;
if(xtrack_enabled){
nav_bearing = wrap_360(target_bearing + get_crosstrack_correction());
}else{
nav_bearing = target_bearing;
}
//if(xtrack_enabled){
// crosstrack_correction = get_crosstrack_correction();
//}else {
// crosstrack_correction = 0;
//}
}
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:
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
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 = 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);
//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
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 = 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);
}
@ -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 = wrap_180(bearing_error);
}
}*/
static long get_altitude_error()
{
@ -189,6 +192,7 @@ static long wrap_180(long error)
return error;
}
/*
static long get_crosstrack_correction(void)
{
// Crosstrack Error
@ -206,19 +210,20 @@ static long get_crosstrack_correction(void)
}
return 0;
}
*/
/*
static long cross_track_test()
{
long temp = wrap_180(target_bearing - crosstrack_bearing);
return abs(temp);
}
*/
/*
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
*/
static long get_altitude_above_home(void)
{
// 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.
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
@ -362,7 +362,7 @@ static void set_mode(byte mode)
led_mode = NORMAL_LEDS;
// most modes do not calculate crosstrack correction
xtrack_enabled = false;
//xtrack_enabled = false;
reset_nav_I();
switch(control_mode)
@ -411,7 +411,7 @@ static void set_mode(byte mode)
// do crosstrack correction
// XXX move to flight commands
xtrack_enabled = true;
//xtrack_enabled = true;
break;
case CIRCLE:
@ -437,7 +437,7 @@ static void set_mode(byte mode)
roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_AUTO;
xtrack_enabled = true;
//xtrack_enabled = true;
init_throttle_cruise();
next_WP = current_loc;
break;
@ -447,7 +447,7 @@ static void set_mode(byte mode)
roll_pitch_mode = RTL_RP;
throttle_mode = RTL_THR;
xtrack_enabled = true;
//xtrack_enabled = true;
init_throttle_cruise();
do_RTL();
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.
#
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.