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:
Chris Anderson 2012-02-18 09:17:17 -08:00
parent d3edd4ed59 e3c24fbce8
commit 520a29641e
50 changed files with 448 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
ArduCopter/ArduCopter.pde
View File

@ -1,8 +1,8 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.3.1"
#define THISFIRMWARE "ArduCopter V2.4"
/*
ArduCopter Version 2.3.1
ArduCopter Version 2.4
Authors: Jason Short
Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
@ -32,9 +32,9 @@ Oliver :Piezo support
Guntars :Arming safety suggestion
Igor van Airde :Control Law optimization
Jean-Louis Naudin :Auto Landing
Sandro Benigno : Camera support
Olivier Adler : PPM Encoder
John Arne Birkeland: PPM Encoder
Sandro Benigno :Camera support
Olivier Adler :PPM Encoder
John Arne Birkeland :PPM Encoder
And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
@ -659,11 +659,6 @@ static int32_t auto_pitch;
static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon;
// This may go away, but for now I'm tracking the desired bank before we apply the Wind compensation I term
// This is mainly for debugging
//static int16_t nav_lat_p;
//static int16_t nav_lon_p;
// The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll = 0;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
@ -829,7 +824,9 @@ void loop()
uint32_t timer = micros();
// We want this to execute fast
// ----------------------------
if ((timer - fast_loopTimer) >= 4500) {
if ((timer - fast_loopTimer) >= 4000) {
//Log_Write_Data(13, (int32_t)(timer - fast_loopTimer));
//PORTK |= B00010000;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer;
@ -1426,19 +1423,6 @@ void update_roll_pitch_mode(void)
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
break;
/* case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
update_simple_mode();
}
// mix in user control with Nav control
control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
break;
*/
case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform
if(do_simple && new_radio_frame){
@ -1942,11 +1926,12 @@ static void tuning(){
case CH6_DAMP:
g.stabilize_d.set(tuning_value);
break;
//tuning_value = (float)g.rc_6.control_in / 100000.0;
//g.rc_6.set_range(0,1000); // 0 to 1
//g.pid_rate_roll.kD(tuning_value);
//g.pid_rate_pitch.kD(tuning_value);
case CH6_RATE_KD:
tuning_value = (float)g.rc_6.control_in / 100000.0;
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_STABILIZE_KP:
@ -1956,26 +1941,21 @@ static void tuning(){
break;
case CH6_STABILIZE_KI:
//g.rc_6.set_range(0,300); // 0 to .3
//tuning_value = (float)g.rc_6.control_in / 1000.0;
g.pi_stabilize_roll.kI(tuning_value);
g.pi_stabilize_pitch.kI(tuning_value);
break;
case CH6_RATE_KP:
//g.rc_6.set_range(40,300); // 0 to .3
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_KI:
//g.rc_6.set_range(0,500); // 0 to .5
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_YAW_KP:
//g.rc_6.set_range(0,1000);
g.pi_stabilize_yaw.kP(tuning_value);
break;
@ -1985,70 +1965,58 @@ static void tuning(){
break;
case CH6_THROTTLE_KP:
//g.rc_6.set_range(0,1000); // 0 to 1
g.pid_throttle.kP(tuning_value);
break;
case CH6_TOP_BOTTOM_RATIO:
//g.rc_6.set_range(800,1000); // .8 to 1
g.top_bottom_ratio = tuning_value;
break;
case CH6_RELAY:
//g.rc_6.set_range(0,1000);
if (g.rc_6.control_in > 525) relay.on();
if (g.rc_6.control_in < 475) relay.off();
break;
case CH6_TRAVERSE_SPEED:
//g.rc_6.set_range(0,1000);
g.waypoint_speed_max = g.rc_6.control_in;
break;
case CH6_LOITER_P:
//g.rc_6.set_range(0,2000);
g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value);
break;
case CH6_NAV_P:
//g.rc_6.set_range(0,4000);
g.pid_nav_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value);
break;
case CH6_NAV_I:
//g.rc_6.set_range(0,500);
g.pid_nav_lat.kI(tuning_value);
g.pid_nav_lon.kI(tuning_value);
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
//g.rc_6.set_range(1000,2000);
g.heli_ext_gyro_gain = tuning_value * 1000;
break;
#endif
case CH6_THR_HOLD_KP:
//g.rc_6.set_range(0,1000); // 0 to 1
g.pi_alt_hold.kP(tuning_value);
break;
case CH6_OPTFLOW_KP:
//g.rc_6.set_range(0,5000); // 0 to 5
g.pid_optflow_roll.kP(tuning_value);
g.pid_optflow_pitch.kP(tuning_value);
break;
case CH6_OPTFLOW_KI:
//g.rc_6.set_range(0,10000); // 0 to 10
g.pid_optflow_roll.kI(tuning_value);
g.pid_optflow_pitch.kI(tuning_value);
break;
case CH6_OPTFLOW_KD:
//g.rc_6.set_range(0,200); // 0 to 0.2
g.pid_optflow_roll.kD(tuning_value);
g.pid_optflow_pitch.kD(tuning_value);
break;

43
ArduCopter/Attitude.pde
View File

@ -21,7 +21,7 @@ get_stabilize_roll(int32_t target_angle)
// limit the error we're feeding to the PID
target_angle = constrain(target_angle, -2500, 2500);
// conver to desired Rate:
// convert to desired Rate:
int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle);
int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt);
@ -107,16 +107,34 @@ get_acro_yaw(int32_t target_rate)
static int
get_rate_roll(int32_t target_rate)
{
int16_t rate_d1 = 0;
static int16_t rate_d2 = 0;
static int16_t rate_d3 = 0;
static int32_t last_rate = 0;
int32_t current_rate = (omega.x * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
last_rate = current_rate;
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500);
last_rate = current_rate;
//int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
//target_rate -= constrain(d_temp, -500, 500);
//last_rate = current_rate;
// D term
// I had tried this before with little result. Recently, someone mentioned to me that
// MultiWii uses a filter of the last three to get around noise and get a stronger signal.
// Works well! Thanks!
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
target_rate -= d_temp;
// output control:
return constrain(target_rate, -2500, 2500);
@ -125,16 +143,31 @@ get_rate_roll(int32_t target_rate)
static int
get_rate_pitch(int32_t target_rate)
{
int16_t rate_d1 = 0;
static int16_t rate_d2 = 0;
static int16_t rate_d3 = 0;
static int32_t last_rate = 0;
int32_t current_rate = (omega.y * DEGX100);
// History of last 3 dir
rate_d3 = rate_d2;
rate_d2 = rate_d1;
rate_d1 = current_rate - last_rate;
last_rate = current_rate;
// rate control
target_rate = target_rate - current_rate;
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// Dampening
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500);
last_rate = current_rate;
//int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
//target_rate -= constrain(d_temp, -500, 500);
//last_rate = current_rate;
// D term
int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d;
target_rate -= d_temp;
// output control:
return constrain(target_rate, -2500, 2500);

20
ArduCopter/Parameters.h
View File

@ -78,7 +78,7 @@ public:
k_param_heli_servo_averaging,
k_param_heli_servo_manual,
k_param_heli_phase_angle,
k_param_heli_coll_yaw_effect, // 97
k_param_heli_collective_yaw_effect, // 97
#endif
// 110: Telemetry control
@ -92,7 +92,7 @@ public:
//
// 140: Sensor parameters
//
k_param_IMU_calibration = 140,
k_param_imu = 140, // sensor calibration
k_param_battery_monitoring,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
@ -268,13 +268,13 @@ public:
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
AP_Int16 heli_roll_max, heli_pitch_max; // maximum allowed roll and pitch of swashplate
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch
AP_Int16 heli_collective_min, heli_collective_max, heli_collective_mid; // min and max collective. mid = main blades at zero pitch
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
AP_Int8 heli_servo_averaging; // 0 or 1 = no averaging (250hz) for **digital servos**, 2=average of two samples (125hz), 3=three samples (83.3hz) **analog servos**, 4=four samples (62.5hz), 5=5 samples(50hz)
AP_Int8 heli_servo_manual; // 0 = normal mode, 1 = radio inputs directly control swash. required for swash set-up
AP_Int16 heli_phase_angle; // 0 to 360 degrees. specifies mixing between roll and pitch for helis
AP_Float heli_coll_yaw_effect; // -5.0 ~ 5.0. Feed forward control from collective to yaw. 1.0 = move rudder right 1% for every 1% of collective above the mid point
AP_Float heli_collective_yaw_effect; // -5.0 ~ 5.0. Feed forward control from collective to yaw. 1.0 = move rudder right 1% for every 1% of collective above the mid point
#endif
// RC channels
@ -377,24 +377,20 @@ public:
auto_slew_rate (AUTO_SLEW_RATE),
#if FRAME_CONFIG == HELI_FRAME
heli_servo_1 (k_param_heli_servo_1),
heli_servo_2 (k_param_heli_servo_2),
heli_servo_3 (k_param_heli_servo_3),
heli_servo_4 (k_param_heli_servo_4),
heli_servo1_pos (-60),
heli_servo2_pos (60),
heli_servo3_pos (180),
heli_roll_max (4500),
heli_pitch_max (4500),
heli_coll_min (1250),
heli_coll_max (1750),
heli_coll_mid (1500),
heli_collective_min (1250),
heli_collective_max (1750),
heli_collective_mid (1500),
heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350),
heli_servo_averaging (0),
heli_servo_manual (0),
heli_phase_angle (0),
heli_coll_yaw_effect (0),
heli_collective_yaw_effect (0),
#endif
camera_pitch_gain (CAM_PITCH_GAIN),

19
ArduCopter/Parameters.pde
View File

@ -72,24 +72,24 @@ static const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(auto_slew_rate, "AUTO_SLEW"),
#if FRAME_CONFIG == HELI_FRAME
GSCALAR(heli_servo_1, "HS1_"),
GSCALAR(heli_servo_2, "HS2_"),
GSCALAR(heli_servo_3, "HS3_"),
GSCALAR(heli_servo_4, "HS4_"),
GGROUP(heli_servo_1, "HS1_", RC_Channel),
GGROUP(heli_servo_2, "HS2_", RC_Channel),
GGROUP(heli_servo_3, "HS3_", RC_Channel),
GGROUP(heli_servo_4, "HS4_", RC_Channel),
GSCALAR(heli_servo1_pos, "SV1_POS_"),
GSCALAR(heli_servo2_pos, "SV2_POS_"),
GSCALAR(heli_servo3_pos, "SV3_POS_"),
GSCALAR(heli_roll_max, "ROL_MAX_"),
GSCALAR(heli_pitch_max, "PIT_MAX_"),
GSCALAR(heli_coll_min, "COL_MIN_"),
GSCALAR(heli_coll_max, "COL_MAX_"),
GSCALAR(heli_coll_mid, "COL_MID_"),
GSCALAR(heli_collective_min, "COL_MIN_"),
GSCALAR(heli_collective_max, "COL_MAX_"),
GSCALAR(heli_collective_mid, "COL_MID_"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
GSCALAR(heli_servo_averaging, "SV_AVG"),
GSCALAR(heli_servo_manual, "HSV_MAN"),
GSCALAR(heli_phase_angle, "H_PHANG"),
GSCALAR(heli_coll_yaw_effect, "H_COLYAW"),
GSCALAR(heli_collective_yaw_effect, "H_COLYAW"),
#endif
// RC channel
@ -142,7 +142,8 @@ static const AP_Param::Info var_info[] PROGMEM = {
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK)
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECT(imu, "IMU_", IMU)
};

20
ArduCopter/config.h
View File

@ -555,7 +555,7 @@
#ifndef STABILIZE_D
# define STABILIZE_D .06
# define STABILIZE_D .2
#endif
@ -567,7 +567,7 @@
// Good for smaller payload motors.
#ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5
# define STABILIZE_ROLL_P 4
#endif
#ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.0
@ -577,7 +577,7 @@
#endif
#ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.5
# define STABILIZE_PITCH_P 4
#endif
#ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0
@ -587,7 +587,7 @@
#endif
#ifndef STABILIZE_YAW_P
# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy
# define STABILIZE_YAW_P 7.0 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01
@ -601,26 +601,26 @@
// Stabilize Rate Control
//
#ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.14
# define RATE_ROLL_P 0.12
#endif
#ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.18
# define RATE_ROLL_I 0.02
#endif
#ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.0025
# define RATE_ROLL_D 0.008
#endif
#ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 5 // degrees
#endif
#ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.14
# define RATE_PITCH_P 0.12
#endif
#ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.18
# define RATE_PITCH_I 0.02
#endif
#ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.0025
# define RATE_PITCH_D 0.008
#endif
#ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 5 // degrees

5
ArduCopter/defines.h
View File

@ -144,6 +144,7 @@
// Rate
#define CH6_RATE_KP 4
#define CH6_RATE_KI 5
#define CH6_RATE_KD 21
#define CH6_YAW_RATE_KP 6
// Altitude rate controller
#define CH6_THROTTLE_KP 7
@ -345,8 +346,8 @@ enum gcs_severity {
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1024 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
// parameters get the first 1280 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
#define ONBOARD_PARAM_NAME_LENGTH 15

22
ArduCopter/heli.pde
View File

@ -7,7 +7,7 @@
static bool heli_swash_initialised = false;
static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000)
static float heli_coll_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
static float heli_collective_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
// heli_servo_averaging:
// 0 or 1 = no averaging, 250hz
@ -39,7 +39,7 @@ static void heli_reset_swash()
heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// set throttle scaling
heli_coll_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0;
heli_collective_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0;
// we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false;
@ -57,17 +57,17 @@ static void heli_init_swash()
g.heli_servo_4.set_angle(4500);
// ensure g.heli_coll values are reasonable
if( g.heli_coll_min >= g.heli_coll_max ) {
g.heli_coll_min = 1000;
g.heli_coll_max = 2000;
if( g.heli_collective_min >= g.heli_collective_max ) {
g.heli_collective_min = 1000;
g.heli_collective_max = 2000;
}
g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max);
g.heli_collective_mid = constrain(g.heli_collective_mid, g.heli_collective_min, g.heli_collective_max);
// calculate throttle mid point
heli_throttle_mid = ((float)(g.heli_coll_mid-g.heli_coll_min))/((float)(g.heli_coll_max-g.heli_coll_min))*1000.0;
heli_throttle_mid = ((float)(g.heli_collective_mid-g.heli_collective_min))/((float)(g.heli_collective_max-g.heli_collective_min))*1000.0;
// determine scalar throttle input
heli_coll_scalar = ((float)(g.heli_coll_max-g.heli_coll_min))/1000.0;
heli_collective_scalar = ((float)(g.heli_collective_max-g.heli_collective_min))/1000.0;
// pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle));
@ -128,7 +128,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
if( heli_swash_initialised ) {
heli_reset_swash();
}
coll_out_scaled = coll_out * heli_coll_scalar + g.rc_3.radio_min - 1000;
coll_out_scaled = coll_out * heli_collective_scalar + g.rc_3.radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
@ -140,12 +140,12 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, 0, 1000);
coll_out_scaled = coll_out * heli_coll_scalar + g.heli_coll_min - 1000;
coll_out_scaled = coll_out * heli_collective_scalar + g.heli_collective_min - 1000;
// rudder feed forward based on collective
#if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator
if( !g.heli_ext_gyro_enabled ) {
yaw_offset = g.heli_coll_yaw_effect * abs(coll_out_scaled - g.heli_coll_mid);
yaw_offset = g.heli_collective_yaw_effect * abs(coll_out_scaled - g.heli_collective_mid);
}
#endif
}

7
ArduCopter/motors_octa_quad.pde
View File

@ -40,8 +40,8 @@ static void output_motors_armed()
g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out * .707;
pitch_out = g.rc_2.pwm_out * .707;
roll_out = (float)g.rc_1.pwm_out * 0.707;
pitch_out = (float)g.rc_2.pwm_out * 0.707;
motor_out[MOT_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // APM2 OUT1 APM1 OUT1 FRONT RIGHT CCW TOP
motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // APM2 OUT2 APM1 OUT2 FRONT LEFT CW TOP
@ -51,7 +51,6 @@ static void output_motors_armed()
motor_out[MOT_6] = g.rc_3.radio_out - roll_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT RIGHT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out - pitch_out; // APM2 OUT7 APM1 OUT10 BACK RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out + roll_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK LEFT CW BOTTOM
}else{
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
@ -64,6 +63,7 @@ static void output_motors_armed()
motor_out[MOT_6] = g.rc_3.radio_out + pitch_out; // APM2 OUT6 APM1 OUT8 FRONT CW BOTTOM
motor_out[MOT_7] = g.rc_3.radio_out - roll_out; // APM2 OUT7 APM1 OUT10 RIGHT CCW BOTTOM
motor_out[MOT_8] = g.rc_3.radio_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK CW BOTTOM
}
// Yaw
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
@ -75,7 +75,6 @@ static void output_motors_armed()
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
}
// TODO add stability patch
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);

4
ArduCopter/motors_quad.pde
View File

@ -37,8 +37,8 @@ static void output_motors_armed()
if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out * .707;
pitch_out = g.rc_2.pwm_out * .707;
roll_out = (float)g.rc_1.pwm_out * 0.707;
pitch_out = (float)g.rc_2.pwm_out * 0.707;
// left
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT

2
ArduCopter/navigation.pde
View File

@ -55,7 +55,7 @@ static void calc_XY_velocity(){
// straightforward approach:
///*
x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * tmp;
x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp;
y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp;
x_actual_speed = x_actual_speed >> 1;

38
ArduCopter/setup.pde
View File

@ -457,7 +457,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
int value = 0;
int temp;
int state = 0; // 0 = set rev+pos, 1 = capture min/max
int max_roll, max_pitch, min_coll, max_coll, min_tail, max_tail;
int max_roll, max_pitch, min_collective, max_collective, min_tail, max_tail;
// initialise swash plate
heli_init_swash();
@ -497,10 +497,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
max_roll = abs(g.rc_1.control_in);
if( abs(g.rc_2.control_in) > max_pitch )
max_pitch = abs(g.rc_2.control_in);
if( g.rc_3.radio_out < min_coll )
min_coll = g.rc_3.radio_out;
if( g.rc_3.radio_out > max_coll )
max_coll = g.rc_3.radio_out;
if( g.rc_3.radio_out < min_collective )
min_collective = g.rc_3.radio_out;
if( g.rc_3.radio_out > max_collective )
max_collective = g.rc_3.radio_out;
min_tail = min(g.rc_4.radio_out, min_tail);
max_tail = max(g.rc_4.radio_out, max_tail);
}
@ -529,8 +529,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
case 'c':
case 'C':
if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
g.heli_coll_mid = g.rc_3.radio_out;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid);
g.heli_collective_mid = g.rc_3.radio_out;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_collective_mid);
}
break;
case 'd':
@ -546,27 +546,27 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
// reset servo ranges
g.heli_roll_max = g.heli_pitch_max = 4500;
g.heli_coll_min = 1000;
g.heli_coll_max = 2000;
g.heli_collective_min = 1000;
g.heli_collective_max = 2000;
g.heli_servo_4.radio_min = 1000;
g.heli_servo_4.radio_max = 2000;
// set sensible values in temp variables
max_roll = abs(g.rc_1.control_in);
max_pitch = abs(g.rc_2.control_in);
min_coll = 2000;
max_coll = 1000;
min_collective = 2000;
max_collective = 1000;
min_tail = max_tail = abs(g.rc_4.radio_out);
}else{
state = 0; // switch back to normal mode
// double check values aren't totally terrible
if( max_roll <= 1000 || max_pitch <= 1000 || (max_coll - min_coll < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_coll,max_coll,min_tail,max_tail);
if( max_roll <= 1000 || max_pitch <= 1000 || (max_collective - min_collective < 200) || (max_tail - min_tail < 200) || min_tail < 1000 || max_tail > 2000 )
Serial.printf_P(PSTR("Invalid min/max captured roll:%d, pitch:%d, collective min: %d max: %d, tail min:%d max:%d\n"),max_roll,max_pitch,min_collective,max_collective,min_tail,max_tail);
else{
g.heli_roll_max = max_roll;
g.heli_pitch_max = max_pitch;
g.heli_coll_min = min_coll;
g.heli_coll_max = max_coll;
g.heli_collective_min = min_collective;
g.heli_collective_max = max_collective;
g.heli_servo_4.radio_min = min_tail;
g.heli_servo_4.radio_max = max_tail;
@ -650,9 +650,9 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
g.heli_servo3_pos.save();
g.heli_roll_max.save();
g.heli_pitch_max.save();
g.heli_coll_min.save();
g.heli_coll_max.save();
g.heli_coll_mid.save();
g.heli_collective_min.save();
g.heli_collective_max.save();
g.heli_collective_mid.save();
g.heli_servo_averaging.save();
// return swash plate movements to attitude controller
@ -942,7 +942,7 @@ static void report_heli()
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_collective_min, (int)g.heli_collective_mid, (int)g.heli_collective_max);
// calculate and print servo rate
if( g.heli_servo_averaging <= 1 ) {

20
ArduCopter/system.pde
View File

@ -327,10 +327,22 @@ static void init_ardupilot()
#if LOGGING_ENABLED == ENABLED
Log_Write_Startup();
Log_Write_Data(10, g.pi_stabilize_roll.kP());
Log_Write_Data(11, g.pi_stabilize_pitch.kP());
Log_Write_Data(12, g.pid_rate_roll.kP());
Log_Write_Data(13, g.pid_rate_pitch.kP());
Log_Write_Data(10, (float)g.pi_stabilize_roll.kP());
Log_Write_Data(11, (float)g.pi_stabilize_roll.kI());
Log_Write_Data(12, (float)g.pid_rate_roll.kP());
Log_Write_Data(13, (float)g.pid_rate_roll.kI());
Log_Write_Data(14, (float)g.pid_rate_roll.kD());
Log_Write_Data(15, (float)g.stabilize_d.get());
Log_Write_Data(16, (float)g.pi_loiter_lon.kP());
Log_Write_Data(17, (float)g.pi_loiter_lon.kI());
Log_Write_Data(18, (float)g.pid_nav_lon.kP());
Log_Write_Data(19, (float)g.pid_nav_lon.kI());
Log_Write_Data(20, (float)g.pid_nav_lon.kD());
Log_Write_Data(21, (int32_t)g.auto_slew_rate.get());
#endif
SendDebug("\nReady to FLY ");

2
ArduPlane/Attitude.pde
View File

@ -314,7 +314,6 @@ static void set_servos(void)
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron]) {
g_rc_function[RC_Channel_aux::k_aileron]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm();
@ -329,6 +328,7 @@ static void set_servos(void)
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
g.channel_rudder.calc_pwm();
#if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0;

30
ArduPlane/GCS_Mavlink.pde
View File

@ -935,14 +935,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.req_stream_id){
case MAV_DATA_STREAM_ALL:
streamRateRawSensors = freq;
streamRateExtendedStatus = freq;
streamRateRCChannels = freq;
streamRateRawController = freq;
streamRatePosition = freq;
streamRateExtra1 = freq;
streamRateExtra2 = freq;
streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
streamRateRawSensors.set_and_save_ifchanged(freq);
streamRateExtendedStatus.set_and_save_ifchanged(freq);
streamRateRCChannels.set_and_save_ifchanged(freq);
streamRateRawController.set_and_save_ifchanged(freq);
streamRatePosition.set_and_save_ifchanged(freq);
streamRateExtra1.set_and_save_ifchanged(freq);
streamRateExtra2.set_and_save_ifchanged(freq);
streamRateExtra3.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_RAW_SENSORS:
@ -950,15 +950,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// we will not continue to broadcast raw sensor data at 50Hz.
break;
case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus.set_and_save(freq);
streamRateExtendedStatus.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels.set_and_save(freq);
streamRateRCChannels.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController.set_and_save(freq);
streamRateRawController.set_and_save_ifchanged(freq);
break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
@ -966,19 +966,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// break;
case MAV_DATA_STREAM_POSITION:
streamRatePosition.set_and_save(freq);
streamRatePosition.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1.set_and_save(freq);
streamRateExtra1.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2.set_and_save(freq);
streamRateExtra2.set_and_save_ifchanged(freq);
break;
case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3.set_and_save(freq);
streamRateExtra3.set_and_save_ifchanged(freq);
break;
default:

2
ArduPlane/Parameters.h
View File

@ -68,7 +68,7 @@ public:
//
// 130: Sensor parameters
//
k_param_IMU_calibration = 130,
k_param_imu = 130, // sensor calibration
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,

3
ArduPlane/Parameters.pde
View File

@ -123,7 +123,8 @@ static const AP_Param::Info var_info[] PROGMEM = {
// variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK)
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECT(imu, "IMU_", IMU)
};

4
ArduPlane/defines.h
View File

@ -200,8 +200,8 @@ enum gcs_severity {
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1024 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
// parameters get the first 1280 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x500 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
// fence points are stored at the end of the EEPROM

5
ArduPlane/radio.pde
View File

@ -157,7 +157,6 @@ static void trim_control_surfaces()
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{
@ -169,6 +168,7 @@ static void trim_control_surfaces()
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom
g.channel_roll.save_eeprom();
@ -190,7 +190,6 @@ static void trim_radio()
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else {
@ -199,8 +198,8 @@ static void trim_radio()
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
}
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom
g.channel_roll.save_eeprom();

5
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

@ -242,11 +242,14 @@
<Compile Include="CommsTCPSerial.cs" />
<Compile Include="CommsUdpSerial.cs" />
<Compile Include="Controls\ImageLabel.cs">
<SubType>UserControl</SubType>
<SubType>Component</SubType>
</Compile>
<Compile Include="Controls\ImageLabel.Designer.cs">
<DependentUpon>ImageLabel.cs</DependentUpon>
</Compile>
<Compile Include="Controls\myGMAP.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Controls\XorPlus.cs">
<SubType>Form</SubType>
</Compile>

2
Tools/ArdupilotMegaPlanner/Controls/ImageLabel.cs
View File

@ -9,7 +9,7 @@ using System.Windows.Forms;
namespace ArdupilotMega
{
public partial class ImageLabel : UserControl
public partial class ImageLabel : ContainerControl
{
public new event EventHandler Click;

39
Tools/ArdupilotMegaPlanner/Controls/myGMAP.cs Normal file
View File

@ -0,0 +1,39 @@
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace ArdupilotMega
{
class myGMAP : GMap.NET.WindowsForms.GMapControl
{
public bool inOnPaint = false;
protected override void OnPaint(System.Windows.Forms.PaintEventArgs e)
{
if (inOnPaint)
{
Console.WriteLine("Was in onpaint Gmap th:" + System.Threading.Thread.CurrentThread.Name);
return;
}
inOnPaint = true;
try
{
base.OnPaint(e);
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
inOnPaint = false;
}
protected override void OnMouseMove(System.Windows.Forms.MouseEventArgs e)
{
try
{
base.OnMouseMove(e);
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
}
}
}

6
Tools/ArdupilotMegaPlanner/CurrentState.cs
View File

@ -309,12 +309,18 @@ namespace ArdupilotMega
if (ind != -1)
logdata = logdata.Substring(0, ind);
try
{
while (messages.Count > 5)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}

4
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs generated
View File

@ -55,7 +55,7 @@
this.zg1 = new ZedGraph.ZedGraphControl();
this.lbl_winddir = new ArdupilotMega.MyLabel();
this.lbl_windvel = new ArdupilotMega.MyLabel();
this.gMapControl1 = new GMap.NET.WindowsForms.GMapControl();
this.gMapControl1 = new myGMAP();
this.panel1 = new System.Windows.Forms.Panel();
this.TXT_lat = new ArdupilotMega.MyLabel();
this.Zoomlevel = new System.Windows.Forms.NumericUpDown();
@ -1320,7 +1320,7 @@
private ArdupilotMega.MyLabel TXT_long;
private ArdupilotMega.MyLabel TXT_alt;
private System.Windows.Forms.CheckBox CHK_autopan;
private GMap.NET.WindowsForms.GMapControl gMapControl1;
private myGMAP gMapControl1;
private ZedGraph.ZedGraphControl zg1;
private System.Windows.Forms.TabControl tabControl1;
private System.Windows.Forms.TabPage tabGauges;

15
Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs
View File

@ -369,7 +369,7 @@ namespace ArdupilotMega.GCSViews
try
{
//Console.WriteLine(DateTime.Now.Millisecond);
MainV2.cs.UpdateCurrentSettings(bindingSource1);
updateBindingSource();
//Console.WriteLine(DateTime.Now.Millisecond + " done ");
if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked == true)
@ -402,6 +402,11 @@ namespace ArdupilotMega.GCSViews
{
gMapControl1.HoldInvalidation = true;
while (gMapControl1.inOnPaint == true)
{
System.Threading.Thread.Sleep(1);
}
if (trackPoints.Count > int.Parse(MainV2.config["NUM_tracklength"].ToString()))
{
trackPoints.RemoveRange(0, trackPoints.Count - int.Parse(MainV2.config["NUM_tracklength"].ToString()));
@ -500,6 +505,14 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("FD Main loop exit");
}
private void updateBindingSource()
{
this.Invoke((System.Windows.Forms.MethodInvoker)delegate()
{
MainV2.cs.UpdateCurrentSettings(bindingSource1);
});
}
private void updateMapPosition(PointLatLng currentloc)
{
this.BeginInvoke((MethodInvoker)delegate()

4
Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.Designer.cs generated
View File

@ -101,7 +101,7 @@
this.lbl_distance = new System.Windows.Forms.Label();
this.lbl_homedist = new System.Windows.Forms.Label();
this.lbl_prevdist = new System.Windows.Forms.Label();
this.MainMap = new GMap.NET.WindowsForms.GMapControl();
this.MainMap = new myGMAP();
this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components);
this.deleteWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.loiterToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
@ -979,7 +979,7 @@
private BSE.Windows.Forms.Panel panelWaypoints;
private BSE.Windows.Forms.Panel panelAction;
private System.Windows.Forms.Panel panelMap;
private GMap.NET.WindowsForms.GMapControl MainMap;
private myGMAP MainMap;
private MyTrackBar trackBar1;
private System.Windows.Forms.Label label11;
private System.Windows.Forms.Label lbl_distance;

127
Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs
View File

@ -81,6 +81,31 @@ namespace ArdupilotMega.GCSViews
public uint magic;
}
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct sitldata
{
public double lat;
public double lon;
public double alt;
public double heading;
public double v_north;
public double v_east;
public double ax;
public double ay;
public double az;
public double phidot;
public double thetadot;
public double psidot;
public double phi;
public double theta;
/// <summary>
/// heading
/// </summary>
public double psi;
public double vcas;
public int check;
}
const int AEROSIMRC_MAX_CHANNELS = 39;
//-----------------------------------------------------------------------------
@ -263,6 +288,11 @@ namespace ArdupilotMega.GCSViews
SetupUDPRecv();
if (chkSensor.Checked)
{
SITLSEND = new UdpClient(simIP, 5501);
}
if (RAD_softXplanes.Checked)
{
SetupUDPXplanes();
@ -286,12 +316,9 @@ namespace ArdupilotMega.GCSViews
System.Threading.Thread.Sleep(2000);
SITLSEND = new UdpClient(simIP, 5501);
SetupTcpJSBSim(); // old style
}
SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw
}
@ -545,7 +572,7 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("REQ streams - sim");
try
{
if (CHK_quad.Checked && !RAD_aerosimrc.Checked)
if (CHK_quad.Checked && !RAD_aerosimrc.Checked)// || chkSensor.Checked && RAD_JSBSim.Checked)
{
comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_CONTROLLER, 0); // request servoout
}
@ -567,6 +594,8 @@ namespace ArdupilotMega.GCSViews
int recv = SimulatorRECV.ReceiveFrom(udpdata, ref Remote);
RECVprocess(udpdata, recv, comPort);
hzcount++;
}
}
catch
@ -598,7 +627,7 @@ namespace ArdupilotMega.GCSViews
if ((DateTime.Now - simsendtime).TotalMilliseconds > 19)
{
hzcount++;
//hzcount++;
simsendtime = DateTime.Now;
processArduPilot();
}
@ -607,7 +636,7 @@ namespace ArdupilotMega.GCSViews
if (hzcounttime.Second != DateTime.Now.Second)
{
// Console.WriteLine("SIM hz {0}", hzcount);
Console.WriteLine("SIM hz {0}", hzcount);
hzcount = 0;
hzcounttime = DateTime.Now;
}
@ -758,26 +787,13 @@ namespace ArdupilotMega.GCSViews
float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds);
float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds);
//Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);
// Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]);
oldatt = att;
if (xplane9)
{
rdiff = DATA[17][1];
pdiff = DATA[17][0];
ydiff = DATA[17][2];
}
else
{
rdiff = DATA[16][1];
pdiff = DATA[16][0];
ydiff = DATA[16][2];
}
Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 ygyro = Constrain(pdiff * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 zgyro = Constrain(ydiff * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 xgyro = Constrain(att.rollspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 ygyro = Constrain(att.pitchspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 zgyro = Constrain(att.yawspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
oldtime = DateTime.Now;
@ -849,7 +865,7 @@ namespace ArdupilotMega.GCSViews
gps.v = ((float)(DATA[3][7] * 0.44704));
#endif
asp.airspeed = ((float)(DATA[3][6] * 0.44704));
asp.airspeed = ((float)(DATA[3][5] * 0.44704));
}
@ -1160,6 +1176,50 @@ namespace ArdupilotMega.GCSViews
return;
}
if (RAD_softXplanes.Checked && chkSensor.Checked)
{
sitldata sitlout = new sitldata();
ArdupilotMega.HIL.Utils.FLIGHTtoBCBF(ref att.pitchspeed, ref att.rollspeed, ref att.yawspeed, DATA[19][0] * deg2rad, DATA[19][1] * deg2rad);
//Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", att.pitchspeed, att.rollspeed, att.yawspeed, DATA[17][0], DATA[17][1], DATA[17][2]);
Tuple<double, double, double> ans = ArdupilotMega.HIL.Utils.OGLtoBCBF(att.pitch, att.roll, att.yaw, 0, 0, 9.8);
//Console.WriteLine("acc {0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", ans.Item1, ans.Item2, ans.Item3, accel3D.X, accel3D.Y, accel3D.Z);
sitlout.alt = gps.alt;
sitlout.lat = gps.lat;
sitlout.lon = gps.lon;
sitlout.heading = gps.hdg;
sitlout.v_north = DATA[21][4];
sitlout.v_east = DATA[21][5];
// correct accel
sitlout.ax = -ans.Item2; // pitch
sitlout.ay = -ans.Item1; // roll
sitlout.az = ans.Item3; // yaw
sitlout.phidot = -0.5;// att.pitchspeed;
// sitlout.thetadot = att.rollspeed;
//sitlout.psidot = att.yawspeed;
sitlout.phi = att.roll * rad2deg;
sitlout.theta = att.pitch * rad2deg;
sitlout.psi = att.yaw * rad2deg;
sitlout.vcas = asp.airspeed;
sitlout.check = (int)0x4c56414e;
byte[] sendme = StructureToByteArray(sitlout);
SITLSEND.Send(sendme,sendme.Length);
return;
}
#if MAVLINK10
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
@ -1624,6 +1684,25 @@ namespace ArdupilotMega.GCSViews
}
}
byte[] StructureToByteArray(object obj)
{
int len = Marshal.SizeOf(obj);
byte[] arr = new byte[len];
IntPtr ptr = Marshal.AllocHGlobal(len);
Marshal.StructureToPtr(obj, ptr, true);
Marshal.Copy(ptr, arr, 0, len);
Marshal.FreeHGlobal(ptr);
return arr;
}
private void RAD_softXplanes_CheckedChanged(object sender, EventArgs e)
{

20
Tools/ArdupilotMegaPlanner/HIL/Utils.cs
View File

@ -175,19 +175,19 @@ namespace ArdupilotMega.HIL
// The +Y axis points straight up away from the center of the earth at the reference point.
// First we shall convert from this East Up South frame, to a more conventional NED (North East Down) frame.
x_NED = radians(x) * -1.0;
y_NED = radians(y) * 1.0;
z_NED = radians(z) * -1.0;
x_NED = (x) * -1.0;
y_NED = (y) * 1.0;
z_NED = (z) * -1.0;
// Next calculate cos & sin of angles for use in the transformation matrix.
// r, p & y subscripts stand for roll pitch and yaw.
Cr = Math.Cos(radians(phi));
Cp = Math.Cos(radians(theta));
Cy = Math.Cos(radians(psi));
Sr = Math.Sin(radians(phi));
Sp = Math.Sin(radians(theta));
Sy = Math.Sin(radians(psi));
Cr = Math.Cos((phi));
Cp = Math.Cos((theta));
Cy = Math.Cos((psi));
Sr = Math.Sin((phi));
Sp = Math.Sin((theta));
Sy = Math.Sin((psi));
// Next we need to rotate our accelerations from the NED reference frame, into the body fixed reference frame
@ -202,7 +202,7 @@ namespace ArdupilotMega.HIL
y = (x_NED * ((Sr * Sp * Cy) - (Cr * Sy))) + (y_NED * ((Sr * Sp * Sy) + (Cr * Cy))) + (z_NED * Sr * Cp);
z = (x_NED * ((Cr * Sp * Cy) + (Sr * Sy))) + (y_NED * ((Cr * Sp * Sy) - (Sr * Cy))) + (z_NED * Cr * Cp);
return new Tuple<double, double, double>(degrees(x), degrees(y), degrees(z));
return new Tuple<double, double, double>((x), (y), (z));
}

14
Tools/ArdupilotMegaPlanner/HUD.cs
View File

@ -240,6 +240,8 @@ namespace hud
started = true;
}
bool inOnPaint = false;
protected override void OnPaint(PaintEventArgs e)
{
//GL.Enable(EnableCap.AlphaTest)
@ -261,6 +263,14 @@ namespace hud
return;
}
if (inOnPaint)
{
Console.WriteLine("Was in onpaint Hud th:" + System.Threading.Thread.CurrentThread.Name);
return;
}
inOnPaint = true;
starttime = DateTime.Now;
try
@ -284,6 +294,8 @@ namespace hud
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
inOnPaint = false;
count++;
huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds;
@ -1479,7 +1491,7 @@ namespace hud
if (e == null || P == null || pth == null || pth.PointCount == 0)
return;
if (!ArdupilotMega.MainV2.MONO)
//if (!ArdupilotMega.MainV2.MONO)
e.DrawPath(P, pth);
//Draw the face

4
Tools/ArdupilotMegaPlanner/MainV2.cs
View File

@ -233,9 +233,9 @@ namespace ArdupilotMega
double Framework = Convert.ToDouble(version_names[version_names.Length - 1].Remove(0, 1), CultureInfo.InvariantCulture);
int SP = Convert.ToInt32(installed_versions.OpenSubKey(version_names[version_names.Length - 1]).GetValue("SP", 0));
if (Framework < 4.0)
if (Framework < 3.5)
{
MessageBox.Show("This program requires .NET Framework 4.0 +. You currently have " + Framework);
MessageBox.Show("This program requires .NET Framework 3.5. You currently have " + Framework);
}
}

2
Tools/ArdupilotMegaPlanner/Program.cs
View File

@ -33,6 +33,8 @@ namespace ArdupilotMega
Application.SetCompatibleTextRenderingDefault(false);
try
{
System.Threading.Thread.CurrentThread.Name = "Base Thread";
Application.Run(new MainV2());
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }

2
Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs
View File

@ -34,5 +34,5 @@ using System.Resources;
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.36")]
[assembly: AssemblyFileVersion("1.1.37")]
[assembly: NeutralResourcesLanguageAttribute("")]

BIN
Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb
View File

Binary file not shown.

1
Tools/CPUInfo/CPUInfo.pde
View File

@ -93,6 +93,7 @@ static void show_timings(void)
TIMEIT("sin()", v_out = sin(v_f), 20);
TIMEIT("cos()", v_out = cos(v_f), 20);
TIMEIT("tan()", v_out = tan(v_f), 20);
TIMEIT("sqrt()",v_out = sqrt(v_f), 20);
TIMEIT("iadd8", v_out_8 += v_8, 100);
TIMEIT("isub8", v_out_8 -= v_8, 100);

2
Tools/autotest/ArduPlane.parm
View File

@ -2,7 +2,7 @@ LOG_BITMASK 4095
SWITCH_ENABLE 0
MAG_ENABLE 1
TRIM_ARSPD_CM 2200
TRIM_PITCH_CD -1000
TRIM_PITCH_CD 0
ARSPD_ENABLE 1
ARSP2PTCH_I 0.1
ARSPD_FBW_MAX 30

2
Tools/autotest/aircraft/Rascal/Rascal.xml
View File

@ -86,7 +86,7 @@
<location unit="IN">
<x> 68.9 </x>
<y> 0 </y>
<z> -3 </z>
<z> -13.1 </z>
</location>
<static_friction> 8.0 </static_friction>
<dynamic_friction> 5.0 </dynamic_friction>

2
Tools/autotest/pysim/aircraft.py
View File

@ -70,7 +70,7 @@ class Aircraft(object):
from math import sin, cos, sqrt, radians
# work out what the accelerometer would see
xAccel = sin(radians(self.pitch)) * cos(radians(self.roll))
xAccel = sin(radians(self.pitch))
yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
zAccel = -cos(radians(self.roll)) * cos(radians(self.pitch))
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))

8
libraries/AC_PID/AC_PID.cpp
View File

@ -28,8 +28,9 @@ int32_t AC_PID::get_i(int32_t error, float dt)
} else if (_integrator > _imax) {
_integrator = _imax;
}
}
return _integrator;
}
return 0;
}
int32_t AC_PID::get_d(int32_t input, float dt)
@ -73,8 +74,7 @@ int32_t AC_PID::get_pid(int32_t error, float dt)
// Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) {
_derivative = (error - _last_input) / dt;
_derivative = (error - _last_error) / dt;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
@ -82,7 +82,7 @@ int32_t AC_PID::get_pid(int32_t error, float dt)
(dt / ( _filter + dt)) * (_derivative - _last_derivative);
// update state
_last_input = error;
_last_error = error;
_last_derivative = _derivative;
// add in derivative component

1
libraries/AP_ADC/AP_ADC_ADS7844.cpp
View File

@ -116,7 +116,6 @@ void AP_ADC_ADS7844::read(uint32_t tnow)
// reader below could get a division by zero
_sum[ch] = 0;
_count[ch] = 1;
last_ch6_micros = tnow;
}
_sum[ch] += (v >> 3);
}

6
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -212,10 +212,10 @@ void AP_Baro_MS5611::_calculate()
SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
if (TEMP < 2000){ // second order temperature compensation
int64_t T2 = (((int64_t)dT)*dT) / 2147483648UL;
int64_t T2 = (((int64_t)dT)*dT) >> 31;
int64_t Aux_64 = (TEMP-2000)*(TEMP-2000);
int64_t OFF2 = 5*Aux_64/2;
int64_t SENS2 = 5*Aux_64/4;
int64_t OFF2 = (5*Aux_64)>>1;
int64_t SENS2 = (5*Aux_64)>>2;
TEMP = TEMP - T2;
OFF = OFF - OFF2;
SENS = SENS - SENS2;

7
libraries/AP_Common/AP_Param.cpp
View File

@ -15,6 +15,7 @@
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <math.h>
#include <string.h>
@ -206,6 +207,12 @@ bool AP_Param::setup(const AP_Param::Info *info, uint8_t num_vars, uint16_t eepr
return true;
}
// check if AP_Param has been initialised
bool AP_Param::initialised(void)
{
return _var_info != NULL;
}
// find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage
const struct AP_Param::Info *AP_Param::find_by_header_group(struct Param_header phdr, void **ptr,

28
libraries/AP_Common/AP_Param.h
View File

@ -18,7 +18,6 @@
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <AP_Math.h>
#define AP_MAX_NAME_SIZE 15
@ -80,6 +79,9 @@ public:
// wrong version is found
static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size);
// return true if AP_Param has been initialised via setup()
static bool initialised(void);
/// Copy the variable's name, prefixed by any containing group name, to a buffer.
///
/// If the variable has no name, the buffer will contain an empty string.
@ -259,6 +261,19 @@ public:
return save();
}
/// Combined set and save, but only does the save if the value if
/// different from the current ram value, thus saving us a
/// scan(). This should only be used where we have not set() the
/// value separately, as otherwise the value in EEPROM won't be
/// updated correctly.
bool set_and_save_ifchanged(T v) {
if (v == _value) {
return true;
}
set(v);
return save();
}
/// Conversion to T returns a reference to the value.
///
/// This allows the class to be used in many situations where the value would be legal.
@ -334,13 +349,13 @@ public:
/// Copy assignment from self does nothing.
///
AP_ParamT<T,PT>& operator=(AP_ParamT<T,PT>& v) {
AP_ParamV<T,PT>& operator=(AP_ParamV<T,PT>& v) {
return v;
}
/// Copy assignment from T is equivalent to ::set.
///
AP_ParamT<T,PT>& operator=(T v) {
AP_ParamV<T,PT>& operator=(T v) {
_value = v;
return *this;
}
@ -415,13 +430,12 @@ AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8
AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32
#define AP_PARAMDEFV(_t, _n, _pt) typedef AP_ParamV<_t, _pt> AP_##_n;
AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
#define AP_PARAMDEFA(_t, _n, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_##_n;
AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F);
// this is used in AP_Math.h
#define AP_PARAMDEFV(_t, _n, _pt) typedef AP_ParamV<_t, _pt> AP_##_n;
/// Rely on built in casting for other variable types
/// to minimize template creation and save memory
#define AP_Uint8 AP_Int8

9
libraries/AP_DCM/AP_DCM.cpp
View File

@ -131,8 +131,11 @@ AP_DCM::matrix_update(float _G_Dt)
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega)
_omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration.
if(_centripetal &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
// Remove _centripetal acceleration.
accel_adjust();
}
#if OUTPUTMODE == 1
@ -174,9 +177,7 @@ AP_DCM::accel_adjust(void)
{
Vector3f veloc, temp;
if (_gps) {
veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
}
// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive

5
libraries/AP_IMU/AP_IMU_INS.cpp
View File

@ -21,11 +21,6 @@
#include "AP_IMU_INS.h"
const AP_Param::GroupInfo AP_IMU_INS::var_info[] PROGMEM = {
AP_GROUPINFO("CAL", 0, AP_IMU_INS, _sensor_cal),
AP_GROUPEND
};
void
AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t),

3
libraries/AP_IMU/AP_IMU_INS.h
View File

@ -64,11 +64,8 @@ public:
virtual void ay(const float v) { _sensor_cal[4] = v; }
virtual void az(const float v) { _sensor_cal[5] = v; }
static const struct AP_Param::GroupInfo var_info[];
private:
AP_InertialSensor *_ins; ///< INS provides an axis and unit correct sensor source.
AP_Vector6f _sensor_cal; ///< Calibrated sensor offsets
virtual void _init_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL); ///< no-save implementation

7
libraries/AP_IMU/IMU.cpp
View File

@ -1,6 +1,13 @@
#include "IMU.h"
// this allows the sensor calibration to be saved to EEPROM
const AP_Param::GroupInfo IMU::var_info[] PROGMEM = {
AP_GROUPINFO("CAL", 0, IMU, _sensor_cal),
AP_GROUPEND
};
/* Empty implementations for the IMU functions.
* Although these will never be used, in certain situations with
* optimizations turned off, having empty implementations in an object

5
libraries/AP_IMU/IMU.h
View File

@ -104,7 +104,12 @@ public:
virtual void ay(const float v);
virtual void az(const float v);
static const struct AP_Param::GroupInfo var_info[];
protected:
AP_Vector6f _sensor_cal; ///< Calibrated sensor offsets
/// Most recent accelerometer reading obtained by ::update
Vector3f _accel;
Vector3f _accel_filtered;

5
libraries/AP_Math/AP_Math.h
View File

@ -2,8 +2,13 @@
// Assorted useful math operations for ArduPilot(Mega)
#include <AP_Common.h>
#include <stdint.h>
#include "vector2.h"
#include "vector3.h"
#include "matrix3.h"
#include "polygon.h"
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);

27
libraries/AP_OpticalFlow/AP_OpticalFlow.cpp
View File

@ -109,30 +109,3 @@ AP_OpticalFlow::update_position(float roll, float pitch, float cos_yaw_x, float
_last_roll = roll;
_last_pitch = pitch;
}
/*
{
// only update position if surface quality is good and angle is not over 45 degrees
if( surface_quality >= 10 && fabs(_dcm->roll) <= FORTYFIVE_DEGREES && fabs(_dcm->pitch) <= FORTYFIVE_DEGREES ) {
altitude = max(altitude, 0);
Vector3f omega = _dcm->get_gyro();
// calculate expected x,y diff due to roll and pitch change
float exp_change_x = omega.x * radians_to_pixels;
float exp_change_y = -omega.y * radians_to_pixels;
// real estimated raw change from mouse
float change_x = dx - exp_change_x;
float change_y = dy - exp_change_y;
// convert raw change to horizontal movement in cm
float x_cm = -change_x * altitude * conv_factor; // perhaps this altitude should actually be the distance to the ground? i.e. if we are very rolled over it should be longer?
float y_cm = -change_y * altitude * conv_factor; // for example if you are leaned over at 45 deg the ground will appear farther away and motion from opt flow sensor will be less
vlon = (float)x_cm * sin_yaw_y - (float)y_cm * cos_yaw_x;
vlat = (float)y_cm * sin_yaw_y - (float)x_cm * cos_yaw_x;
}
}
*/

2
libraries/AP_OpticalFlow/AP_OpticalFlow.h
View File

@ -17,8 +17,8 @@
write_register() : writes a value to one of the sensor's register (will be sensor specific)
*/
#include <FastSerial.h>
#include <AP_Math.h>
#include <AP_DCM.h>
#include <AP_Common.h>
// standard rotation matrices

12
libraries/Desktop/support/sitl_adc.h
View File

@ -8,6 +8,15 @@
#include <stdlib.h>
#define NOISE_BITS 4
static inline float noise_generator(void)
{
float noise = ((unsigned)random()) & ((1<<NOISE_BITS)-1);
noise -= 0.5*(1<<NOISE_BITS);
return noise;
}
// this implements the UDR2 register
struct ADC_UDR2 {
uint16_t value, next_value;
@ -42,8 +51,7 @@ struct ADC_UDR2 {
default: return *this;
}
idx = 1;
int noise = (((unsigned long)random()) % 3) - 1;
next_value = (unsigned)(next_analog + noise + 0.5);
next_value = (unsigned)(next_analog + noise_generator() + 0.5);
if (next_value >= 0x1000) {
next_value = 0xFFF;
}