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:
James Goppert 2011-11-01 21:58:10 -04:00
parent 04d4aa4310 40c87dd295
commit 8058154f61
12 changed files with 112 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
ArduCopter/ArduCopter.pde
View File

@ -664,6 +664,7 @@ static void medium_loop()
}else{
g_gps->new_data = false;
}
break;
// command processing
@ -729,6 +730,11 @@ static void medium_loop()
// -----------------------
arm_motors();
// Do an extra baro read
// ---------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
barometer.Read();
#endif
slow_loop();
break;
@ -1066,7 +1072,15 @@ void update_throttle_mode(void)
g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle);
#else
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
if(manual_boost != 0){
//remove alt_hold_velocity when implemented
g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + alt_hold_velocity();
// reset next_WP.alt
next_WP.alt = max(current_loc.alt, 100);
}else{
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + alt_hold_velocity();
}
#endif
break;
}
@ -1204,7 +1218,7 @@ static void update_altitude()
float scale;
// read barometer
baro_alt = read_barometer();
baro_alt = (baro_alt + read_barometer()) >> 1;
if(baro_alt < 1000){
@ -1228,26 +1242,18 @@ static void update_altitude()
current_loc.alt = baro_alt + home.alt;
}
// calc the accel rate limit to 2m/s
altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer
// rate limiter to reduce some of the motor pulsing
if (altitude_rate > 0){
// going up
altitude_rate = min(altitude_rate, old_rate + 20);
}else{
// going down
altitude_rate = max(altitude_rate, old_rate - 20);
}
old_rate = altitude_rate;
old_altitude = current_loc.alt;
// calc the vertical accel rate
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale
altitude_rate = (temp_rate - old_rate) * 10;
old_rate = temp_rate;
#endif
}
static void
adjust_altitude()
{
/*
// old vert control
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
@ -1258,6 +1264,25 @@ adjust_altitude()
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
}*/
if(g.rc_3.control_in <= 180){
// 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();
}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();
}else {
manual_boost = 0;
}
}
@ -1339,7 +1364,7 @@ static void tuning(){
g.pi_nav_lat.kP(tuning_value);
g.pi_nav_lon.kP(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
g.rc_6.set_range(1000,2000);

4
ArduCopter/Attitude.pde
View File

@ -83,7 +83,7 @@ get_stabilize_yaw(long target_angle)
return (int)constrain(rate, -2500, 2500);
}
#define ALT_ERROR_MAX 500
#define ALT_ERROR_MAX 300
static int
get_nav_throttle(long z_error)
{
@ -94,7 +94,7 @@ get_nav_throttle(long z_error)
rate_error = rate_error - altitude_rate;
// limit the rate
rate_error = constrain(rate_error, -120, 140);
rate_error = constrain(rate_error, -80, 140);
return (int)g.pi_throttle.get_pi(rate_error, .1);
}

19
ArduCopter/Log.pde
View File

@ -660,10 +660,13 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteInt(nav_throttle); //7
DataFlash.WriteInt(angle_boost); //8
DataFlash.WriteInt(manual_boost); //9
//DataFlash.WriteInt((int)(accels_rot.z * 1000)); //10
DataFlash.WriteInt((int)(barometer.RawPress - barometer._offset_press)); //9
DataFlash.WriteInt(g.rc_3.servo_out); //10
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11
DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12
DataFlash.WriteInt(g.rc_3.servo_out); //11
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //12
DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //13
DataFlash.WriteByte(END_BYTE);
}
@ -672,8 +675,8 @@ static void Log_Write_Control_Tuning()
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
// 1 2 3 4 5 6 7 8 9 10 11 12
Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
// 1 2 3 4 5 6 7 8 9 10 11 12 13
Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
// Control
//DataFlash.ReadByte(),
@ -692,10 +695,12 @@ static void Log_Read_Control_Tuning()
DataFlash.ReadInt(), //7
DataFlash.ReadInt(), //8
DataFlash.ReadInt(), //9
DataFlash.ReadInt(), //10
//(float)DataFlash.ReadInt() / 1000, //10
DataFlash.ReadInt(), //11
DataFlash.ReadInt()); //12
DataFlash.ReadInt(), //12
DataFlash.ReadInt()); //13
}
// Write a performance monitoring packet. Total length : 19 bytes

8
ArduCopter/config.h
View File

@ -509,10 +509,10 @@
#endif
#ifndef THR_HOLD_P
# define THR_HOLD_P 0.5 //
# define THR_HOLD_P 0.4 //
#endif
#ifndef THR_HOLD_I
# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup
# define THR_HOLD_I 0.02 // with 4m error, 12.5s windup
#endif
#ifndef THR_HOLD_IMAX
# define THR_HOLD_IMAX 300
@ -520,10 +520,10 @@
// RATE control
#ifndef THROTTLE_P
# define THROTTLE_P 0.8 //
# define THROTTLE_P 1.0 //
#endif
#ifndef THROTTLE_I
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
# define THROTTLE_I 0.0 //
#endif
#ifndef THROTTLE_IMAX
# define THROTTLE_IMAX 50

1
ArduCopter/system.pde
View File

@ -512,6 +512,7 @@ init_throttle_cruise()
// 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();
#if FRAME_CONFIG == HELI_FRAME
g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in));
#else

11
ArduCopter/test.pde
View File

@ -429,7 +429,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
while(1){
//delay(20);
if (millis() - fast_loopTimer >= 5) {
if (millis() - fast_loopTimer >=5) {
// IMU
// ---
@ -795,9 +795,16 @@ test_baro(uint8_t argc, const Menu::arg *argv)
init_barometer();
while(1){
delay(100);
barometer.Read();
delay(100);
baro_alt = read_barometer();
Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt);
int temp_rate = (barometer._offset_press - barometer.RawPress) << 1;
altitude_rate = (temp_rate - old_rate) * 10;
old_rate = temp_rate;
// 1 2 3 4 5 1 2 3 4 5
Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, altitude_rate, barometer.RawTemp, barometer.RawPress, temp_rate);
//Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2);
if(Serial.available() > 0){
return (0);

9
Tools/autotest/ArduCopter.parm
View File

@ -29,12 +29,3 @@ FLTMODE3 2
FLTMODE4 6
FLTMODE5 5
FLTMODE6 0
NAV_LAT_I 0
NAV_LON_I 0
NAV_LAT_P 1
NAV_LON_P 1
STB_PIT_P 2
STB_RLL_P 2
XTRACK_P 1
RATE_PIT_P 0.1
RATE_RLL_P 0.1

14
Tools/autotest/arducopter.py
View File

@ -10,6 +10,8 @@ import mavutil
HOME_LOCATION='-35.362938,149.165085,650,270'
homeloc = None
# a list of pexpect objects to read while waiting for
# messages. This keeps the output to stdout flowing
expect_list = []
@ -229,9 +231,9 @@ def land(mavproxy, mav, timeout=60):
return False
def fly_mission(mavproxy, mav, filename, timeout=120):
def fly_mission(mavproxy, mav, filename):
'''fly a mission from a file'''
startloc = current_location(mav)
global homeloc
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
@ -239,7 +241,7 @@ def fly_mission(mavproxy, mav, filename, timeout=120):
mavproxy.send('switch 1\n') # auto mode
mavproxy.expect('AUTO>')
wait_distance(mav, 30, timeout=120)
wait_location(mav, startloc, timeout=300)
wait_location(mav, homeloc, timeout=600)
def setup_rc(mavproxy):
@ -252,7 +254,7 @@ def setup_rc(mavproxy):
def fly_ArduCopter():
'''fly ArduCopter in SIL'''
global expect_list
global expect_list, homeloc
util.rmfile('eeprom.bin')
sil = util.start_SIL('ArduCopter')
@ -298,14 +300,16 @@ def fly_ArduCopter():
failed = False
try:
mav.wait_heartbeat()
mav.recv_match(type='GPS_RAW')
mav.recv_match(type='GPS_RAW', blocking=True)
setup_rc(mavproxy)
homeloc = current_location(mav)
arm_motors(mavproxy)
takeoff(mavproxy, mav)
fly_square(mavproxy, mav)
loiter(mavproxy, mav)
land(mavproxy, mav)
fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"))
land(mavproxy, mav)
disarm_motors(mavproxy)
except pexpect.TIMEOUT, e:
failed = True

2
Tools/autotest/util.py
View File

@ -50,7 +50,7 @@ def build_SIL(atype):
def start_SIL(atype):
'''launch a SIL instance'''
ret = pexpect.spawn('tmp/%s.build/%s.elf' % (atype, atype),
ret = pexpect.spawn(reltopdir('tmp/%s.build/%s.elf' % (atype, atype)),
logfile=sys.stdout, timeout=5)
ret.expect('Waiting for connection')
return ret

26
libraries/APM_BMP085/APM_BMP085.cpp
View File

@ -95,7 +95,7 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
BMP085_State = 1;
}
/*
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_Class::Read()
@ -130,6 +130,30 @@ uint8_t APM_BMP085_Class::Read()
}
return(result);
}
*/
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_Class::Read()
{
uint8_t result = 0;
if (BMP085_State == 1){
if (digitalRead(BMP085_EOC)){
BMP085_State = 2;
ReadTemp(); // On state 1 we read temp
Command_ReadPress();
}
}else{
if (digitalRead(BMP085_EOC)){
BMP085_State = 1; // Start again from state = 1
ReadPress();
Calculate();
Command_ReadTemp(); // Read Temp
result = 1; // New pressure reading
}
}
return(result);
}
// Send command to Read Pressure

6
libraries/APM_BMP085/APM_BMP085.h
View File

@ -1,8 +1,8 @@
#ifndef APM_BMP085_h
#define APM_BMP085_h
#define TEMP_FILTER_SIZE 16
#define PRESS_FILTER_SIZE 10
#define TEMP_FILTER_SIZE 2
#define PRESS_FILTER_SIZE 2
#include "APM_BMP085_hil.h"
@ -13,6 +13,7 @@ class APM_BMP085_Class
_temp_index(0),
_press_index(0){}; // Constructor
int32_t RawPress;
int32_t _offset_press;
int32_t RawTemp;
int16_t Temp;
int32_t Press;
@ -32,7 +33,6 @@ class APM_BMP085_Class
int _temp_filter[TEMP_FILTER_SIZE];
int _press_filter[PRESS_FILTER_SIZE];
long _offset_press;
long _offset_temp;
uint8_t _temp_index;

4
libraries/Desktop/support/FastSerial.cpp
View File

@ -45,6 +45,7 @@
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include "desktop.h"
#define LISTEN_BASE_PORT 5760
@ -131,6 +132,7 @@ static void tcp_start_connection(unsigned int serial_port, bool wait_for_connect
fprintf(stderr, "accept() error - %s", strerror(errno));
exit(1);
}
setsockopt(s->fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one));
s->connected = true;
}
}
@ -170,7 +172,9 @@ static void check_connection(struct tcp_state *s)
if (select_check(s->listen_fd)) {
s->fd = accept(s->listen_fd, NULL, NULL);
if (s->fd != -1) {
int one = 1;
s->connected = true;
setsockopt(s->fd, SOL_TCP, TCP_NODELAY, &one, sizeof(one));
printf("New connection on serial port %u\n", s->serial_port);
}
}