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 e6aee7963f 17fbbb4e7b
commit 47e88b1f91
12 changed files with 151 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
ArduCopter/ArduCopter.pde
View File

@ -151,10 +151,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
#if HIL_MODE == HIL_MODE_DISABLED #if HIL_MODE == HIL_MODE_DISABLED
// real sensors // real sensors
#if CONFIG_ADC == ENABLED #if CONFIG_ADC == ENABLED
AP_ADC_ADS7844 adc; AP_ADC_ADS7844 adc;
#endif #endif
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
APM_BMP085_HIL_Class barometer; APM_BMP085_HIL_Class barometer;
@ -164,35 +164,35 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
AP_Compass_HMC5843 compass(Parameters::k_param_compass); AP_Compass_HMC5843 compass(Parameters::k_param_compass);
#endif #endif
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
AP_OpticalFlow_ADNS3080 optflow; AP_OpticalFlow_ADNS3080 optflow;
#endif #endif
// real GPS selection // real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA #elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1); AP_GPS_NMEA g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF #elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1); AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX #elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1); AP_GPS_UBLOX g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1); AP_GPS_MTK g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16 #elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1); AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE #elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL); AP_GPS_None g_gps_driver(NULL);
#else #else
#error Unrecognised GPS_PROTOCOL setting. #error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL #endif // GPS PROTOCOL
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 #if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN ); AP_InertialSensor_MPU6000 ins( CONFIG_MPU6000_CHIP_SELECT_PIN );
@ -246,19 +246,16 @@ GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3);
// //
ModeFilter sonar_mode_filter; ModeFilter sonar_mode_filter;
#if CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
#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, #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN);
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN #endif
AP_AnalogSource_Arduino sonar_analog_source(CONFIG_SONAR_SOURCE_ANALOG_PIN); #if SONAR_TYPE == MAX_SONAR_XL
#endif AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter);
#else
#if SONAR_TYPE == MAX_SONAR_XL #error Unrecognised SONAR_TYPE setting.
AP_RangeFinder_MaxsonarXL sonar(&sonar_analog_source, &sonar_mode_filter); #endif
#else
#error Unrecognised SONAR_TYPE setting.
#endif
#endif #endif
// agmatthews USERHOOKS // agmatthews USERHOOKS
@ -560,7 +557,7 @@ static bool new_radio_frame;
AP_Relay relay; AP_Relay relay;
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
static bool usb_connected; static bool usb_connected;
#endif #endif
@ -748,7 +745,7 @@ static void medium_loop()
//------------------------------------------------- //-------------------------------------------------
case 3: case 3:
medium_loopCounter++; medium_loopCounter++;
// test
// perform next command // perform next command
// -------------------- // --------------------
if(control_mode == AUTO){ if(control_mode == AUTO){
@ -913,8 +910,8 @@ static void slow_loop()
update_lights(); update_lights();
// send all requested output streams with rates requested // send all requested output streams with rates requested
// between 1 and 5 Hz // between 3 and 5 Hz
gcs_data_stream_send(1,5); gcs_data_stream_send(3,5);
if(g.radio_tuning > 0) if(g.radio_tuning > 0)
tuning(); tuning();
@ -923,9 +920,9 @@ static void slow_loop()
update_motor_leds(); update_motor_leds();
#endif #endif
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
check_usb_mux(); check_usb_mux();
#endif #endif
break; break;
default: default:
@ -947,6 +944,7 @@ static void super_slow_loop()
Log_Write_Current(); Log_Write_Current();
gcs_send_message(MSG_HEARTBEAT); gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS // agmatthews - USERHOOKS
#ifdef USERHOOK_SUPERSLOWLOOP #ifdef USERHOOK_SUPERSLOWLOOP
USERHOOK_SUPERSLOWLOOP USERHOOK_SUPERSLOWLOOP
@ -1269,7 +1267,7 @@ static void read_AHRS(void)
#endif #endif
dcm.update_DCM_fast(); dcm.update_DCM_fast();
omega = dcm.get_gyro(); omega = imu.get_gyro();
} }
static void update_trig(void){ 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 -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static int static int
get_stabilize_roll(int32_t target_angle) get_stabilize_roll(int32_t target_angle)
{ {
int32_t error; int32_t error;
int32_t rate; int32_t rate;
// angle error
error = wrap_180(target_angle - dcm.roll_sensor); error = wrap_180(target_angle - dcm.roll_sensor);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// desired Rate: // conver to desired Rate:
rate = g.pi_stabilize_roll.get_pi(error, G_Dt); 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 #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: error = rate - (omega.x * DEGX100);
error = rate - (degrees(omega.x) * 100.0);
rate = g.pi_rate_roll.get_pi(error, G_Dt); rate = g.pi_rate_roll.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif #endif
// output control: // output control:
return (int)constrain(rate, -2500, 2500); rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
} }
static int static int
@ -31,24 +37,29 @@ get_stabilize_pitch(int32_t target_angle)
int32_t error; int32_t error;
int32_t rate; int32_t rate;
// angle error
error = wrap_180(target_angle - dcm.pitch_sensor); error = wrap_180(target_angle - dcm.pitch_sensor);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// desired Rate: // conver to desired Rate:
rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); 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 #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
// Rate P: error = rate - (omega.y * DEGX100);
error = rate - (degrees(omega.y) * 100.0);
rate = g.pi_rate_pitch.get_pi(error, G_Dt); rate = g.pi_rate_pitch.get_pi(error, G_Dt);
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
#endif #endif
// output control: // output control:
return (int)constrain(rate, -2500, 2500); rate = constrain(rate, -2500, 2500);
return (int)rate + iterm;
} }

8
ArduCopter/GCS_Mavlink.pde
View File

@ -1722,9 +1722,9 @@ static void mavlink_delay(unsigned long t)
gcs_update(); gcs_update();
} }
delay(1); delay(1);
#if USB_MUX_PIN > 0 #if USB_MUX_PIN > 0
check_usb_mux(); check_usb_mux();
#endif #endif
} while (millis() - tstart < t); } while (millis() - tstart < t);
in_mavlink_delay = false; in_mavlink_delay = false;
@ -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); vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap); va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); if (gcs3.initialised) {
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}
} }

62
ArduCopter/config.h
View File

@ -369,8 +369,6 @@
#ifndef THROTTLE_FAILSAFE_ACTION #ifndef THROTTLE_FAILSAFE_ACTION
# define THROTTLE_FAILSAFE_ACTION 2 # define THROTTLE_FAILSAFE_ACTION 2
#endif #endif
#ifndef MINIMUM_THROTTLE #ifndef MINIMUM_THROTTLE
# define MINIMUM_THROTTLE 130 # define MINIMUM_THROTTLE 130
#endif #endif
@ -485,11 +483,11 @@
// and charachteristics changes. // and charachteristics changes.
#ifdef MOTORS_JD880 #ifdef MOTORS_JD880
# define STABILIZE_ROLL_P 3.6 # define STABILIZE_ROLL_P 3.6
# define STABILIZE_ROLL_I 0.06 # define STABILIZE_ROLL_I 0.08
# define STABILIZE_ROLL_IMAX 2.0 // degrees # define STABILIZE_ROLL_IMAX 40.0 // degrees
# define STABILIZE_PITCH_P 3.6 # define STABILIZE_PITCH_P 3.6
# define STABILIZE_PITCH_I 0.06 # define STABILIZE_PITCH_I 0.08
# define STABILIZE_PITCH_IMAX 2.0 // degrees # define STABILIZE_PITCH_IMAX 40.0 // degrees
#endif #endif
// Jasons default values that are good for smaller payload motors. // Jasons default values that are good for smaller payload motors.
@ -497,20 +495,20 @@
# define STABILIZE_ROLL_P 4.6 # define STABILIZE_ROLL_P 4.6
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.08
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 1.5 // degrees # define STABILIZE_ROLL_IMAX 40 // degrees
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.6 # define STABILIZE_PITCH_P 4.6
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.08
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 1.5 // degrees # define STABILIZE_PITCH_IMAX 40 // degrees
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -583,15 +581,6 @@
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
// how much to we pitch towards the target
#ifndef PITCH_MAX
# define PITCH_MAX 22 // degrees
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Navigation control gains // Navigation control gains
// //
@ -670,15 +659,20 @@
# define DEBUG_LEVEL SEVERITY_LOW # define DEBUG_LEVEL SEVERITY_LOW
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Dataflash logging control // Dataflash logging control
// //
#ifdef LOGGING_ENABLED
#ifndef LOGGING_ENABLED #undef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif #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 #ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED # define LOG_ATTITUDE_FAST DISABLED
#endif #endif
@ -723,17 +717,17 @@
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0) #define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \ #define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \ LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \ LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \ LOGBIT(GPS) | \
LOGBIT(PM) | \ LOGBIT(PM) | \
LOGBIT(CTUN) | \ LOGBIT(CTUN) | \
LOGBIT(NTUN) | \ LOGBIT(NTUN) | \
LOGBIT(MODE) | \ LOGBIT(MODE) | \
LOGBIT(RAW) | \ LOGBIT(RAW) | \
LOGBIT(CMD) | \ LOGBIT(CMD) | \
LOGBIT(CUR) | \ LOGBIT(CUR) | \
LOGBIT(MOTORS) | \ LOGBIT(MOTORS) | \
LOGBIT(OPTFLOW) LOGBIT(OPTFLOW)
// if we are using fast, Disable Medium // if we are using fast, Disable Medium

6
ArduCopter/control_modes.pde
View File

@ -14,9 +14,9 @@ static void read_control_switch()
set_mode(flight_modes[switchPosition]); set_mode(flight_modes[switchPosition]);
if(g.ch7_option == CH7_SIMPLE_MODE){ if(g.ch7_option != CH7_SIMPLE_MODE){
// setup Simple mode // set Simple mode using stored paramters from Mission planner
// do we enable simple mode? // rather than by the control switch
do_simple = (g.simple_modes & (1 << switchPosition)); do_simple = (g.simple_modes & (1 << switchPosition));
} }
}else{ }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 //#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
// RADIANS // RADIANS
#define RADX100 0.000174533 #define RADX100 0.000174532925
#define DEGX100 5729.57795
// EEPROM addresses // EEPROM addresses
#define EEPROM_MAX_ADDR 4096 #define EEPROM_MAX_ADDR 4096

3
ArduCopter/system.pde
View File

@ -343,10 +343,11 @@ static void init_ardupilot()
reset_control_switch(); reset_control_switch();
#if HIL_MODE != HIL_MODE_ATTITUDE #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.ki_roll_pitch(0.00001278), // 50 hz I term
dcm.kp_yaw(0.08); dcm.kp_yaw(0.08);
dcm.ki_yaw(0.00004); dcm.ki_yaw(0.00004);
dcm._clamp = 5;
#endif #endif
// init the Z damopener // 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_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(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_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); static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); 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 #if CONFIG_SONAR == ENABLED
{"sonar", test_sonar}, {"sonar", test_sonar},
#endif #endif
//{"compass", test_mag}, {"compass", test_mag},
#ifdef OPTFLOW_ENABLED #ifdef OPTFLOW_ENABLED
{"optflow", test_optflow}, {"optflow", test_optflow},
#endif #endif
@ -125,7 +125,8 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
return(0); return(0);
} }
/*static int8_t /*
//static int8_t
//test_radio_pwm(uint8_t argc, const Menu::arg *argv) //test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); 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) //test_tri(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); 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) //test_nav(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); 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) //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) //test_stabilize(uint8_t argc, const Menu::arg *argv)
{ {
static byte ts_num; static byte ts_num;
@ -404,7 +407,7 @@ static int8_t
/*#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED /*#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_ADC == ENABLED
static int8_t //static int8_t
//test_adc(uint8_t argc, const Menu::arg *argv) //test_adc(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
@ -538,12 +541,6 @@ test_dcm_eulers(uint8_t argc, const Menu::arg *argv)
// IMU // IMU
// --- // ---
read_AHRS(); 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++; medium_loopCounter++;
if(medium_loopCounter == 4){ if(medium_loopCounter == 4){
@ -606,8 +603,8 @@ test_gps(uint8_t argc, const Menu::arg *argv)
} }
/* /*
static int8_t //static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv) //test_dcm(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
@ -657,8 +654,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
} }
*/ */
/* /*
static int8_t //static int8_t
test_dcm(uint8_t argc, const Menu::arg *argv) //test_dcm(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
@ -681,7 +678,7 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
*/ */
/*static int8_t /*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; static byte ts_num;
float old_yaw; 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) //test_relay(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
@ -894,9 +891,9 @@ test_baro(uint8_t argc, const Menu::arg *argv)
} }
#endif #endif
/*
static int8_t 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) { if(g.compass_enabled) {
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@ -924,7 +921,7 @@ static int8_t
return (0); return (0);
} }
} }
*/
/* /*
static int8_t static int8_t
//test_reverse(uint8_t argc, const Menu::arg *argv) //test_reverse(uint8_t argc, const Menu::arg *argv)

4
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); vsnprintf((char *)pending_status.text, sizeof(pending_status.text), fmtstr, ap);
va_end(ap); va_end(ap);
mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0); mavlink_send_message(MAVLINK_COMM_0, MSG_STATUSTEXT, 0);
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); if (gcs3.initialised) {
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
}
} }

15
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 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 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) // Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) // 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 // 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); _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1);
@ -346,7 +349,7 @@ AP_DCM::drift_correction(void)
// Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second // Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second
integrator_magnitude = _omega_I.length(); integrator_magnitude = _omega_I.length();
if (integrator_magnitude > radians(30)) { if (integrator_magnitude > radians(30)) {
_omega_I *= (radians(30) / integrator_magnitude); _omega_I *= (radians(30) / integrator_magnitude);
} }
//Serial.print("*"); //Serial.print("*");
} }
@ -379,18 +382,16 @@ AP_DCM::euler_rp(void)
{ {
pitch = -asin(_dcm_matrix.c.x); pitch = -asin(_dcm_matrix.c.x);
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
roll_sensor = degrees(roll) * 100; roll_sensor = roll * DEGX100; //degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100; pitch_sensor = pitch * DEGX100; //degrees(pitch) * 100;
} }
void void
AP_DCM::euler_yaw(void) AP_DCM::euler_yaw(void)
{ {
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); 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) if (yaw_sensor < 0)
yaw_sensor += 36000; yaw_sensor += 36000;
} }

6
libraries/AP_DCM/AP_DCM.h
View File

@ -35,7 +35,8 @@ public:
_ki_roll_pitch(0.00001278), _ki_roll_pitch(0.00001278),
_kp_yaw(0.8), // .8 _kp_yaw(0.8), // .8
_ki_yaw(0.00004), // 0.00004 _ki_yaw(0.00004), // 0.00004
_toggle(0) _toggle(0),
_clamp(3)
{} {}
// Accessors // Accessors
@ -44,7 +45,7 @@ public:
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();}
Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values
float get_health(void) {return _health;} float get_health(void) {return _health;}
void set_centripetal(bool b) {_centripetal = b;} void set_centripetal(bool b) {_centripetal = b;}
bool get_centripetal(void) {return _centripetal;} bool get_centripetal(void) {return _centripetal;}
@ -81,6 +82,7 @@ public:
static const float kDCM_kp_rp_high = 0.15; static const float kDCM_kp_rp_high = 0.15;
static const float kDCM_kp_rp_medium = 0.05967; static const float kDCM_kp_rp_medium = 0.05967;
static const float kDCM_kp_rp_low = 0.01; static const float kDCM_kp_rp_low = 0.01;
int8_t _clamp;
private: private:

10
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -13,6 +13,7 @@
#include "AP_GPS_MTK16.h" #include "AP_GPS_MTK16.h"
#include <stdint.h> #include <stdint.h>
#include <wiring.h>
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s) AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
@ -141,6 +142,15 @@ restart:
parsed = true; 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! /* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) { if(!_offset_calculated && parsed) {
long tempd1 = date; long tempd1 = date;