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 -*- /// -*- 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 Authors: Jason Short
Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen 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 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 Guntars :Arming safety suggestion
Igor van Airde :Control Law optimization Igor van Airde :Control Law optimization
Jean-Louis Naudin :Auto Landing Jean-Louis Naudin :Auto Landing
Sandro Benigno : Camera support Sandro Benigno :Camera support
Olivier Adler : PPM Encoder Olivier Adler :PPM Encoder
John Arne Birkeland: PPM Encoder John Arne Birkeland :PPM Encoder
And much more so PLEASE PM me on DIYDRONES to add your contribution to the List 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; static int16_t nav_lat;
// The desired bank towards East (Positive) or West (Negative) // The desired bank towards East (Positive) or West (Negative)
static int16_t nav_lon; 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. // The Commanded ROll from the autopilot based on optical flow sensor.
static int32_t of_roll = 0; static int32_t of_roll = 0;
// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. // 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(); uint32_t timer = micros();
// We want this to execute fast // 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; //PORTK |= B00010000;
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops
fast_loopTimer = timer; 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); g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
break; 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: case ROLL_PITCH_AUTO:
// apply SIMPLE mode transform // apply SIMPLE mode transform
if(do_simple && new_radio_frame){ if(do_simple && new_radio_frame){
@ -1942,11 +1926,12 @@ static void tuning(){
case CH6_DAMP: case CH6_DAMP:
g.stabilize_d.set(tuning_value); g.stabilize_d.set(tuning_value);
break;
//tuning_value = (float)g.rc_6.control_in / 100000.0; case CH6_RATE_KD:
//g.rc_6.set_range(0,1000); // 0 to 1 tuning_value = (float)g.rc_6.control_in / 100000.0;
//g.pid_rate_roll.kD(tuning_value); g.pid_rate_roll.kD(tuning_value);
//g.pid_rate_pitch.kD(tuning_value); g.pid_rate_pitch.kD(tuning_value);
break; break;
case CH6_STABILIZE_KP: case CH6_STABILIZE_KP:
@ -1956,26 +1941,21 @@ static void tuning(){
break; break;
case CH6_STABILIZE_KI: 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_roll.kI(tuning_value);
g.pi_stabilize_pitch.kI(tuning_value); g.pi_stabilize_pitch.kI(tuning_value);
break; break;
case CH6_RATE_KP: case CH6_RATE_KP:
//g.rc_6.set_range(40,300); // 0 to .3
g.pid_rate_roll.kP(tuning_value); g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value);
break; break;
case CH6_RATE_KI: case CH6_RATE_KI:
//g.rc_6.set_range(0,500); // 0 to .5
g.pid_rate_roll.kI(tuning_value); g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value); g.pid_rate_pitch.kI(tuning_value);
break; break;
case CH6_YAW_KP: case CH6_YAW_KP:
//g.rc_6.set_range(0,1000);
g.pi_stabilize_yaw.kP(tuning_value); g.pi_stabilize_yaw.kP(tuning_value);
break; break;
@ -1985,70 +1965,58 @@ static void tuning(){
break; break;
case CH6_THROTTLE_KP: case CH6_THROTTLE_KP:
//g.rc_6.set_range(0,1000); // 0 to 1
g.pid_throttle.kP(tuning_value); g.pid_throttle.kP(tuning_value);
break; break;
case CH6_TOP_BOTTOM_RATIO: case CH6_TOP_BOTTOM_RATIO:
//g.rc_6.set_range(800,1000); // .8 to 1
g.top_bottom_ratio = tuning_value; g.top_bottom_ratio = tuning_value;
break; break;
case CH6_RELAY: 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 > 525) relay.on();
if (g.rc_6.control_in < 475) relay.off(); if (g.rc_6.control_in < 475) relay.off();
break; break;
case CH6_TRAVERSE_SPEED: case CH6_TRAVERSE_SPEED:
//g.rc_6.set_range(0,1000);
g.waypoint_speed_max = g.rc_6.control_in; g.waypoint_speed_max = g.rc_6.control_in;
break; break;
case CH6_LOITER_P: case CH6_LOITER_P:
//g.rc_6.set_range(0,2000);
g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lat.kP(tuning_value);
g.pi_loiter_lon.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value);
break; break;
case CH6_NAV_P: case CH6_NAV_P:
//g.rc_6.set_range(0,4000);
g.pid_nav_lat.kP(tuning_value); g.pid_nav_lat.kP(tuning_value);
g.pid_nav_lon.kP(tuning_value); g.pid_nav_lon.kP(tuning_value);
break; break;
case CH6_NAV_I: case CH6_NAV_I:
//g.rc_6.set_range(0,500);
g.pid_nav_lat.kI(tuning_value); g.pid_nav_lat.kI(tuning_value);
g.pid_nav_lon.kI(tuning_value); g.pid_nav_lon.kI(tuning_value);
break; break;
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO: case CH6_HELI_EXTERNAL_GYRO:
//g.rc_6.set_range(1000,2000);
g.heli_ext_gyro_gain = tuning_value * 1000; g.heli_ext_gyro_gain = tuning_value * 1000;
break; break;
#endif #endif
case CH6_THR_HOLD_KP: case CH6_THR_HOLD_KP:
//g.rc_6.set_range(0,1000); // 0 to 1
g.pi_alt_hold.kP(tuning_value); g.pi_alt_hold.kP(tuning_value);
break; break;
case CH6_OPTFLOW_KP: case CH6_OPTFLOW_KP:
//g.rc_6.set_range(0,5000); // 0 to 5
g.pid_optflow_roll.kP(tuning_value); g.pid_optflow_roll.kP(tuning_value);
g.pid_optflow_pitch.kP(tuning_value); g.pid_optflow_pitch.kP(tuning_value);
break; break;
case CH6_OPTFLOW_KI: case CH6_OPTFLOW_KI:
//g.rc_6.set_range(0,10000); // 0 to 10
g.pid_optflow_roll.kI(tuning_value); g.pid_optflow_roll.kI(tuning_value);
g.pid_optflow_pitch.kI(tuning_value); g.pid_optflow_pitch.kI(tuning_value);
break; break;
case CH6_OPTFLOW_KD: 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_roll.kD(tuning_value);
g.pid_optflow_pitch.kD(tuning_value); g.pid_optflow_pitch.kD(tuning_value);
break; 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 // limit the error we're feeding to the PID
target_angle = constrain(target_angle, -2500, 2500); 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); 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); 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 static int
get_rate_roll(int32_t target_rate) 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; static int32_t last_rate = 0;
int32_t current_rate = (omega.x * DEGX100); 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 // rate control
target_rate = target_rate - current_rate; target_rate = target_rate - current_rate;
target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt); target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt);
// Dampening // Dampening
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500); //int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
last_rate = current_rate; //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: // output control:
return constrain(target_rate, -2500, 2500); return constrain(target_rate, -2500, 2500);
@ -125,16 +143,31 @@ get_rate_roll(int32_t target_rate)
static int static int
get_rate_pitch(int32_t target_rate) 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; static int32_t last_rate = 0;
int32_t current_rate = (omega.y * DEGX100); 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 // rate control
target_rate = target_rate - current_rate; target_rate = target_rate - current_rate;
target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt); target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt);
// Dampening // Dampening
target_rate -= constrain((current_rate - last_rate) * g.stabilize_d, -500, 500); //int16_t d_temp = (float)(current_rate - last_rate) * g.stabilize_d;
last_rate = current_rate; //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: // output control:
return constrain(target_rate, -2500, 2500); 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_averaging,
k_param_heli_servo_manual, k_param_heli_servo_manual,
k_param_heli_phase_angle, k_param_heli_phase_angle,
k_param_heli_coll_yaw_effect, // 97 k_param_heli_collective_yaw_effect, // 97
#endif #endif
// 110: Telemetry control // 110: Telemetry control
@ -92,7 +92,7 @@ public:
// //
// 140: Sensor parameters // 140: Sensor parameters
// //
k_param_IMU_calibration = 140, k_param_imu = 140, // sensor calibration
k_param_battery_monitoring, k_param_battery_monitoring,
k_param_volt_div_ratio, k_param_volt_div_ratio,
k_param_curr_amp_per_volt, 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 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_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_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_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_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_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_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_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 #endif
// RC channels // RC channels
@ -377,24 +377,20 @@ public:
auto_slew_rate (AUTO_SLEW_RATE), auto_slew_rate (AUTO_SLEW_RATE),
#if FRAME_CONFIG == HELI_FRAME #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_servo1_pos (-60),
heli_servo2_pos (60), heli_servo2_pos (60),
heli_servo3_pos (180), heli_servo3_pos (180),
heli_roll_max (4500), heli_roll_max (4500),
heli_pitch_max (4500), heli_pitch_max (4500),
heli_coll_min (1250), heli_collective_min (1250),
heli_coll_max (1750), heli_collective_max (1750),
heli_coll_mid (1500), heli_collective_mid (1500),
heli_ext_gyro_enabled (0), heli_ext_gyro_enabled (0),
heli_ext_gyro_gain (1350), heli_ext_gyro_gain (1350),
heli_servo_averaging (0), heli_servo_averaging (0),
heli_servo_manual (0), heli_servo_manual (0),
heli_phase_angle (0), heli_phase_angle (0),
heli_coll_yaw_effect (0), heli_collective_yaw_effect (0),
#endif #endif
camera_pitch_gain (CAM_PITCH_GAIN), 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"), GSCALAR(auto_slew_rate, "AUTO_SLEW"),
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
GSCALAR(heli_servo_1, "HS1_"), GGROUP(heli_servo_1, "HS1_", RC_Channel),
GSCALAR(heli_servo_2, "HS2_"), GGROUP(heli_servo_2, "HS2_", RC_Channel),
GSCALAR(heli_servo_3, "HS3_"), GGROUP(heli_servo_3, "HS3_", RC_Channel),
GSCALAR(heli_servo_4, "HS4_"), GGROUP(heli_servo_4, "HS4_", RC_Channel),
GSCALAR(heli_servo1_pos, "SV1_POS_"), GSCALAR(heli_servo1_pos, "SV1_POS_"),
GSCALAR(heli_servo2_pos, "SV2_POS_"), GSCALAR(heli_servo2_pos, "SV2_POS_"),
GSCALAR(heli_servo3_pos, "SV3_POS_"), GSCALAR(heli_servo3_pos, "SV3_POS_"),
GSCALAR(heli_roll_max, "ROL_MAX_"), GSCALAR(heli_roll_max, "ROL_MAX_"),
GSCALAR(heli_pitch_max, "PIT_MAX_"), GSCALAR(heli_pitch_max, "PIT_MAX_"),
GSCALAR(heli_coll_min, "COL_MIN_"), GSCALAR(heli_collective_min, "COL_MIN_"),
GSCALAR(heli_coll_max, "COL_MAX_"), GSCALAR(heli_collective_max, "COL_MAX_"),
GSCALAR(heli_coll_mid, "COL_MID_"), GSCALAR(heli_collective_mid, "COL_MID_"),
GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"), GSCALAR(heli_ext_gyro_enabled, "GYR_ENABLE_"),
GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"), GSCALAR(heli_ext_gyro_gain, "GYR_GAIN_"),
GSCALAR(heli_servo_averaging, "SV_AVG"), GSCALAR(heli_servo_averaging, "SV_AVG"),
GSCALAR(heli_servo_manual, "HSV_MAN"), GSCALAR(heli_servo_manual, "HSV_MAN"),
GSCALAR(heli_phase_angle, "H_PHANG"), GSCALAR(heli_phase_angle, "H_PHANG"),
GSCALAR(heli_coll_yaw_effect, "H_COLYAW"), GSCALAR(heli_collective_yaw_effect, "H_COLYAW"),
#endif #endif
// RC channel // 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 // variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass), GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), 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 #ifndef STABILIZE_D
# define STABILIZE_D .06 # define STABILIZE_D .2
#endif #endif
@ -567,7 +567,7 @@
// Good for smaller payload motors. // Good for smaller payload motors.
#ifndef STABILIZE_ROLL_P #ifndef STABILIZE_ROLL_P
# define STABILIZE_ROLL_P 4.5 # define STABILIZE_ROLL_P 4
#endif #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.0 # define STABILIZE_ROLL_I 0.0
@ -577,7 +577,7 @@
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 4.5 # define STABILIZE_PITCH_P 4
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.0 # define STABILIZE_PITCH_I 0.0
@ -587,7 +587,7 @@
#endif #endif
#ifndef STABILIZE_YAW_P #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 #endif
#ifndef STABILIZE_YAW_I #ifndef STABILIZE_YAW_I
# define STABILIZE_YAW_I 0.01 # define STABILIZE_YAW_I 0.01
@ -601,26 +601,26 @@
// Stabilize Rate Control // Stabilize Rate Control
// //
#ifndef RATE_ROLL_P #ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.14 # define RATE_ROLL_P 0.12
#endif #endif
#ifndef RATE_ROLL_I #ifndef RATE_ROLL_I
# define RATE_ROLL_I 0.18 # define RATE_ROLL_I 0.02
#endif #endif
#ifndef RATE_ROLL_D #ifndef RATE_ROLL_D
# define RATE_ROLL_D 0.0025 # define RATE_ROLL_D 0.008
#endif #endif
#ifndef RATE_ROLL_IMAX #ifndef RATE_ROLL_IMAX
# define RATE_ROLL_IMAX 5 // degrees # define RATE_ROLL_IMAX 5 // degrees
#endif #endif
#ifndef RATE_PITCH_P #ifndef RATE_PITCH_P
# define RATE_PITCH_P 0.14 # define RATE_PITCH_P 0.12
#endif #endif
#ifndef RATE_PITCH_I #ifndef RATE_PITCH_I
# define RATE_PITCH_I 0.18 # define RATE_PITCH_I 0.02
#endif #endif
#ifndef RATE_PITCH_D #ifndef RATE_PITCH_D
# define RATE_PITCH_D 0.0025 # define RATE_PITCH_D 0.008
#endif #endif
#ifndef RATE_PITCH_IMAX #ifndef RATE_PITCH_IMAX
# define RATE_PITCH_IMAX 5 // degrees # define RATE_PITCH_IMAX 5 // degrees

5
ArduCopter/defines.h
View File

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

22
ArduCopter/heli.pde
View File

@ -7,7 +7,7 @@
static bool heli_swash_initialised = false; static bool heli_swash_initialised = false;
static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000) 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: // heli_servo_averaging:
// 0 or 1 = no averaging, 250hz // 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)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle));
// set throttle scaling // 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 // we must be in set-up mode so mark swash as uninitialised
heli_swash_initialised = false; heli_swash_initialised = false;
@ -57,17 +57,17 @@ static void heli_init_swash()
g.heli_servo_4.set_angle(4500); g.heli_servo_4.set_angle(4500);
// ensure g.heli_coll values are reasonable // ensure g.heli_coll values are reasonable
if( g.heli_coll_min >= g.heli_coll_max ) { if( g.heli_collective_min >= g.heli_collective_max ) {
g.heli_coll_min = 1000; g.heli_collective_min = 1000;
g.heli_coll_max = 2000; 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 // 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 // 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 // pitch factors
heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); 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 ) { if( heli_swash_initialised ) {
heli_reset_swash(); 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 }else{ // regular flight mode
// check if we need to reinitialise the swash // 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); 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); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max);
coll_out = constrain(coll_out, 0, 1000); 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 // rudder feed forward based on collective
#if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator
if( !g.heli_ext_gyro_enabled ) { 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 #endif
} }

7
ArduCopter/motors_octa_quad.pde
View File

@ -40,8 +40,8 @@ static void output_motors_armed()
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
if(g.frame_orientation == X_FRAME){ if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out * .707; roll_out = (float)g.rc_1.pwm_out * 0.707;
pitch_out = g.rc_2.pwm_out * .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_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 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_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_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 motor_out[MOT_8] = g.rc_3.radio_out + roll_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK LEFT CW BOTTOM
}else{ }else{
roll_out = g.rc_1.pwm_out; roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.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_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_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 motor_out[MOT_8] = g.rc_3.radio_out - pitch_out; // APM2 OUT8 APM1 OUT11 BACK CW BOTTOM
}
// Yaw // Yaw
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW 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_4] -= g.rc_4.pwm_out; // CW
motor_out[MOT_6] -= 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 motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
}
// TODO add stability patch // TODO add stability patch
motor_out[MOT_1] = min(motor_out[MOT_1], out_max); 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){ if(g.frame_orientation == X_FRAME){
roll_out = g.rc_1.pwm_out * .707; roll_out = (float)g.rc_1.pwm_out * 0.707;
pitch_out = g.rc_2.pwm_out * .707; pitch_out = (float)g.rc_2.pwm_out * 0.707;
// left // left
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT 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: // 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; y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp;
x_actual_speed = x_actual_speed >> 1; 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 value = 0;
int temp; int temp;
int state = 0; // 0 = set rev+pos, 1 = capture min/max 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 // initialise swash plate
heli_init_swash(); heli_init_swash();
@ -497,10 +497,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
max_roll = abs(g.rc_1.control_in); max_roll = abs(g.rc_1.control_in);
if( abs(g.rc_2.control_in) > max_pitch ) if( abs(g.rc_2.control_in) > max_pitch )
max_pitch = abs(g.rc_2.control_in); max_pitch = abs(g.rc_2.control_in);
if( g.rc_3.radio_out < min_coll ) if( g.rc_3.radio_out < min_collective )
min_coll = g.rc_3.radio_out; min_collective = g.rc_3.radio_out;
if( g.rc_3.radio_out > max_coll ) if( g.rc_3.radio_out > max_collective )
max_coll = g.rc_3.radio_out; max_collective = g.rc_3.radio_out;
min_tail = min(g.rc_4.radio_out, min_tail); min_tail = min(g.rc_4.radio_out, min_tail);
max_tail = max(g.rc_4.radio_out, max_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':
case 'C': case 'C':
if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) { if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) {
g.heli_coll_mid = g.rc_3.radio_out; g.heli_collective_mid = g.rc_3.radio_out;
Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_collective_mid);
} }
break; break;
case 'd': case 'd':
@ -546,27 +546,27 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
// reset servo ranges // reset servo ranges
g.heli_roll_max = g.heli_pitch_max = 4500; g.heli_roll_max = g.heli_pitch_max = 4500;
g.heli_coll_min = 1000; g.heli_collective_min = 1000;
g.heli_coll_max = 2000; g.heli_collective_max = 2000;
g.heli_servo_4.radio_min = 1000; g.heli_servo_4.radio_min = 1000;
g.heli_servo_4.radio_max = 2000; g.heli_servo_4.radio_max = 2000;
// set sensible values in temp variables // set sensible values in temp variables
max_roll = abs(g.rc_1.control_in); max_roll = abs(g.rc_1.control_in);
max_pitch = abs(g.rc_2.control_in); max_pitch = abs(g.rc_2.control_in);
min_coll = 2000; min_collective = 2000;
max_coll = 1000; max_collective = 1000;
min_tail = max_tail = abs(g.rc_4.radio_out); min_tail = max_tail = abs(g.rc_4.radio_out);
}else{ }else{
state = 0; // switch back to normal mode state = 0; // switch back to normal mode
// double check values aren't totally terrible // 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 ) 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_coll,max_coll,min_tail,max_tail); 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{ else{
g.heli_roll_max = max_roll; g.heli_roll_max = max_roll;
g.heli_pitch_max = max_pitch; g.heli_pitch_max = max_pitch;
g.heli_coll_min = min_coll; g.heli_collective_min = min_collective;
g.heli_coll_max = max_coll; g.heli_collective_max = max_collective;
g.heli_servo_4.radio_min = min_tail; g.heli_servo_4.radio_min = min_tail;
g.heli_servo_4.radio_max = max_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_servo3_pos.save();
g.heli_roll_max.save(); g.heli_roll_max.save();
g.heli_pitch_max.save(); g.heli_pitch_max.save();
g.heli_coll_min.save(); g.heli_collective_min.save();
g.heli_coll_max.save(); g.heli_collective_max.save();
g.heli_coll_mid.save(); g.heli_collective_mid.save();
g.heli_servo_averaging.save(); g.heli_servo_averaging.save();
// return swash plate movements to attitude controller // 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("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("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 // calculate and print servo rate
if( g.heli_servo_averaging <= 1 ) { if( g.heli_servo_averaging <= 1 ) {

20
ArduCopter/system.pde
View File

@ -327,10 +327,22 @@ static void init_ardupilot()
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
Log_Write_Startup(); Log_Write_Startup();
Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(10, (float)g.pi_stabilize_roll.kP());
Log_Write_Data(11, g.pi_stabilize_pitch.kP()); Log_Write_Data(11, (float)g.pi_stabilize_roll.kI());
Log_Write_Data(12, g.pid_rate_roll.kP());
Log_Write_Data(13, g.pid_rate_pitch.kP()); 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 #endif
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");

2
ArduPlane/Attitude.pde
View File

@ -314,7 +314,6 @@ static void set_servos(void)
if (g.mix_mode == 0) { if (g.mix_mode == 0) {
g.channel_roll.calc_pwm(); g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm(); g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g_rc_function[RC_Channel_aux::k_aileron]) { 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]->servo_out = g.channel_roll.servo_out;
g_rc_function[RC_Channel_aux::k_aileron]->calc_pwm(); 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_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_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 #if THROTTLE_OUT == 0
g.channel_throttle.servo_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){ switch(packet.req_stream_id){
case MAV_DATA_STREAM_ALL: case MAV_DATA_STREAM_ALL:
streamRateRawSensors = freq; streamRateRawSensors.set_and_save_ifchanged(freq);
streamRateExtendedStatus = freq; streamRateExtendedStatus.set_and_save_ifchanged(freq);
streamRateRCChannels = freq; streamRateRCChannels.set_and_save_ifchanged(freq);
streamRateRawController = freq; streamRateRawController.set_and_save_ifchanged(freq);
streamRatePosition = freq; streamRatePosition.set_and_save_ifchanged(freq);
streamRateExtra1 = freq; streamRateExtra1.set_and_save_ifchanged(freq);
streamRateExtra2 = freq; streamRateExtra2.set_and_save_ifchanged(freq);
streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. streamRateExtra3.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: 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. // we will not continue to broadcast raw sensor data at 50Hz.
break; break;
case MAV_DATA_STREAM_EXTENDED_STATUS: case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus.set_and_save(freq); streamRateExtendedStatus.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_RC_CHANNELS: case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels.set_and_save(freq); streamRateRCChannels.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_RAW_CONTROLLER: case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController.set_and_save(freq); streamRateRawController.set_and_save_ifchanged(freq);
break; break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION: //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
@ -966,19 +966,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// break; // break;
case MAV_DATA_STREAM_POSITION: case MAV_DATA_STREAM_POSITION:
streamRatePosition.set_and_save(freq); streamRatePosition.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_EXTRA1: case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1.set_and_save(freq); streamRateExtra1.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_EXTRA2: case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2.set_and_save(freq); streamRateExtra2.set_and_save_ifchanged(freq);
break; break;
case MAV_DATA_STREAM_EXTRA3: case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3.set_and_save(freq); streamRateExtra3.set_and_save_ifchanged(freq);
break; break;
default: default:

2
ArduPlane/Parameters.h
View File

@ -68,7 +68,7 @@ public:
// //
// 130: Sensor parameters // 130: Sensor parameters
// //
k_param_IMU_calibration = 130, k_param_imu = 130, // sensor calibration
k_param_altitude_mix, k_param_altitude_mix,
k_param_airspeed_ratio, k_param_airspeed_ratio,
k_param_ground_temperature, 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 // variables not in the g class which contain EEPROM saved variables
GOBJECT(compass, "COMPASS_", Compass), GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), 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 // EEPROM addresses
#define EEPROM_MAX_ADDR 4096 #define EEPROM_MAX_ADDR 4096
// parameters get the first 1024 bytes of EEPROM, remainder is for waypoints // parameters get the first 1280 bytes of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_START_BYTE 0x500 // where in memory home WP is stored + all other WP
#define WP_SIZE 15 #define WP_SIZE 15
// fence points are stored at the end of the EEPROM // 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){ if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in; g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.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 G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
}else{ }else{
@ -169,6 +168,7 @@ static void trim_control_surfaces()
g.channel_roll.radio_trim = center; g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center; g.channel_pitch.radio_trim = center;
} }
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom // save to eeprom
g.channel_roll.save_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_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.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 G_RC_AUX(k_aileron)->radio_trim = g_rc_function[RC_Channel_aux::k_aileron]->radio_in; // Second aileron channel
} else { } else {
@ -199,8 +198,8 @@ static void trim_radio()
uint16_t center = 1500; uint16_t center = 1500;
g.channel_roll.radio_trim = center; g.channel_roll.radio_trim = center;
g.channel_pitch.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 // save to eeprom
g.channel_roll.save_eeprom(); g.channel_roll.save_eeprom();

5
Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj
View File

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

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

@ -9,7 +9,7 @@ using System.Windows.Forms;
namespace ArdupilotMega namespace ArdupilotMega
{ {
public partial class ImageLabel : UserControl public partial class ImageLabel : ContainerControl
{ {
public new event EventHandler Click; 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) if (ind != -1)
logdata = logdata.Substring(0, ind); logdata = logdata.Substring(0, ind);
try
{
while (messages.Count > 5) while (messages.Count > 5)
{ {
messages.RemoveAt(0); messages.RemoveAt(0);
} }
messages.Add(logdata + "\n"); messages.Add(logdata + "\n");
}
catch { }
mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null; 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.zg1 = new ZedGraph.ZedGraphControl();
this.lbl_winddir = new ArdupilotMega.MyLabel(); this.lbl_winddir = new ArdupilotMega.MyLabel();
this.lbl_windvel = 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.panel1 = new System.Windows.Forms.Panel();
this.TXT_lat = new ArdupilotMega.MyLabel(); this.TXT_lat = new ArdupilotMega.MyLabel();
this.Zoomlevel = new System.Windows.Forms.NumericUpDown(); this.Zoomlevel = new System.Windows.Forms.NumericUpDown();
@ -1320,7 +1320,7 @@
private ArdupilotMega.MyLabel TXT_long; private ArdupilotMega.MyLabel TXT_long;
private ArdupilotMega.MyLabel TXT_alt; private ArdupilotMega.MyLabel TXT_alt;
private System.Windows.Forms.CheckBox CHK_autopan; private System.Windows.Forms.CheckBox CHK_autopan;
private GMap.NET.WindowsForms.GMapControl gMapControl1; private myGMAP gMapControl1;
private ZedGraph.ZedGraphControl zg1; private ZedGraph.ZedGraphControl zg1;
private System.Windows.Forms.TabControl tabControl1; private System.Windows.Forms.TabControl tabControl1;
private System.Windows.Forms.TabPage tabGauges; private System.Windows.Forms.TabPage tabGauges;

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

@ -369,7 +369,7 @@ namespace ArdupilotMega.GCSViews
try try
{ {
//Console.WriteLine(DateTime.Now.Millisecond); //Console.WriteLine(DateTime.Now.Millisecond);
MainV2.cs.UpdateCurrentSettings(bindingSource1); updateBindingSource();
//Console.WriteLine(DateTime.Now.Millisecond + " done "); //Console.WriteLine(DateTime.Now.Millisecond + " done ");
if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked == true) if (tunning.AddMilliseconds(50) < DateTime.Now && CB_tuning.Checked == true)
@ -402,6 +402,11 @@ namespace ArdupilotMega.GCSViews
{ {
gMapControl1.HoldInvalidation = true; gMapControl1.HoldInvalidation = true;
while (gMapControl1.inOnPaint == true)
{
System.Threading.Thread.Sleep(1);
}
if (trackPoints.Count > int.Parse(MainV2.config["NUM_tracklength"].ToString())) if (trackPoints.Count > int.Parse(MainV2.config["NUM_tracklength"].ToString()))
{ {
trackPoints.RemoveRange(0, 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"); 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) private void updateMapPosition(PointLatLng currentloc)
{ {
this.BeginInvoke((MethodInvoker)delegate() 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_distance = new System.Windows.Forms.Label();
this.lbl_homedist = new System.Windows.Forms.Label(); this.lbl_homedist = new System.Windows.Forms.Label();
this.lbl_prevdist = 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.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components);
this.deleteWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.deleteWPToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem();
this.loiterToolStripMenuItem = 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 panelWaypoints;
private BSE.Windows.Forms.Panel panelAction; private BSE.Windows.Forms.Panel panelAction;
private System.Windows.Forms.Panel panelMap; private System.Windows.Forms.Panel panelMap;
private GMap.NET.WindowsForms.GMapControl MainMap; private myGMAP MainMap;
private MyTrackBar trackBar1; private MyTrackBar trackBar1;
private System.Windows.Forms.Label label11; private System.Windows.Forms.Label label11;
private System.Windows.Forms.Label lbl_distance; 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; 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; const int AEROSIMRC_MAX_CHANNELS = 39;
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -263,6 +288,11 @@ namespace ArdupilotMega.GCSViews
SetupUDPRecv(); SetupUDPRecv();
if (chkSensor.Checked)
{
SITLSEND = new UdpClient(simIP, 5501);
}
if (RAD_softXplanes.Checked) if (RAD_softXplanes.Checked)
{ {
SetupUDPXplanes(); SetupUDPXplanes();
@ -286,12 +316,9 @@ namespace ArdupilotMega.GCSViews
System.Threading.Thread.Sleep(2000); System.Threading.Thread.Sleep(2000);
SITLSEND = new UdpClient(simIP, 5501);
SetupTcpJSBSim(); // old style SetupTcpJSBSim(); // old style
} }
SetupUDPXplanes(); // fg udp style SetupUDPXplanes(); // fg udp style
SetupUDPMavLink(); // pass traffic - raw SetupUDPMavLink(); // pass traffic - raw
} }
@ -545,7 +572,7 @@ namespace ArdupilotMega.GCSViews
Console.WriteLine("REQ streams - sim"); Console.WriteLine("REQ streams - sim");
try 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 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); int recv = SimulatorRECV.ReceiveFrom(udpdata, ref Remote);
RECVprocess(udpdata, recv, comPort); RECVprocess(udpdata, recv, comPort);
hzcount++;
} }
} }
catch catch
@ -598,7 +627,7 @@ namespace ArdupilotMega.GCSViews
if ((DateTime.Now - simsendtime).TotalMilliseconds > 19) if ((DateTime.Now - simsendtime).TotalMilliseconds > 19)
{ {
hzcount++; //hzcount++;
simsendtime = DateTime.Now; simsendtime = DateTime.Now;
processArduPilot(); processArduPilot();
} }
@ -607,7 +636,7 @@ namespace ArdupilotMega.GCSViews
if (hzcounttime.Second != DateTime.Now.Second) if (hzcounttime.Second != DateTime.Now.Second)
{ {
// Console.WriteLine("SIM hz {0}", hzcount); Console.WriteLine("SIM hz {0}", hzcount);
hzcount = 0; hzcount = 0;
hzcounttime = DateTime.Now; hzcounttime = DateTime.Now;
} }
@ -758,26 +787,13 @@ namespace ArdupilotMega.GCSViews
float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds); float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds);
float ydiff = (float)((att.yaw - oldatt.yaw) / 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; 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(att.rollspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 ygyro = Constrain(att.pitchspeed * 1000.0, Int16.MinValue, Int16.MaxValue);
Int16 xgyro = Constrain(rdiff * 1000.0, Int16.MinValue, Int16.MaxValue); Int16 zgyro = Constrain(att.yawspeed * 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);
oldtime = DateTime.Now; oldtime = DateTime.Now;
@ -849,7 +865,7 @@ namespace ArdupilotMega.GCSViews
gps.v = ((float)(DATA[3][7] * 0.44704)); gps.v = ((float)(DATA[3][7] * 0.44704));
#endif #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; 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 #if MAVLINK10
TimeSpan gpsspan = DateTime.Now - lastgpsupdate; 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) 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. // 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. // 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; x_NED = (x) * -1.0;
y_NED = radians(y) * 1.0; y_NED = (y) * 1.0;
z_NED = radians(z) * -1.0; z_NED = (z) * -1.0;
// Next calculate cos & sin of angles for use in the transformation matrix. // Next calculate cos & sin of angles for use in the transformation matrix.
// r, p & y subscripts stand for roll pitch and yaw. // r, p & y subscripts stand for roll pitch and yaw.
Cr = Math.Cos(radians(phi)); Cr = Math.Cos((phi));
Cp = Math.Cos(radians(theta)); Cp = Math.Cos((theta));
Cy = Math.Cos(radians(psi)); Cy = Math.Cos((psi));
Sr = Math.Sin(radians(phi)); Sr = Math.Sin((phi));
Sp = Math.Sin(radians(theta)); Sp = Math.Sin((theta));
Sy = Math.Sin(radians(psi)); Sy = Math.Sin((psi));
// Next we need to rotate our accelerations from the NED reference frame, into the body fixed reference frame // 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); 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); 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; started = true;
} }
bool inOnPaint = false;
protected override void OnPaint(PaintEventArgs e) protected override void OnPaint(PaintEventArgs e)
{ {
//GL.Enable(EnableCap.AlphaTest) //GL.Enable(EnableCap.AlphaTest)
@ -261,6 +263,14 @@ namespace hud
return; return;
} }
if (inOnPaint)
{
Console.WriteLine("Was in onpaint Hud th:" + System.Threading.Thread.CurrentThread.Name);
return;
}
inOnPaint = true;
starttime = DateTime.Now; starttime = DateTime.Now;
try try
@ -284,6 +294,8 @@ namespace hud
} }
catch (Exception ex) { Console.WriteLine(ex.ToString()); } catch (Exception ex) { Console.WriteLine(ex.ToString()); }
inOnPaint = false;
count++; count++;
huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds; huddrawtime += (int)(DateTime.Now - starttime).TotalMilliseconds;
@ -1479,7 +1491,7 @@ namespace hud
if (e == null || P == null || pth == null || pth.PointCount == 0) if (e == null || P == null || pth == null || pth.PointCount == 0)
return; return;
if (!ArdupilotMega.MainV2.MONO) //if (!ArdupilotMega.MainV2.MONO)
e.DrawPath(P, pth); e.DrawPath(P, pth);
//Draw the face //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); 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)); 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); Application.SetCompatibleTextRenderingDefault(false);
try try
{ {
System.Threading.Thread.CurrentThread.Name = "Base Thread";
Application.Run(new MainV2()); Application.Run(new MainV2());
} }
catch (Exception ex) { Console.WriteLine(ex.ToString()); } 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: // by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")] // [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")] [assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.1.36")] [assembly: AssemblyFileVersion("1.1.37")]
[assembly: NeutralResourcesLanguageAttribute("")] [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("sin()", v_out = sin(v_f), 20);
TIMEIT("cos()", v_out = cos(v_f), 20); TIMEIT("cos()", v_out = cos(v_f), 20);
TIMEIT("tan()", v_out = tan(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("iadd8", v_out_8 += v_8, 100);
TIMEIT("isub8", 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 SWITCH_ENABLE 0
MAG_ENABLE 1 MAG_ENABLE 1
TRIM_ARSPD_CM 2200 TRIM_ARSPD_CM 2200
TRIM_PITCH_CD -1000 TRIM_PITCH_CD 0
ARSPD_ENABLE 1 ARSPD_ENABLE 1
ARSP2PTCH_I 0.1 ARSP2PTCH_I 0.1
ARSPD_FBW_MAX 30 ARSPD_FBW_MAX 30

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

@ -86,7 +86,7 @@
<location unit="IN"> <location unit="IN">
<x> 68.9 </x> <x> 68.9 </x>
<y> 0 </y> <y> 0 </y>
<z> -3 </z> <z> -13.1 </z>
</location> </location>
<static_friction> 8.0 </static_friction> <static_friction> 8.0 </static_friction>
<dynamic_friction> 5.0 </dynamic_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 from math import sin, cos, sqrt, radians
# work out what the accelerometer would see # 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)) yAccel = -sin(radians(self.roll)) * cos(radians(self.pitch))
zAccel = -cos(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)) 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) { } else if (_integrator > _imax) {
_integrator = _imax; _integrator = _imax;
} }
}
return _integrator; return _integrator;
}
return 0;
} }
int32_t AC_PID::get_d(int32_t input, float dt) 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 // Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) { if ((fabs(_kd) > 0) && (dt > 0)) {
_derivative = (error - _last_error) / dt;
_derivative = (error - _last_input) / dt;
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // 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); (dt / ( _filter + dt)) * (_derivative - _last_derivative);
// update state // update state
_last_input = error; _last_error = error;
_last_derivative = _derivative; _last_derivative = _derivative;
// add in derivative component // 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 // reader below could get a division by zero
_sum[ch] = 0; _sum[ch] = 0;
_count[ch] = 1; _count[ch] = 1;
last_ch6_micros = tnow;
} }
_sum[ch] += (v >> 3); _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; SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
if (TEMP < 2000){ // second order temperature compensation 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 Aux_64 = (TEMP-2000)*(TEMP-2000);
int64_t OFF2 = 5*Aux_64/2; int64_t OFF2 = (5*Aux_64)>>1;
int64_t SENS2 = 5*Aux_64/4; int64_t SENS2 = (5*Aux_64)>>2;
TEMP = TEMP - T2; TEMP = TEMP - T2;
OFF = OFF - OFF2; OFF = OFF - OFF2;
SENS = SENS - SENS2; SENS = SENS - SENS2;

7
libraries/AP_Common/AP_Param.cpp
View File

@ -15,6 +15,7 @@
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h>
#include <math.h> #include <math.h>
#include <string.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; 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 // find the info structure given a header and a group_info table
// return the Info structure and a pointer to the variables storage // 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, 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/pgmspace.h>
#include <avr/eeprom.h> #include <avr/eeprom.h>
#include <AP_Math.h>
#define AP_MAX_NAME_SIZE 15 #define AP_MAX_NAME_SIZE 15
@ -80,6 +79,9 @@ public:
// wrong version is found // wrong version is found
static bool setup(const struct Info *info, uint8_t num_vars, uint16_t eeprom_size); 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. /// 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. /// If the variable has no name, the buffer will contain an empty string.
@ -259,6 +261,19 @@ public:
return save(); 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. /// 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. /// 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. /// 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; return v;
} }
/// Copy assignment from T is equivalent to ::set. /// Copy assignment from T is equivalent to ::set.
/// ///
AP_ParamT<T,PT>& operator=(T v) { AP_ParamV<T,PT>& operator=(T v) {
_value = v; _value = v;
return *this; 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(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32 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; #define AP_PARAMDEFA(_t, _n, _size, _pt) typedef AP_ParamA<_t, _size, _pt> AP_##_n;
AP_PARAMDEFA(float, Vector6f, 6, AP_PARAM_VECTOR6F); 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 /// Rely on built in casting for other variable types
/// to minimize template creation and save memory /// to minimize template creation and save memory
#define AP_Uint8 AP_Int8 #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_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 _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){ if(_centripetal &&
accel_adjust(); // Remove _centripetal acceleration. _gps != NULL &&
_gps->status() == GPS::GPS_OK) {
// Remove _centripetal acceleration.
accel_adjust();
} }
#if OUTPUTMODE == 1 #if OUTPUTMODE == 1
@ -174,9 +177,7 @@ AP_DCM::accel_adjust(void)
{ {
Vector3f veloc, temp; Vector3f veloc, temp;
if (_gps) {
veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units 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 // 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" #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 void
AP_IMU_INS::init( Start_style style, AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t), 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 ay(const float v) { _sensor_cal[4] = v; }
virtual void az(const float v) { _sensor_cal[5] = v; } virtual void az(const float v) { _sensor_cal[5] = v; }
static const struct AP_Param::GroupInfo var_info[];
private: private:
AP_InertialSensor *_ins; ///< INS provides an axis and unit correct sensor source. 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), virtual void _init_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL); ///< no-save implementation 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" #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. /* Empty implementations for the IMU functions.
* Although these will never be used, in certain situations with * Although these will never be used, in certain situations with
* optimizations turned off, having empty implementations in an object * 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 ay(const float v);
virtual void az(const float v); virtual void az(const float v);
static const struct AP_Param::GroupInfo var_info[];
protected: protected:
AP_Vector6f _sensor_cal; ///< Calibrated sensor offsets
/// Most recent accelerometer reading obtained by ::update /// Most recent accelerometer reading obtained by ::update
Vector3f _accel; Vector3f _accel;
Vector3f _accel_filtered; Vector3f _accel_filtered;

5
libraries/AP_Math/AP_Math.h
View File

@ -2,8 +2,13 @@
// Assorted useful math operations for ArduPilot(Mega) // Assorted useful math operations for ArduPilot(Mega)
#include <AP_Common.h>
#include <stdint.h> #include <stdint.h>
#include "vector2.h" #include "vector2.h"
#include "vector3.h" #include "vector3.h"
#include "matrix3.h" #include "matrix3.h"
#include "polygon.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_roll = roll;
_last_pitch = pitch; _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) 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_Math.h>
#include <AP_DCM.h>
#include <AP_Common.h> #include <AP_Common.h>
// standard rotation matrices // standard rotation matrices

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

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