mirror of https://github.com/ArduPilot/ardupilot
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
parent
000eaf6a68
commit
e977116f9b
|
|
@ -146,6 +146,8 @@ PID pid_stabilize_roll (EE_GAIN_4);
|
|||
PID pid_stabilize_pitch (EE_GAIN_5);
|
||||
PID pid_yaw (EE_GAIN_6);
|
||||
|
||||
Vector3f omega;
|
||||
|
||||
// roll pitch
|
||||
float stabilize_dampener;
|
||||
int max_stabilize_dampener;
|
||||
|
|
@ -202,7 +204,9 @@ int airspeed; // m/s * 100
|
|||
|
||||
// Throttle Failsafe
|
||||
// ------------------
|
||||
boolean motor_armed;
|
||||
boolean motor_armed = false;
|
||||
boolean motor_auto_safe = false;
|
||||
|
||||
byte throttle_failsafe_enabled;
|
||||
int throttle_failsafe_value;
|
||||
byte throttle_failsafe_action;
|
||||
|
|
@ -254,9 +258,9 @@ int temp_unfilt;
|
|||
|
||||
// From IMU
|
||||
// --------
|
||||
long roll_sensor; // degrees * 100
|
||||
long pitch_sensor; // degrees * 100
|
||||
long yaw_sensor; // degrees * 100
|
||||
//long roll_sensor; // degrees * 100
|
||||
//long pitch_sensor; // degrees * 100
|
||||
//long yaw_sensor; // degrees * 100
|
||||
float roll; // radians
|
||||
float pitch; // radians
|
||||
float yaw; // radians
|
||||
|
|
@ -531,6 +535,10 @@ void medium_loop()
|
|||
navigate();
|
||||
}
|
||||
|
||||
// calc pitch and roll to target
|
||||
// -----------------------------
|
||||
calc_nav();
|
||||
|
||||
break;
|
||||
|
||||
// command processing
|
||||
|
|
@ -557,7 +565,7 @@ void medium_loop()
|
|||
medium_loopCounter++;
|
||||
|
||||
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)
|
||||
Log_Write_Control_Tuning();
|
||||
|
|
@ -606,7 +614,7 @@ void medium_loop()
|
|||
|
||||
|
||||
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)
|
||||
Log_Write_Raw();
|
||||
|
|
@ -776,8 +784,12 @@ void update_current_flight_mode(void)
|
|||
// ------------------------------------
|
||||
auto_yaw();
|
||||
|
||||
// mix in user control
|
||||
control_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
|
||||
// apply throttle control
|
||||
output_auto_throttle();
|
||||
|
|
@ -787,6 +799,42 @@ void update_current_flight_mode(void)
|
|||
}else{
|
||||
|
||||
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:
|
||||
// Intput Pitch, Roll, Yaw and Throttle
|
||||
|
|
@ -804,12 +852,15 @@ void update_current_flight_mode(void)
|
|||
// apply throttle control
|
||||
output_manual_throttle();
|
||||
|
||||
// mix in user control
|
||||
control_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case FBW:
|
||||
|
||||
// we are currently using manual throttle during alpha testing.
|
||||
fbw_timer++;
|
||||
//call at 5 hz
|
||||
|
|
@ -840,8 +891,12 @@ void update_current_flight_mode(void)
|
|||
// apply throttle control
|
||||
output_manual_throttle();
|
||||
|
||||
// apply nav_pitch and nav_roll to output
|
||||
fbw_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case ALT_HOLD:
|
||||
|
|
@ -867,8 +922,12 @@ void update_current_flight_mode(void)
|
|||
// apply throttle control
|
||||
output_auto_throttle();
|
||||
|
||||
// mix in user control
|
||||
control_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
|
|
@ -889,8 +948,12 @@ void update_current_flight_mode(void)
|
|||
// apply throttle control
|
||||
output_auto_throttle();
|
||||
|
||||
// mix in user control
|
||||
control_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
case POSITION_HOLD:
|
||||
|
|
@ -911,11 +974,16 @@ void update_current_flight_mode(void)
|
|||
|
||||
// Output Pitch, Roll, Yaw and Throttle
|
||||
// ------------------------------------
|
||||
|
||||
// apply throttle control
|
||||
output_auto_throttle();
|
||||
|
||||
// mix in user control
|
||||
control_nav_mixer();
|
||||
|
||||
// perform stabilzation
|
||||
output_stabilize();
|
||||
output_stabilize_roll();
|
||||
output_stabilize_pitch();
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -936,8 +1004,8 @@ void update_navigation()
|
|||
if(control_mode == AUTO){
|
||||
verify_must();
|
||||
verify_may();
|
||||
}else{
|
||||
|
||||
}else{
|
||||
switch(control_mode){
|
||||
case RTL:
|
||||
update_crosstrack();
|
||||
|
|
@ -950,9 +1018,7 @@ void update_navigation()
|
|||
void read_AHRS(void)
|
||||
{
|
||||
// Perform IMU calculations and get attitude info
|
||||
//-----------------------------------------------------
|
||||
//-----------------------------------------------
|
||||
dcm.update_DCM(G_Dt);
|
||||
roll_sensor = dcm.roll_sensor;
|
||||
pitch_sensor = dcm.pitch_sensor;
|
||||
yaw_sensor = dcm.yaw_sensor;
|
||||
omega = dcm.get_gyro();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// output is in degrees = target pitch and roll of copter
|
||||
rc_1.servo_out = rc_1.control_mix(nav_roll);
|
||||
rc_2.servo_out = rc_2.control_mix(nav_pitch);
|
||||
}
|
||||
|
||||
roll_error = rc_1.servo_out - roll_sensor;
|
||||
pitch_error = rc_2.servo_out - pitch_sensor;
|
||||
void fbw_nav_mixer()
|
||||
{
|
||||
// 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
|
||||
roll_error = constrain(roll_error, -2500, 2500);
|
||||
pitch_error = constrain(pitch_error, -2500, 2500);
|
||||
error = constrain(error, -2500, 2500);
|
||||
|
||||
// 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_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, delta_ms_fast_loop, 1.0);
|
||||
rc_1.servo_out = pid_stabilize_roll.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
|
||||
|
|
@ -40,7 +46,26 @@ void output_stabilize()
|
|||
rate = degrees(omega.x) * 100.0; // 6rad = 34377
|
||||
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
|
||||
}
|
||||
|
||||
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
|
||||
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
|
||||
|
|
@ -50,19 +75,15 @@ void
|
|||
clear_yaw_control()
|
||||
{
|
||||
//Serial.print("Clear ");
|
||||
rate_yaw_flag = false; // exit rate_yaw_flag
|
||||
nav_yaw = yaw_sensor; // save our Yaw
|
||||
rate_yaw_flag = false; // exit rate_yaw_flag
|
||||
nav_yaw = dcm.yaw_sensor; // save our Yaw
|
||||
yaw_error = 0;
|
||||
}
|
||||
|
||||
|
||||
void output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
|
||||
if(hold){
|
||||
// yaw hold
|
||||
|
||||
// look to see if we have exited rate control properly - ie stopped turning
|
||||
if(rate_yaw_flag){
|
||||
// we are still in motion from rate control
|
||||
if(fabs(omega.y) < .15){
|
||||
|
|
@ -72,8 +93,6 @@ void output_yaw_with_hold(boolean hold)
|
|||
// return to rate control until we slow down.
|
||||
hold = false;
|
||||
}
|
||||
}else{
|
||||
|
||||
}
|
||||
|
||||
}else{
|
||||
|
|
@ -86,9 +105,8 @@ void output_yaw_with_hold(boolean hold)
|
|||
}
|
||||
|
||||
if(hold){
|
||||
|
||||
// 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);
|
||||
|
||||
// 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
|
||||
|
||||
}else{
|
||||
|
||||
// rate control
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
|
|
@ -119,11 +136,29 @@ void output_yaw_with_hold(boolean hold)
|
|||
|
||||
|
||||
|
||||
|
||||
void output_rate_control()
|
||||
void output_rate_roll()
|
||||
{
|
||||
/*
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
// rate control
|
||||
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_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_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.
|
||||
|
|
|
|||
|
|
@ -15,13 +15,13 @@ void
|
|||
camera_stabilization()
|
||||
{
|
||||
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();
|
||||
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.
|
||||
// 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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -78,9 +78,9 @@ void print_attitude(void)
|
|||
SendSer(",THH:");
|
||||
SendSer(rc_3.servo_out, DEC);
|
||||
SendSer (",RLL:");
|
||||
SendSer(roll_sensor / 100, DEC);
|
||||
SendSer(dcm.roll_sensor / 100, DEC);
|
||||
SendSer (",PCH:");
|
||||
SendSer(pitch_sensor / 100, DEC);
|
||||
SendSer(dcm.pitch_sensor / 100, DEC);
|
||||
SendSerln(",***");
|
||||
}
|
||||
|
||||
|
|
@ -107,7 +107,7 @@ void print_position(void)
|
|||
SendSer(",ALH:");
|
||||
SendSer(next_WP.alt/100,DEC);
|
||||
SendSer(",CRS:");
|
||||
SendSer(yaw_sensor/100,DEC);
|
||||
SendSer(dcm.yaw_sensor/100,DEC);
|
||||
SendSer(",BER:");
|
||||
SendSer(target_bearing/100,DEC);
|
||||
SendSer(",WPN:");
|
||||
|
|
|
|||
|
|
@ -71,13 +71,13 @@ void send_message(byte id, long param) {
|
|||
case MSG_ATTITUDE: // ** Attitude message
|
||||
mess_buffer[0] = 0x06;
|
||||
ck = 6;
|
||||
tempint = roll_sensor; // Roll (degrees * 100)
|
||||
tempint = dcm.roll_sensor; // Roll (degrees * 100)
|
||||
mess_buffer[3] = tempint & 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[6] = (tempint >> 8) & 0xff;
|
||||
tempint = yaw_sensor; // Yaw (degrees * 100)
|
||||
tempint = dcm.yaw_sensor; // Yaw (degrees * 100)
|
||||
mess_buffer[7] = tempint & 0xff;
|
||||
mess_buffer[8] = (tempint >> 8) & 0xff;
|
||||
break;
|
||||
|
|
@ -105,7 +105,7 @@ void send_message(byte id, long param) {
|
|||
mess_buffer[13] = tempint & 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[16] = (tempint >> 8) & 0xff;
|
||||
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ void output_HIL_(void)
|
|||
output_int((int)(rc_4.servo_out)); // 3 bytes 6, 7
|
||||
output_int((int)wp_distance); // 4 bytes 8, 9
|
||||
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_byte(wp_index); // 8 bytes 16
|
||||
output_byte(control_mode); // 9 bytes 17
|
||||
|
|
|
|||
|
|
@ -263,10 +263,10 @@ void Log_Write_Control_Tuning()
|
|||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||
DataFlash.WriteInt((int)(rc_1.servo_out));
|
||||
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)nav_pitch);
|
||||
DataFlash.WriteInt((int)pitch_sensor);
|
||||
DataFlash.WriteInt((int)dcm.pitch_sensor);
|
||||
DataFlash.WriteInt((int)(rc_3.servo_out));
|
||||
DataFlash.WriteInt((int)(rc_4.servo_out));
|
||||
DataFlash.WriteInt((int)(accel.y * 10000));
|
||||
|
|
@ -279,7 +279,7 @@ void Log_Write_Nav_Tuning()
|
|||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
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((uint16_t)target_bearing);
|
||||
DataFlash.WriteInt((uint16_t)nav_bearing);
|
||||
|
|
|
|||
|
|
@ -272,6 +272,11 @@
|
|||
# define ACRO_RATE_YAW_IMAX 0
|
||||
#endif
|
||||
|
||||
#ifndef ACRO_RATE_TRIGGER
|
||||
# define ACRO_RATE_TRIGGER 4200
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// STABILZE Angle Control
|
||||
|
|
|
|||
|
|
@ -82,6 +82,7 @@ void auto_yaw()
|
|||
picth and roll control
|
||||
****************************************************************/
|
||||
|
||||
|
||||
/*// how hard to tilt towards the target
|
||||
// -----------------------------------
|
||||
void calc_nav_pid()
|
||||
|
|
@ -98,7 +99,7 @@ void calc_nav_pitch()
|
|||
{
|
||||
// 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;
|
||||
float roll_out;
|
||||
|
|
@ -119,7 +120,7 @@ void calc_nav_pitch()
|
|||
// --------------------------------------------------
|
||||
void calc_nav_roll()
|
||||
{
|
||||
long angle = wrap_360(nav_bearing - yaw_sensor);
|
||||
long angle = wrap_360(nav_bearing - dcm.yaw_sensor);
|
||||
|
||||
bool rev = false;
|
||||
float roll_out;
|
||||
|
|
|
|||
|
|
@ -32,13 +32,14 @@ void arm_motors()
|
|||
/*****************************************
|
||||
* 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 counteri;
|
||||
|
||||
// Quadcopter mix
|
||||
if (motor_armed == true) {
|
||||
if (motor_armed == true && motor_auto_safe == true) {
|
||||
int out_min = rc_3.radio_min;
|
||||
|
||||
// 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[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){
|
||||
|
||||
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[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){
|
||||
|
||||
|
|
@ -100,13 +114,22 @@ void set_servos_4(void)
|
|||
int roll_out = (float)rc_1.pwm_out * .866;
|
||||
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[RIGHTFRONT] = rc_3.radio_out - roll_out + pitch_out - rc_4.pwm_out; // CW
|
||||
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out - rc_4.pwm_out; // CW
|
||||
motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_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; // CW
|
||||
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; // CW
|
||||
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) {
|
||||
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[LEFT] = constrain(motor_out[LEFT], out_min, rc_3.radio_max);
|
||||
|
|
@ -140,9 +151,9 @@ void set_servos_4(void)
|
|||
|
||||
|
||||
num++;
|
||||
if (num > 50){
|
||||
if (num > 10){
|
||||
num = 0;
|
||||
|
||||
Serial.print("!");
|
||||
//debugging with Channel 6
|
||||
|
||||
/*
|
||||
|
|
@ -196,8 +207,18 @@ void set_servos_4(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
}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
|
||||
APM_RC.OutputCh(CH_1, rc_3.radio_min);
|
||||
|
|
|
|||
|
|
@ -39,11 +39,6 @@ void navigate()
|
|||
// control mode specific updates to nav_bearing
|
||||
// --------------------------------------------
|
||||
update_navigation();
|
||||
|
||||
// calc pitch and roll to target
|
||||
// -----------------------------
|
||||
calc_nav();
|
||||
|
||||
}
|
||||
|
||||
#define DIST_ERROR_MAX 3000
|
||||
|
|
@ -99,7 +94,7 @@ void verify_missed_wp()
|
|||
|
||||
void calc_bearing_error()
|
||||
{
|
||||
bearing_error = nav_bearing - yaw_sensor;
|
||||
bearing_error = nav_bearing - dcm.yaw_sensor;
|
||||
bearing_error = wrap_180(bearing_error);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -261,6 +261,12 @@ void set_mode(byte mode)
|
|||
|
||||
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");
|
||||
//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
|
||||
switch(control_mode)
|
||||
|
|
|
|||
|
|
@ -163,7 +163,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
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){
|
||||
delay(20);
|
||||
|
|
@ -203,6 +203,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
static byte ts_num;
|
||||
|
||||
|
||||
print_hit_enter();
|
||||
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("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
|
||||
|
||||
motor_armed = true;
|
||||
trim_radio();
|
||||
|
||||
motor_auto_safe = false;
|
||||
motor_armed = true;
|
||||
|
||||
while(1){
|
||||
// 50 hz
|
||||
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
|
||||
if(rc_5.control_in < 600){
|
||||
roll_sensor = pitch_sensor = 0;
|
||||
dcm.roll_sensor = dcm.pitch_sensor = 0;
|
||||
}
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
|
|
@ -260,8 +263,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
|
|||
if (ts_num > 10){
|
||||
ts_num = 0;
|
||||
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, "),
|
||||
(int)(roll_sensor/100),
|
||||
(int)(pitch_sensor/100),
|
||||
(int)(dcm.roll_sensor/100),
|
||||
(int)(dcm.pitch_sensor/100),
|
||||
rc_1.pwm_out,
|
||||
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
|
||||
|
||||
//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)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)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
||||
|
||||
if(Serial.available() > 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
|
||||
if(rc_5.control_in < 600){
|
||||
roll_sensor = pitch_sensor = 0;
|
||||
dcm.roll_sensor = dcm.pitch_sensor = 0;
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
//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)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)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
|
|
@ -441,9 +444,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
|||
gyros.x, gyros.y, gyros.z);
|
||||
|
||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
|
||||
roll_sensor,
|
||||
pitch_sensor,
|
||||
yaw_sensor);
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
dcm.yaw_sensor);
|
||||
}
|
||||
|
||||
if(Serial.available() > 0){
|
||||
|
|
@ -591,7 +594,6 @@ test_omega(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
old_yaw = dcm.yaw;
|
||||
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
ts_num++;
|
||||
if (ts_num > 2){
|
||||
ts_num = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue