Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

new rate control, FBW fixes, new safety. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1556 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-25 05:53:36 +00:00
parent 000eaf6a68
commit e977116f9b
13 changed files with 240 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
ArduCopterMega/ArduCopterMega.pde
View File

@ -146,6 +146,8 @@ PID pid_stabilize_roll (EE_GAIN_4);
PID pid_stabilize_pitch (EE_GAIN_5); PID pid_stabilize_pitch (EE_GAIN_5);
PID pid_yaw (EE_GAIN_6); PID pid_yaw (EE_GAIN_6);
Vector3f omega;
// roll pitch // roll pitch
float stabilize_dampener; float stabilize_dampener;
int max_stabilize_dampener; int max_stabilize_dampener;
@ -202,7 +204,9 @@ int airspeed; // m/s * 100
// Throttle Failsafe // Throttle Failsafe
// ------------------ // ------------------
boolean motor_armed; boolean motor_armed = false;
boolean motor_auto_safe = false;
byte throttle_failsafe_enabled; byte throttle_failsafe_enabled;
int throttle_failsafe_value; int throttle_failsafe_value;
byte throttle_failsafe_action; byte throttle_failsafe_action;
@ -254,9 +258,9 @@ int temp_unfilt;
// From IMU // From IMU
// -------- // --------
long roll_sensor; // degrees * 100 //long roll_sensor; // degrees * 100
long pitch_sensor; // degrees * 100 //long pitch_sensor; // degrees * 100
long yaw_sensor; // degrees * 100 //long yaw_sensor; // degrees * 100
float roll; // radians float roll; // radians
float pitch; // radians float pitch; // radians
float yaw; // radians float yaw; // radians
@ -531,6 +535,10 @@ void medium_loop()
navigate(); navigate();
} }
// calc pitch and roll to target
// -----------------------------
calc_nav();
break; break;
// command processing // command processing
@ -557,7 +565,7 @@ void medium_loop()
medium_loopCounter++; medium_loopCounter++;
if (log_bitmask & MASK_LOG_ATTITUDE_MED && (log_bitmask & MASK_LOG_ATTITUDE_FAST == 0)) if (log_bitmask & MASK_LOG_ATTITUDE_MED && (log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
if (log_bitmask & MASK_LOG_CTUN) if (log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
@ -606,7 +614,7 @@ void medium_loop()
if (log_bitmask & MASK_LOG_ATTITUDE_FAST) if (log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)roll_sensor, (int)pitch_sensor, (int)yaw_sensor); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
if (log_bitmask & MASK_LOG_RAW) if (log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
@ -776,8 +784,12 @@ void update_current_flight_mode(void)
// ------------------------------------ // ------------------------------------
auto_yaw(); auto_yaw();
// mix in user control
control_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
// apply throttle control // apply throttle control
output_auto_throttle(); output_auto_throttle();
@ -787,6 +799,42 @@ void update_current_flight_mode(void)
}else{ }else{
switch(control_mode){ switch(control_mode){
case ACRO:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
// Yaw control
output_manual_yaw();
// apply throttle control
output_manual_throttle();
// mix in user control
control_nav_mixer();
// perform rate or stabilzation
// ----------------------------
// Roll control
if(abs(rc_1.control_in) >= ACRO_RATE_TRIGGER){
output_rate_roll(); // rate control yaw
}else{
output_stabilize_roll(); // hold yaw
}
// Roll control
if(abs(rc_2.control_in) >= ACRO_RATE_TRIGGER){
output_rate_pitch(); // rate control yaw
}else{
output_stabilize_pitch(); // hold yaw
}
break;
case STABILIZE: case STABILIZE:
// Intput Pitch, Roll, Yaw and Throttle // Intput Pitch, Roll, Yaw and Throttle
@ -804,12 +852,15 @@ void update_current_flight_mode(void)
// apply throttle control // apply throttle control
output_manual_throttle(); output_manual_throttle();
// mix in user control
control_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
break; break;
case FBW: case FBW:
// we are currently using manual throttle during alpha testing. // we are currently using manual throttle during alpha testing.
fbw_timer++; fbw_timer++;
//call at 5 hz //call at 5 hz
@ -840,8 +891,12 @@ void update_current_flight_mode(void)
// apply throttle control // apply throttle control
output_manual_throttle(); output_manual_throttle();
// apply nav_pitch and nav_roll to output
fbw_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
break; break;
case ALT_HOLD: case ALT_HOLD:
@ -867,8 +922,12 @@ void update_current_flight_mode(void)
// apply throttle control // apply throttle control
output_auto_throttle(); output_auto_throttle();
// mix in user control
control_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
break; break;
case RTL: case RTL:
@ -889,8 +948,12 @@ void update_current_flight_mode(void)
// apply throttle control // apply throttle control
output_auto_throttle(); output_auto_throttle();
// mix in user control
control_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
break; break;
case POSITION_HOLD: case POSITION_HOLD:
@ -911,11 +974,16 @@ void update_current_flight_mode(void)
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
// apply throttle control // apply throttle control
output_auto_throttle(); output_auto_throttle();
// mix in user control
control_nav_mixer();
// perform stabilzation // perform stabilzation
output_stabilize(); output_stabilize_roll();
output_stabilize_pitch();
break; break;
default: default:
@ -936,8 +1004,8 @@ void update_navigation()
if(control_mode == AUTO){ if(control_mode == AUTO){
verify_must(); verify_must();
verify_may(); verify_may();
}else{
}else{
switch(control_mode){ switch(control_mode){
case RTL: case RTL:
update_crosstrack(); update_crosstrack();
@ -950,9 +1018,7 @@ void update_navigation()
void read_AHRS(void) void read_AHRS(void)
{ {
// Perform IMU calculations and get attitude info // Perform IMU calculations and get attitude info
//----------------------------------------------------- //-----------------------------------------------
dcm.update_DCM(G_Dt); dcm.update_DCM(G_Dt);
roll_sensor = dcm.roll_sensor; omega = dcm.get_gyro();
pitch_sensor = dcm.pitch_sensor;
yaw_sensor = dcm.yaw_sensor;
} }

91
ArduCopterMega/Attitude.pde
View File

@ -9,28 +9,34 @@ void init_pids()
} }
void output_stabilize() void control_nav_mixer()
{ {
float roll_error, pitch_error;
Vector3f omega = dcm.get_gyro();
float rate;
int dampener;
// control +- 45° is mixed with the navigation request by the Autopilot // control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter // output is in degrees = target pitch and roll of copter
rc_1.servo_out = rc_1.control_mix(nav_roll); rc_1.servo_out = rc_1.control_mix(nav_roll);
rc_2.servo_out = rc_2.control_mix(nav_pitch); rc_2.servo_out = rc_2.control_mix(nav_pitch);
}
roll_error = rc_1.servo_out - roll_sensor; void fbw_nav_mixer()
pitch_error = rc_2.servo_out - pitch_sensor; {
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
rc_1.servo_out = nav_roll;
rc_2.servo_out = nav_pitch;
}
void output_stabilize_roll()
{
float error, rate;
int dampener;
error = rc_1.servo_out - dcm.roll_sensor;
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
roll_error = constrain(roll_error, -2500, 2500); error = constrain(error, -2500, 2500);
pitch_error = constrain(pitch_error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel // write out angles back to servo out - this will be converted to PWM by RC_Channel
rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, delta_ms_fast_loop, 1.0); rc_1.servo_out = pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0);
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation: // We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
@ -40,7 +46,26 @@ void output_stabilize()
rate = degrees(omega.x) * 100.0; // 6rad = 34377 rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000 dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
}
void output_stabilize_pitch()
{
float error, rate;
int dampener;
error = rc_2.servo_out - dcm.pitch_sensor;
// limit the error we're feeding to the PID
error = constrain(error, -2500, 2500);
// write out angles back to servo out - this will be converted to PWM by RC_Channel
rc_2.servo_out = pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates
// omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.y) * 100.0; // 6rad = 34377 rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000 dampener = (rate * stabilize_dampener); // 34377 * .175 = 6000
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
@ -51,18 +76,14 @@ clear_yaw_control()
{ {
//Serial.print("Clear "); //Serial.print("Clear ");
rate_yaw_flag = false; // exit rate_yaw_flag rate_yaw_flag = false; // exit rate_yaw_flag
nav_yaw = yaw_sensor; // save our Yaw nav_yaw = dcm.yaw_sensor; // save our Yaw
yaw_error = 0; yaw_error = 0;
} }
void output_yaw_with_hold(boolean hold) void output_yaw_with_hold(boolean hold)
{ {
Vector3f omega = dcm.get_gyro();
if(hold){ if(hold){
// yaw hold // look to see if we have exited rate control properly - ie stopped turning
if(rate_yaw_flag){ if(rate_yaw_flag){
// we are still in motion from rate control // we are still in motion from rate control
if(fabs(omega.y) < .15){ if(fabs(omega.y) < .15){
@ -72,8 +93,6 @@ void output_yaw_with_hold(boolean hold)
// return to rate control until we slow down. // return to rate control until we slow down.
hold = false; hold = false;
} }
}else{
} }
}else{ }else{
@ -86,9 +105,8 @@ void output_yaw_with_hold(boolean hold)
} }
if(hold){ if(hold){
// try and hold the current nav_yaw setting // try and hold the current nav_yaw setting
yaw_error = nav_yaw - yaw_sensor; // +- 60° yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
yaw_error = wrap_180(yaw_error); yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
@ -105,7 +123,6 @@ void output_yaw_with_hold(boolean hold)
rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
}else{ }else{
// rate control // rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! rate = constrain(rate, -36000, 36000); // limit to something fun!
@ -119,11 +136,29 @@ void output_yaw_with_hold(boolean hold)
void output_rate_roll()
void output_rate_control()
{ {
/* // rate control
Vector3f omega = dcm.get_gyro(); long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
rc_1.servo_out = pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
rc_1.servo_out = constrain(rc_1.servo_out, -2400, 2400); // limit to 2400
}
void output_rate_pitch()
{
// rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
rc_2.servo_out = pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
rc_2.servo_out = constrain(rc_2.servo_out, -2400, 2400); // limit to 2400
}
/*
rc_1.servo_out = rc_2.control_in; rc_1.servo_out = rc_2.control_in;
rc_2.servo_out = rc_2.control_in; rc_2.servo_out = rc_2.control_in;
@ -140,7 +175,7 @@ void output_rate_control()
rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT);
*/ */
} //}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.

6
ArduCopterMega/Camera.pde
View File

@ -15,13 +15,13 @@ void
camera_stabilization() camera_stabilization()
{ {
rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
rc_camera_pitch.servo_out = rc_camera_pitch.control_mix(pitch_sensor / 2); rc_camera_pitch.servo_out = rc_camera_pitch.control_mix(dcm.pitch_sensor / 2);
rc_camera_pitch.calc_pwm(); rc_camera_pitch.calc_pwm();
APM_RC.OutputCh(CH_5,rc_camera_pitch.radio_out); APM_RC.OutputCh(CH_5, rc_camera_pitch.radio_out);
//If you want to do control mixing use this function. //If you want to do control mixing use this function.
// set servo_out to the control input from radio // set servo_out to the control input from radio
//rc_camera_yaw = rc_2.control_mix(pitch_sensor); //rc_camera_yaw = rc_2.control_mix(dcm.pitch_sensor);
//rc_camera_yaw.calc_pwm(); //rc_camera_yaw.calc_pwm();
} }

6
ArduCopterMega/GCS_Ardupilot.pde
View File

@ -78,9 +78,9 @@ void print_attitude(void)
SendSer(",THH:"); SendSer(",THH:");
SendSer(rc_3.servo_out, DEC); SendSer(rc_3.servo_out, DEC);
SendSer (",RLL:"); SendSer (",RLL:");
SendSer(roll_sensor / 100, DEC); SendSer(dcm.roll_sensor / 100, DEC);
SendSer (",PCH:"); SendSer (",PCH:");
SendSer(pitch_sensor / 100, DEC); SendSer(dcm.pitch_sensor / 100, DEC);
SendSerln(",***"); SendSerln(",***");
} }
@ -107,7 +107,7 @@ void print_position(void)
SendSer(",ALH:"); SendSer(",ALH:");
SendSer(next_WP.alt/100,DEC); SendSer(next_WP.alt/100,DEC);
SendSer(",CRS:"); SendSer(",CRS:");
SendSer(yaw_sensor/100,DEC); SendSer(dcm.yaw_sensor/100,DEC);
SendSer(",BER:"); SendSer(",BER:");
SendSer(target_bearing/100,DEC); SendSer(target_bearing/100,DEC);
SendSer(",WPN:"); SendSer(",WPN:");

8
ArduCopterMega/GCS_Standard.pde
View File

@ -71,13 +71,13 @@ void send_message(byte id, long param) {
case MSG_ATTITUDE: // ** Attitude message case MSG_ATTITUDE: // ** Attitude message
mess_buffer[0] = 0x06; mess_buffer[0] = 0x06;
ck = 6; ck = 6;
tempint = roll_sensor; // Roll (degrees * 100) tempint = dcm.roll_sensor; // Roll (degrees * 100)
mess_buffer[3] = tempint & 0xff; mess_buffer[3] = tempint & 0xff;
mess_buffer[4] = (tempint >> 8) & 0xff; mess_buffer[4] = (tempint >> 8) & 0xff;
tempint = pitch_sensor; // Pitch (degrees * 100) tempint = dcm.pitch_sensor; // Pitch (degrees * 100)
mess_buffer[5] = tempint & 0xff; mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff; mess_buffer[6] = (tempint >> 8) & 0xff;
tempint = yaw_sensor; // Yaw (degrees * 100) tempint = dcm.yaw_sensor; // Yaw (degrees * 100)
mess_buffer[7] = tempint & 0xff; mess_buffer[7] = tempint & 0xff;
mess_buffer[8] = (tempint >> 8) & 0xff; mess_buffer[8] = (tempint >> 8) & 0xff;
break; break;
@ -105,7 +105,7 @@ void send_message(byte id, long param) {
mess_buffer[13] = tempint & 0xff; mess_buffer[13] = tempint & 0xff;
mess_buffer[14] = (tempint >> 8) & 0xff; mess_buffer[14] = (tempint >> 8) & 0xff;
tempint = yaw_sensor; // Ground Course in degreees * 100 in 2 bytes tempint = dcm.yaw_sensor; // Ground Course in degreees * 100 in 2 bytes
mess_buffer[15] = tempint & 0xff; mess_buffer[15] = tempint & 0xff;
mess_buffer[16] = (tempint >> 8) & 0xff; mess_buffer[16] = (tempint >> 8) & 0xff;

2
ArduCopterMega/HIL_output.pde
View File

@ -39,7 +39,7 @@ void output_HIL_(void)
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8, 9 output_int((int)wp_distance); // 4 bytes 8, 9
output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)roll_sensor); // 6 bytes 12,13 output_int((int)dcm.roll_sensor); // 6 bytes 12,13
output_int((int)loiter_total); // 7 bytes 14,15 output_int((int)loiter_total); // 7 bytes 14,15
output_byte(wp_index); // 8 bytes 16 output_byte(wp_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17 output_byte(control_mode); // 9 bytes 17

6
ArduCopterMega/Log.pde
View File

@ -263,10 +263,10 @@ void Log_Write_Control_Tuning()
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(rc_1.servo_out)); DataFlash.WriteInt((int)(rc_1.servo_out));
DataFlash.WriteInt((int)nav_roll); DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)roll_sensor); DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)(rc_2.servo_out)); DataFlash.WriteInt((int)(rc_2.servo_out));
DataFlash.WriteInt((int)nav_pitch); DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)pitch_sensor); DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(rc_3.servo_out)); DataFlash.WriteInt((int)(rc_3.servo_out));
DataFlash.WriteInt((int)(rc_4.servo_out)); DataFlash.WriteInt((int)(rc_4.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000)); DataFlash.WriteInt((int)(accel.y * 10000));
@ -279,7 +279,7 @@ void Log_Write_Nav_Tuning()
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG); DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)yaw_sensor); DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)wp_distance); DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing); DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing); DataFlash.WriteInt((uint16_t)nav_bearing);

5
ArduCopterMega/config.h
View File

@ -272,6 +272,11 @@
# define ACRO_RATE_YAW_IMAX 0 # define ACRO_RATE_YAW_IMAX 0
#endif #endif
#ifndef ACRO_RATE_TRIGGER
# define ACRO_RATE_TRIGGER 4200
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// STABILZE Angle Control // STABILZE Angle Control

5
ArduCopterMega/flight_control.pde
View File

@ -82,6 +82,7 @@ void auto_yaw()
picth and roll control picth and roll control
****************************************************************/ ****************************************************************/
/*// how hard to tilt towards the target /*// how hard to tilt towards the target
// ----------------------------------- // -----------------------------------
void calc_nav_pid() void calc_nav_pid()
@ -98,7 +99,7 @@ void calc_nav_pitch()
{ {
// how hard to pitch to target // how hard to pitch to target
long angle = wrap_360(nav_bearing - yaw_sensor); long angle = wrap_360(nav_bearing - dcm.yaw_sensor);
bool rev = false; bool rev = false;
float roll_out; float roll_out;
@ -119,7 +120,7 @@ void calc_nav_pitch()
// -------------------------------------------------- // --------------------------------------------------
void calc_nav_roll() void calc_nav_roll()
{ {
long angle = wrap_360(nav_bearing - yaw_sensor); long angle = wrap_360(nav_bearing - dcm.yaw_sensor);
bool rev = false; bool rev = false;
float roll_out; float roll_out;

67
ArduCopterMega/motors.pde
View File

@ -32,13 +32,14 @@ void arm_motors()
/***************************************** /*****************************************
* Set the flight control servos based on the current calculated values * Set the flight control servos based on the current calculated values
*****************************************/ *****************************************/
void set_servos_4(void) void
set_servos_4()
{ {
static byte num; static byte num;
static byte counteri; static byte counteri;
// Quadcopter mix // Quadcopter mix
if (motor_armed == true) { if (motor_armed == true && motor_auto_safe == true) {
int out_min = rc_3.radio_min; int out_min = rc_3.radio_min;
// Throttle is 0 to 1000 only // Throttle is 0 to 1000 only
@ -65,6 +66,12 @@ void set_servos_4(void)
motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out; motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out;
motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out;
motor_out[RIGHT] += rc_4.pwm_out;
motor_out[LEFT] += rc_4.pwm_out;
motor_out[FRONT] -= rc_4.pwm_out;
motor_out[BACK] -= rc_4.pwm_out;
}else if(frame_type == X_FRAME){ }else if(frame_type == X_FRAME){
int roll_out = rc_1.pwm_out / 2; int roll_out = rc_1.pwm_out / 2;
@ -75,6 +82,13 @@ void set_servos_4(void)
motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out;
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out;
//Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
motor_out[RIGHT] += rc_4.pwm_out;
motor_out[LEFT] += rc_4.pwm_out;
motor_out[FRONT] -= rc_4.pwm_out;
motor_out[BACK] -= rc_4.pwm_out;
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
}else if(frame_type == TRI_FRAME){ }else if(frame_type == TRI_FRAME){
@ -100,13 +114,22 @@ void set_servos_4(void)
int roll_out = (float)rc_1.pwm_out * .866; int roll_out = (float)rc_1.pwm_out * .866;
int pitch_out = rc_2.pwm_out / 2; int pitch_out = rc_2.pwm_out / 2;
motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out + rc_4.pwm_out; // CCW motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; // CCW
motor_out[RIGHTFRONT] = rc_3.radio_out - roll_out + pitch_out - rc_4.pwm_out; // CW motor_out[RIGHTFRONT] = rc_3.radio_out - roll_out + pitch_out; // CW
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out - rc_4.pwm_out; // CW motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; // CW
motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out + rc_4.pwm_out; // CCW motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out; // CCW
motor_out[LEFTBACK] = rc_3.radio_out + roll_out - pitch_out; // CW
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; // CCW
motor_out[FRONT] += rc_4.pwm_out; // CCW
motor_out[RIGHTFRONT] -= rc_4.pwm_out; // CW
motor_out[LEFT] -= rc_4.pwm_out; // CW
motor_out[RIGHT] += rc_4.pwm_out; // CCW
motor_out[LEFTBACK] += rc_4.pwm_out; // CW
motor_out[BACK] -= rc_4.pwm_out; // CCW
motor_out[LEFTBACK] = rc_3.radio_out + roll_out - pitch_out + rc_4.pwm_out; // CW
motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out - rc_4.pwm_out; // CCW
/* /*
if(counteri == 5) { if(counteri == 5) {
Serial.printf(" %d %d \n%d %d %d %d \n %d %d \n\n", motor_out[FRONT], motor_out[RIGHTFRONT], motor_out[LEFT], motor_out[RIGHT], roll_out, pitch_out, motor_out[LEFTBACK], motor_out[BACK]); Serial.printf(" %d %d \n%d %d %d %d \n %d %d \n\n", motor_out[FRONT], motor_out[RIGHTFRONT], motor_out[LEFT], motor_out[RIGHT], roll_out, pitch_out, motor_out[LEFTBACK], motor_out[BACK]);
@ -120,18 +143,6 @@ void set_servos_4(void)
} }
//Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
if((frame_type == PLUS_FRAME) || (frame_type == X_FRAME)){
motor_out[RIGHT] += rc_4.pwm_out;
motor_out[LEFT] += rc_4.pwm_out;
motor_out[FRONT] -= rc_4.pwm_out;
motor_out[BACK] -= rc_4.pwm_out;
}
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]);
motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max); motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max);
motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max); motor_out[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max);
@ -140,9 +151,9 @@ void set_servos_4(void)
num++; num++;
if (num > 50){ if (num > 10){
num = 0; num = 0;
Serial.print("!");
//debugging with Channel 6 //debugging with Channel 6
/* /*
@ -196,8 +207,18 @@ void set_servos_4(void)
} }
} }
}else{ }else{
num++;
if (num > 10){
num = 0;
Serial.print("-");
}
if(rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_safe = true;
}
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, rc_3.radio_min); APM_RC.OutputCh(CH_1, rc_3.radio_min);

7
ArduCopterMega/navigation.pde
View File

@ -39,11 +39,6 @@ void navigate()
// control mode specific updates to nav_bearing // control mode specific updates to nav_bearing
// -------------------------------------------- // --------------------------------------------
update_navigation(); update_navigation();
// calc pitch and roll to target
// -----------------------------
calc_nav();
} }
#define DIST_ERROR_MAX 3000 #define DIST_ERROR_MAX 3000
@ -99,7 +94,7 @@ void verify_missed_wp()
void calc_bearing_error() void calc_bearing_error()
{ {
bearing_error = nav_bearing - yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }

6
ArduCopterMega/system.pde
View File

@ -261,6 +261,12 @@ void set_mode(byte mode)
save_EEPROM_PID(); save_EEPROM_PID();
// used to stop fly_aways
if(rc_1.control_in == 0){
// we are on the ground is this is true
// disarm motors temp
motor_auto_safe = false;
}
//send_message(SEVERITY_LOW,"control mode"); //send_message(SEVERITY_LOW,"control mode");
//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
switch(control_mode) switch(control_mode)

30
ArduCopterMega/test.pde
View File

@ -163,7 +163,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, turn off radio.\n")); Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(1){ while(1){
delay(20); delay(20);
@ -203,6 +203,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
{ {
static byte ts_num; static byte ts_num;
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
@ -214,9 +215,11 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP()); Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_armed = true;
trim_radio(); trim_radio();
motor_auto_safe = false;
motor_armed = true;
while(1){ while(1){
// 50 hz // 50 hz
if (millis() - fast_loopTimer > 19) { if (millis() - fast_loopTimer > 19) {
@ -245,7 +248,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
// allow us to zero out sensors with control switches // allow us to zero out sensors with control switches
if(rc_5.control_in < 600){ if(rc_5.control_in < 600){
roll_sensor = pitch_sensor = 0; dcm.roll_sensor = dcm.pitch_sensor = 0;
} }
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
@ -260,8 +263,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
if (ts_num > 10){ if (ts_num > 10){
ts_num = 0; ts_num = 0;
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, "), Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, "),
(int)(roll_sensor/100), (int)(dcm.roll_sensor/100),
(int)(pitch_sensor/100), (int)(dcm.pitch_sensor/100),
rc_1.pwm_out, rc_1.pwm_out,
pid_stabilize_roll.get_integrator()); pid_stabilize_roll.get_integrator());
@ -269,8 +272,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
} }
// R: 1417, L: 1453 F: 1453 B: 1417 // R: 1417, L: 1453 F: 1453 B: 1417
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -331,7 +334,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
// allow us to zero out sensors with control switches // allow us to zero out sensors with control switches
if(rc_5.control_in < 600){ if(rc_5.control_in < 600){
roll_sensor = pitch_sensor = 0; dcm.roll_sensor = dcm.pitch_sensor = 0;
} }
// custom code/exceptions for flight modes // custom code/exceptions for flight modes
@ -369,8 +372,8 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
// R: 1417, L: 1453 F: 1453 B: 1417 // R: 1417, L: 1453 F: 1453 B: 1417
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
@ -441,9 +444,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
gyros.x, gyros.y, gyros.z); gyros.x, gyros.y, gyros.z);
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"), Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
roll_sensor, dcm.roll_sensor,
pitch_sensor, dcm.pitch_sensor,
yaw_sensor); dcm.yaw_sensor);
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -591,7 +594,6 @@ test_omega(uint8_t argc, const Menu::arg *argv)
old_yaw = dcm.yaw; old_yaw = dcm.yaw;
Vector3f omega = dcm.get_gyro();
ts_num++; ts_num++;
if (ts_num > 2){ if (ts_num > 2){
ts_num = 0; ts_num = 0;