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

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

This commit is contained in:
Michael Oborne 2012-01-15 17:01:05 +08:00
parent 5cbecce8a1 7e411f5fb6
commit 456b532602
16 changed files with 103 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/APM_Config.h
View File

@ -64,4 +64,10 @@
#define USERHOOK_VARIABLES "UserVariables.h"
// to enable, set to 1
// to disable, set to 0
#define AUTO_THROTTLE_HOLD 1
//# define LOGGING_ENABLED DISABLED

46
ArduCopter/ArduCopter.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.2"
#define THISFIRMWARE "ArduCopter V2.2 b2"
/*
ArduCopter Version 2.2
Authors: Jason Short
@ -858,6 +858,12 @@ void loop()
// -------------------------------------------------
estimate_velocity();
// check for new GPS messages
// --------------------------
if(GPS_enabled){
update_GPS();
}
// perform 10hz tasks
// ------------------
medium_loop();
@ -879,7 +885,7 @@ void loop()
Log_Write_Performance();
gps_fix_count = 0;
perf_mon_counter = 0;
perf_mon_counter = 0;
}
//PORTK &= B10111111;
}
@ -901,10 +907,6 @@ static void fast_loop()
// IMU DCM Algorithm
read_AHRS();
if(GPS_enabled){
update_GPS();
}
// custom code/exceptions for flight modes
// ---------------------------------------
update_yaw_mode();
@ -1287,13 +1289,13 @@ static void update_GPS(void)
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if(gps_watchdog < 12){
if(gps_watchdog < 30){
gps_watchdog++;
}else{
// after 12 reads we guess we may have lost GPS signal, stop navigating
// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
nav_roll = 0;
nav_pitch = 0;
nav_roll >>= 1;
nav_pitch >>= 1;
}
if (g_gps->new_data && g_gps->fix) {
@ -1355,8 +1357,6 @@ static void update_GPS(void)
update_altitude();
#endif
} else {
g_gps->new_data = false;
}
}
@ -1460,7 +1460,7 @@ void update_roll_pitch_mode(void)
// new radio frame is used to make sure we only call this at 50hz
void update_simple_mode(void)
{
float simple_sin_y=0, simple_cos_x=0;
static float simple_sin_y=0, simple_cos_x=0;
// used to manage state machine
// which improves speed of function
@ -1494,6 +1494,10 @@ void update_throttle_mode(void)
{
int16_t throttle_out;
#if AUTO_THROTTLE_HOLD != 0
static float throttle_avg = THROTTLE_CRUISE;
#endif
switch(throttle_mode){
case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){
@ -1508,11 +1512,13 @@ void update_throttle_mode(void)
}
#endif
#if AUTO_THROTTLE_HOLD != 0
// calc average throttle
if ((g.rc_3.control_in > MINIMUM_THROTTLE)){
//throttle_avg = throttle_avg * .98 + rc_3.control_in * .02;
//g.throttle_cruise = throttle_avg;
if ((g.rc_3.control_in > MINIMUM_THROTTLE) && abs(climb_rate) < 60){
throttle_avg = throttle_avg * .98 + (float)g.rc_3.control_in * .02;
g.throttle_cruise = throttle_avg;
}
#endif
// Code to manage the Copter state
if ((millis() - takeoff_timer) > 5000){
@ -1875,17 +1881,11 @@ adjust_altitude()
// we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180;
manual_boost = max(-120, manual_boost);
g.throttle_cruise += g.pi_alt_hold.get_integrator();
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
update_throttle_cruise();
}else if (g.rc_3.control_in >= 650){
// we add 0 to 100 PWM to hover
manual_boost = g.rc_3.control_in - 650;
g.throttle_cruise += g.pi_alt_hold.get_integrator();
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
update_throttle_cruise();
}else {
manual_boost = 0;
}

6
ArduCopter/Attitude.pde
View File

@ -166,7 +166,7 @@ static int16_t
get_nav_throttle(int32_t z_error)
{
static int16_t old_output = 0;
static int16_t rate_d = 0;
//static int16_t rate_d = 0;
int16_t rate_error;
int16_t output;
@ -187,10 +187,10 @@ get_nav_throttle(int32_t z_error)
output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
// a positive climb rate means we're going up
rate_d = ((rate_d + climb_rate)>>1) * 1; // replace with gain
//rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain
// slight adjustment to alt hold output
output -= constrain(rate_d, -25, 25);
//output -= constrain(rate_d, -25, 25);
// light filter of output
output = (old_output * 3 + output) / 4;

5
ArduCopter/commands.pde
View File

@ -101,16 +101,19 @@ static void set_cmd_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
// Make sure our WP_total
if(g.command_total <= i)
if(g.command_total < (i+1))
g.command_total.set_and_save(i+1);
}
static int32_t read_alt_to_hold()
{
return current_loc.alt;
/*
if(g.RTL_altitude <= 0)
return current_loc.alt;
else
return g.RTL_altitude;// + home.alt;
*/
}

10
ArduCopter/commands_logic.pde
View File

@ -271,7 +271,7 @@ static void do_land()
set_next_WP(&current_loc);
// Set a new target altitude
set_new_altitude(0);
set_new_altitude(-200);
}
static void do_loiter_unlimited()
@ -290,7 +290,7 @@ static void do_loiter_turns()
wp_control = CIRCLE_MODE;
// reset desired location
circle_angle = 0;
if(command_nav_queue.lat == 0){
// allow user to specify just the altitude
@ -305,6 +305,10 @@ static void do_loiter_turns()
loiter_total = command_nav_queue.p1 * 360;
loiter_sum = 0;
old_target_bearing = target_bearing;
circle_angle = target_bearing + 18000;
circle_angle = wrap_360(circle_angle);
circle_angle *= RADX100;
}
static void do_loiter_time()
@ -364,7 +368,7 @@ static bool verify_land()
wp_control = NO_NAV_MODE;
// try and come down faster
landing_boost++;
landing_boost = min(landing_boost, 20);
landing_boost = min(landing_boost, 30);
}else{
landing_boost = 0;
wp_control = LOITER_MODE;

2
ArduCopter/config.h
View File

@ -614,7 +614,7 @@
# define LOITER_P 2.0 // was .25 in previous
#endif
#ifndef LOITER_I
# define LOITER_I 0.05 // Wind control
# define LOITER_I 0.04 // Wind control
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees°

12
ArduCopter/control_modes.pde
View File

@ -111,6 +111,7 @@ static void read_trim_switch()
trim_flag = false;
if(control_mode == AUTO){
// reset the mission
CH7_wp_index = 0;
g.command_total.set_and_save(1);
return;
@ -118,7 +119,7 @@ static void read_trim_switch()
if(CH7_wp_index == 0){
// this is our first WP, let's save WP 1 as a takeoff
// increment index
// increment index to WP index of 1 (home is stored at 0)
CH7_wp_index = 1;
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
@ -134,7 +135,7 @@ static void read_trim_switch()
// increment index
CH7_wp_index++;
// set the next_WP, 0 is Home so we don't set that
// set the next_WP (home is stored at 0)
// max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100);
@ -149,8 +150,11 @@ static void read_trim_switch()
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
// save the index
g.command_total.set_and_save(CH7_wp_index + 1);
// 0 = home
// 1 = takeoff
// 2 = WP 2
// 3 = command total
}
}
}

2
ArduCopter/motors.pde
View File

@ -121,6 +121,8 @@ static void init_disarm_motors()
motor_armed = false;
compass.save_offsets();
g.throttle_cruise.save();
// we are not in the air
takeoff_complete = false;

12
ArduCopter/navigation.pde
View File

@ -46,25 +46,26 @@ static void calc_XY_velocity(){
static int32_t last_longitude = 0;
static int32_t last_latitude = 0;
static int16_t x_speed_old = 0;
static int16_t y_speed_old = 0;
//static int16_t x_speed_old = 0;
//static int16_t y_speed_old = 0;
// y_GPS_speed positve = Up
// x_GPS_speed positve = Right
// this speed is ~ in cm because we are using 10^7 numbers from GPS
float tmp = 1.0/dTnav;
//float tmp = 5;
// straightforward approach:
/*
///*
int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp;
// slight averaging filter
x_GPS_speed = (x_GPS_speed + x_estimate) >> 1;
y_GPS_speed = (y_GPS_speed + y_estimate) >> 1;
*/
//*/
/*
// Ryan Beall's forward estimator:
int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * tmp;
@ -74,6 +75,7 @@ static void calc_XY_velocity(){
x_speed_old = x_speed_new;
y_speed_old = y_speed_new;
*/
last_longitude = g_gps->longitude;
last_latitude = g_gps->latitude;

16
ArduCopter/system.pde
View File

@ -558,9 +558,7 @@ static void set_mode(byte mode)
if(throttle_mode == THROTTLE_MANUAL){
// reset all of the throttle iterms
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
update_throttle_cruise();
}else {
// an automatic throttle
// todo: replace with a throttle cruise estimator
@ -605,15 +603,27 @@ init_simple_bearing()
initial_simple_bearing = dcm.yaw_sensor;
}
static void update_throttle_cruise()
{
int16_t tmp = g.pi_alt_hold.get_integrator();
if(tmp != 0){
g.throttle_cruise += tmp;
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
}
}
static void
init_throttle_cruise()
{
#if AUTO_THROTTLE_HOLD == 0
// are we moving from manual throttle to auto_throttle?
if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){
g.pi_throttle.reset_I();
g.pi_alt_hold.reset_I();
g.throttle_cruise.set_and_save(g.rc_3.control_in);
}
#endif
}
#if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED

19
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -56,6 +56,8 @@ extern "C" {
// is not available to the arduino digitalRead function.
#define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC))
// oversampling 3 gives highest resolution
#define OVERSAMPLING 3
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
@ -64,7 +66,6 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
// We read the calibration data registers
@ -142,7 +143,7 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
// Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress()
{
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) {
healthy = false;
}
}
@ -163,14 +164,15 @@ void AP_Baro_BMP085::ReadPress()
return;
}
RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
if(_offset_press == 0){
if (_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
} else{
RawPress -= _offset_press;
}
// filter
_press_filter[_press_index++] = RawPress;
@ -186,7 +188,6 @@ void AP_Baro_BMP085::ReadPress()
// grab result
RawPress /= PRESS_FILTER_SIZE;
//RawPress >>= 3;
RawPress += _offset_press;
}
@ -261,16 +262,16 @@ void AP_Baro_BMP085::Calculate()
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11;
x3 = x1 + x2;
//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0
//b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0
tmp = ac1;
tmp = (tmp*4 + x3)<<oss;
tmp = (tmp*4 + x3)<<OVERSAMPLING;
b3 = (tmp+2)/4;
x1 = ac3 * b6 >> 13;
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING);
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8);

13
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -28,14 +28,11 @@ class AP_Baro_BMP085 : public AP_Baro
private:
int32_t RawPress;
int32_t _offset_press;
int32_t _offset_press;
int32_t RawTemp;
int16_t Temp;
int32_t Press;
//int Altitude;
uint8_t oss;
uint32_t Press;
bool _apm2_hardware;
//int32_t Press0; // Pressure at sea level
// State machine
@ -44,9 +41,9 @@ class AP_Baro_BMP085 : public AP_Baro
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
int _temp_filter[TEMP_FILTER_SIZE];
int _press_filter[PRESS_FILTER_SIZE];
long _offset_temp;
int16_t _temp_filter[TEMP_FILTER_SIZE];
int32_t _press_filter[PRESS_FILTER_SIZE];
int32_t _offset_temp;
uint8_t _temp_index;
uint8_t _press_index;

2
libraries/AP_Baro/AP_Baro_BMP085_hil.h
View File

@ -17,7 +17,6 @@ public:
//int Altitude;
bool healthy;
uint8_t oss;
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
@ -26,7 +25,6 @@ public:
int32_t get_raw_pressure();
int32_t get_raw_temp();
void setHIL(float Temp, float Press);
int32_t _offset_press;
};
#endif // __AP_BARO_BMP085_HIL_H__

13
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
View File

@ -237,17 +237,17 @@ AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn )
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
}
// returns resolution (either 400 or 1200 counts per inch)
// returns resolution (either 400 or 1600 counts per inch)
int
AP_OpticalFlow_ADNS3080::get_resolution()
{
if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 )
return 400;
else
return 1200;
return 1600;
}
// set parameter to 400 or 1200 counts per inch
// set parameter to 400 or 1600 counts per inch
void
AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
{
@ -255,12 +255,17 @@ AP_OpticalFlow_ADNS3080::set_resolution(int resolution)
if( resolution == ADNS3080_RESOLUTION_400 ) {
regVal &= ~0x10;
}else if( resolution == ADNS3080_RESOLUTION_1200) {
scaler = AP_OPTICALFLOW_ADNS3080_SCALER;
}else if( resolution == ADNS3080_RESOLUTION_1600) {
regVal |= 0x10;
scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4;
}
delayMicroseconds(50); // small delay
write_register(ADNS3080_CONFIGURATION_BITS, regVal);
// this will affect conversion factors so update them
update_conversion_factors();
}
// get_frame_rate_auto - return whether frame rate is set to "auto" or manual

6
libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h
View File

@ -77,7 +77,7 @@
#define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01
#define ADNS3080_RESOLUTION_400 400
#define ADNS3080_RESOLUTION_1200 1200
#define ADNS3080_RESOLUTION_1600 1600
// Extended Configuration bits
#define ADNS3080_SERIALNPU_OFF 0x02
@ -118,8 +118,8 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow
bool get_led_always_on(); // returns true if LED is always on, false if only on when required
void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required
int get_resolution(); // returns resolution (either 400 or 1200 counts per inch)
void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch
int get_resolution(); // returns resolution (either 400 or 1600 counts per inch)
void set_resolution(int resolution); // set parameter to 400 or 1600 counts per inch
bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual
void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false)

5
libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde
View File

@ -201,7 +201,7 @@ void set_resolution()
Serial.print("resolution: ");
Serial.println(resolution);
Serial.println("Choose new value:");
Serial.println(" 1) 1200");
Serial.println(" 1) 1600");
Serial.println(" 4) 400");
Serial.println(" x) exit");
Serial.println();
@ -215,10 +215,9 @@ void set_resolution()
// update resolution
if( value == '1' ) {
flowSensor.set_resolution(ADNS3080_RESOLUTION_1200);
flowSensor.set_resolution(ADNS3080_RESOLUTION_1600);
}
if( value == '4' ) {
Serial.println("you want 1200!");
flowSensor.set_resolution(ADNS3080_RESOLUTION_400);
}