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-12-04 00:45:34 -05:00
parent fd77fe5a9d fa7f39ba20
commit f8fab5f7d6
12 changed files with 151 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/ArduCopter.pde
View File

@ -246,14 +246,11 @@ GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
//
ModeFilter sonar_mode_filter;
#if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
AP_AnalogSource_ADC sonar_analog_source( &adc,
CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
AP_AnalogSource_ADC sonar_analog_source( &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
#endif
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
#else
@ -748,7 +745,7 @@ static void medium_loop()
//-------------------------------------------------
case 3:
medium_loopCounter++;
// test
// perform next command
// --------------------
if(control_mode == AUTO){
@ -913,8 +910,8 @@ static void slow_loop()
update_lights();
// send all requested output streams with rates requested
// between 1 and 5 Hz
gcs_data_stream_send(1,5);
// between 3 and 5 Hz
gcs_data_stream_send(3,5);
if(g.radio_tuning > 0)
tuning();
@ -947,6 +944,7 @@ static void super_slow_loop()
Log_Write_Current();
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS
#ifdef USERHOOK_SUPERSLOWLOOP
USERHOOK_SUPERSLOWLOOP
@ -1269,7 +1267,7 @@ static void read_AHRS(void)
#endif
dcm.update_DCM_fast();
omega = dcm.get_gyro();
omega = imu.get_gyro();
}
static void update_trig(void){

35
ArduCopter/Attitude.pde
View File

@ -1,28 +1,34 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static int
get_stabilize_roll(int32_t target_angle)
{
int32_t error;
int32_t rate;
// angle error
error = wrap_180(target_angle - dcm.roll_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// desired Rate:
// conver to desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_integrator();
// remove iterm from PI output
rate -= iterm;
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
error = rate - (degrees(omega.x) * 100.0);
error = rate - (omega.x * DEGX100);
rate = g.pi_rate_roll.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control:
return (int)constrain(rate, -2500, 2500);
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
}
static int
@ -31,24 +37,29 @@ get_stabilize_pitch(int32_t target_angle)
int32_t error;
int32_t rate;
// angle error
error = wrap_180(target_angle - dcm.pitch_sensor);
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// desired Rate:
// conver to desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate);
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_stabilize_roll.get_integrator();
// remove iterm from PI output
rate -= iterm;
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P:
error = rate - (degrees(omega.y) * 100.0);
error = rate - (omega.y * DEGX100);
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif
// output control:
return (int)constrain(rate, -2500, 2500);
rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
}

2
ArduCopter/GCS_Mavlink.pde
View File

@ -1799,5 +1799,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
if (gcs3.initialised) {
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}
}

40
ArduCopter/config.h
View File

@ -369,8 +369,6 @@
#ifndef THROTTLE_FAILSAFE_ACTION
# define THROTTLE_FAILSAFE_ACTION 2
#endif
#ifndef MINIMUM_THROTTLE
# define MINIMUM_THROTTLE 130
#endif
@ -485,11 +483,11 @@
// and charachteristics changes.
#ifdef MOTORS_JD880
# define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_I 0.06
# define STABILIZE_ROLL_IMAX 2.0 // degrees
# define STABILIZE_ROLL_I 0.08
# define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 3.6
# define STABILIZE_PITCH_I 0.06
# define STABILIZE_PITCH_IMAX 2.0 // degrees
# define STABILIZE_PITCH_I 0.08
# define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif
// Jasons default values that are good for smaller payload motors.
@ -497,20 +495,20 @@
# define STABILIZE_ROLL_P 4.6
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.0
# define STABILIZE_ROLL_I 0.08
#endif
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 1.5 // degrees
# define STABILIZE_ROLL_IMAX 40 // degrees
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.6
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0
# define STABILIZE_PITCH_I 0.08
#endif
#ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 1.5 // degrees
# define STABILIZE_PITCH_IMAX 40 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
@ -583,15 +581,6 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
// how much to we pitch towards the target
#ifndef PITCH_MAX
# define PITCH_MAX 22 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
@ -670,15 +659,20 @@
# define DEBUG_LEVEL SEVERITY_LOW
#endif
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#ifdef LOGGING_ENABLED
#undef LOGGING_ENABLED
#endif
#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included
#define LOGGING_ENABLED ENABLED
#else
#define LOGGING_ENABLED DISABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif

6
ArduCopter/control_modes.pde
View File

@ -14,9 +14,9 @@ static void read_control_switch()
set_mode(flight_modes[switchPosition]);
if(g.ch7_option == CH7_SIMPLE_MODE){
// setup Simple mode
// do we enable simple mode?
if(g.ch7_option != CH7_SIMPLE_MODE){
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
do_simple = (g.simple_modes & (1 << switchPosition));
}
}else{

4
ArduCopter/defines.h
View File

@ -334,7 +334,9 @@ enum gcs_severity {
//#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
// RADIANS
#define RADX100 0.000174533
#define RADX100 0.000174532925
#define DEGX100 5729.57795
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096

3
ArduCopter/system.pde
View File

@ -343,10 +343,11 @@ static void init_ardupilot()
reset_control_switch();
#if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000);
dcm.kp_roll_pitch(0.130000);
dcm.ki_roll_pitch(0.00001278), // 50 hz I term
dcm.kp_yaw(0.08);
dcm.ki_yaw(0.00004);
dcm._clamp = 5;
#endif
// init the Z damopener

43
ArduCopter/test.pde
View File

@ -30,7 +30,7 @@ static int8_t test_current(uint8_t argc, const Menu::arg *argv);
//static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
//static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv);
@ -87,7 +87,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
#if CONFIG_SONAR == ENABLED
{"sonar", test_sonar},
#endif
//{"compass", test_mag},
{"compass", test_mag},
#ifdef OPTFLOW_ENABLED
{"optflow", test_optflow},
#endif
@ -125,7 +125,8 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
return(0);
}
/*static int8_t
/*
//static int8_t
//test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
@ -157,7 +158,8 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
}
}*/
/*static int8_t
/*
//static int8_t
//test_tri(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
@ -185,7 +187,7 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
}*/
/*
static int8_t
//static int8_t
//test_nav(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
@ -258,7 +260,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
}
/*
static int8_t
//static int8_t
//test_failsafe(uint8_t argc, const Menu::arg *argv)
{
@ -313,7 +315,8 @@ static int8_t
}
*/
/*static int8_t
/*
//static int8_t
//test_stabilize(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
@ -404,7 +407,7 @@ static int8_t
/*#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED
static int8_t
//static int8_t
//test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
@ -538,12 +541,6 @@ test_dcm_eulers(uint8_t argc, const Menu::arg *argv)
// IMU
// ---
read_AHRS();
//Vector3f accels = imu.get_accel();
//Vector3f gyros = imu.get_gyro();
//Vector3f accel_filt = imu.get_accel_filtered();
//accels_rot = dcm.get_dcm_matrix() * accel_filt;
medium_loopCounter++;
if(medium_loopCounter == 4){
@ -606,8 +603,8 @@ test_gps(uint8_t argc, const Menu::arg *argv)
}
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
//static int8_t
//test_dcm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -657,8 +654,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
}
*/
/*
static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv)
//static int8_t
//test_dcm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -681,7 +678,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
*/
/*static int8_t
test_omega(uint8_t argc, const Menu::arg *argv)
//test_omega(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
float old_yaw;
@ -782,7 +779,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
}
/*
static int8_t
//static int8_t
//test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
@ -894,9 +891,9 @@ test_baro(uint8_t argc, const Menu::arg *argv)
}
#endif
/*
static int8_t
//test_mag(uint8_t argc, const Menu::arg *argv)
test_mag(uint8_t argc, const Menu::arg *argv)
{
if(g.compass_enabled) {
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@ -924,7 +921,7 @@ static int8_t
return (0);
}
}
*/
/*
static int8_t
//test_reverse(uint8_t argc, const Menu::arg *argv)

2
ArduPlane/GCS_Mavlink.pde
View File

@ -2103,5 +2103,7 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...)
vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
if (gcs3.initialised) {
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}
}

13
libraries/AP_DCM/AP_DCM.cpp
View File

@ -1,3 +1,6 @@
#define RADX100 0.000174532925
#define DEGX100 5729.57795
/*
APM_DCM_FW.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
@ -274,7 +277,7 @@ AP_DCM::drift_correction(void)
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
accel_weight = constrain(1 - 3 * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0)
accel_weight = constrain(1 - _clamp * fabs(1 - accel_magnitude), 0, 1); // upped to (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0)
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1);
@ -379,18 +382,16 @@ AP_DCM::euler_rp(void)
{
pitch = -asin(_dcm_matrix.c.x);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
roll_sensor = roll * DEGX100; //degrees(roll) * 100;
pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100;
}
void
AP_DCM::euler_yaw(void)
{
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
yaw_sensor = degrees(yaw) * 100;
yaw_sensor = yaw * DEGX100; //degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
}

4
libraries/AP_DCM/AP_DCM.h
View File

@ -35,7 +35,8 @@ public:
_ki_roll_pitch(0.00001278),
_kp_yaw(0.8), // .8
_ki_yaw(0.00004), // 0.00004
_toggle(0)
_toggle(0),
_clamp(3)
{}
// Accessors
@ -81,6 +82,7 @@ public:
static const float kDCM_kp_rp_high = 0.15;
static const float kDCM_kp_rp_medium = 0.05967;
static const float kDCM_kp_rp_low = 0.01;
int8_t _clamp;
private:

10
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -13,6 +13,7 @@
#include "AP_GPS_MTK16.h"
#include <stdint.h>
#include <wiring.h>
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
@ -141,6 +142,15 @@ restart:
parsed = true;
#ifdef FAKE_GPS_LOCK_TIME
if (millis() > FAKE_GPS_LOCK_TIME*1000) {
fix = true;
latitude = -35000000UL;
longitude = 149000000UL;
altitude = 584;
}
#endif
/* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) {
long tempd1 = date;