mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
James Goppert
commit
47e88b1f91
|
|
@ -151,10 +151,10 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
|||
|
||||
#if HIL_MODE == HIL_MODE_DISABLED
|
||||
|
||||
// real sensors
|
||||
#if CONFIG_ADC == ENABLED
|
||||
// real sensors
|
||||
#if CONFIG_ADC == ENABLED
|
||||
AP_ADC_ADS7844 adc;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef DESKTOP_BUILD
|
||||
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);
|
||||
#endif
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
#endif
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
|
||||
AP_GPS_NMEA g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
|
||||
AP_GPS_SIRF g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
|
||||
AP_GPS_UBLOX g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
|
||||
AP_GPS_MTK g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
|
||||
AP_GPS_MTK16 g_gps_driver(&Serial1);
|
||||
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||
AP_GPS_None g_gps_driver(NULL);
|
||||
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
|
||||
AP_GPS_None g_gps_driver(NULL);
|
||||
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
#endif // GPS PROTOCOL
|
||||
#else
|
||||
#error Unrecognised GPS_PROTOCOL setting.
|
||||
#endif // GPS PROTOCOL
|
||||
|
||||
#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000
|
||||
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;
|
||||
#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);
|
||||
#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
|
||||
#error Unrecognised SONAR_TYPE setting.
|
||||
#endif
|
||||
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
|
||||
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
|
||||
#error Unrecognised SONAR_TYPE setting.
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// agmatthews USERHOOKS
|
||||
|
|
@ -560,7 +557,7 @@ static bool new_radio_frame;
|
|||
AP_Relay relay;
|
||||
|
||||
#if USB_MUX_PIN > 0
|
||||
static bool usb_connected;
|
||||
static bool usb_connected;
|
||||
#endif
|
||||
|
||||
|
||||
|
|
@ -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();
|
||||
|
|
@ -923,9 +920,9 @@ static void slow_loop()
|
|||
update_motor_leds();
|
||||
#endif
|
||||
|
||||
#if USB_MUX_PIN > 0
|
||||
#if USB_MUX_PIN > 0
|
||||
check_usb_mux();
|
||||
#endif
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -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){
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1722,9 +1722,9 @@ static void mavlink_delay(unsigned long t)
|
|||
gcs_update();
|
||||
}
|
||||
delay(1);
|
||||
#if USB_MUX_PIN > 0
|
||||
#if USB_MUX_PIN > 0
|
||||
check_usb_mux();
|
||||
#endif
|
||||
#endif
|
||||
} while (millis() - tstart < t);
|
||||
|
||||
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);
|
||||
va_end(ap);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -723,17 +717,17 @@
|
|||
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
|
||||
|
||||
#define DEFAULT_LOG_BITMASK \
|
||||
LOGBIT(ATTITUDE_FAST) | \
|
||||
LOGBIT(ATTITUDE_MED) | \
|
||||
LOGBIT(GPS) | \
|
||||
LOGBIT(PM) | \
|
||||
LOGBIT(CTUN) | \
|
||||
LOGBIT(NTUN) | \
|
||||
LOGBIT(MODE) | \
|
||||
LOGBIT(RAW) | \
|
||||
LOGBIT(CMD) | \
|
||||
LOGBIT(CUR) | \
|
||||
LOGBIT(MOTORS) | \
|
||||
LOGBIT(ATTITUDE_FAST) | \
|
||||
LOGBIT(ATTITUDE_MED) | \
|
||||
LOGBIT(GPS) | \
|
||||
LOGBIT(PM) | \
|
||||
LOGBIT(CTUN) | \
|
||||
LOGBIT(NTUN) | \
|
||||
LOGBIT(MODE) | \
|
||||
LOGBIT(RAW) | \
|
||||
LOGBIT(CMD) | \
|
||||
LOGBIT(CUR) | \
|
||||
LOGBIT(MOTORS) | \
|
||||
LOGBIT(OPTFLOW)
|
||||
|
||||
// if we are using fast, Disable Medium
|
||||
|
|
|
|||
|
|
@ -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{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
|
||||
if (gcs3.initialised) {
|
||||
mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
integrator_magnitude = _omega_I.length();
|
||||
if (integrator_magnitude > radians(30)) {
|
||||
_omega_I *= (radians(30) / integrator_magnitude);
|
||||
_omega_I *= (radians(30) / integrator_magnitude);
|
||||
}
|
||||
//Serial.print("*");
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -44,7 +45,7 @@ public:
|
|||
Matrix3f get_dcm_matrix(void) {return _dcm_matrix; }
|
||||
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
|
||||
|
||||
|
||||
float get_health(void) {return _health;}
|
||||
void set_centripetal(bool b) {_centripetal = b;}
|
||||
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_medium = 0.05967;
|
||||
static const float kDCM_kp_rp_low = 0.01;
|
||||
int8_t _clamp;
|
||||
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue