mirror of https://github.com/ArduPilot/ardupilot
Added Tri-copter support for Max
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1412 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
9c9c541a51
commit
ff256d95e2
|
|
@ -16,8 +16,6 @@ void output_stabilize()
|
|||
long rate;
|
||||
int dampener;
|
||||
|
||||
//pitch_sensor = roll_sensor = 0; // testing only
|
||||
|
||||
// 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);
|
||||
|
|
@ -30,14 +28,10 @@ void output_stabilize()
|
|||
roll_error = constrain(roll_error, -2500, 2500);
|
||||
pitch_error = constrain(pitch_error, -2500, 2500);
|
||||
|
||||
//Serial.printf("s: %d \t mix %d, err %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error);
|
||||
|
||||
// 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, deltaMiliSeconds, 1.0);
|
||||
rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0);
|
||||
|
||||
//Serial.printf("\tpid: %d", (int)rc_1.servo_out);
|
||||
|
||||
// We adjust the output by the rate of rotation:
|
||||
// Rate control through bias corrected gyro rates
|
||||
// omega is the raw gyro reading
|
||||
|
|
@ -45,16 +39,11 @@ void output_stabilize()
|
|||
// Limit dampening to be equal to propotional term for symmetry
|
||||
rate = degrees(omega.x) * 100; // 6rad = 34377
|
||||
dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000
|
||||
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500
|
||||
rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
|
||||
|
||||
rate = degrees(omega.y) * 100; // 6rad = 34377
|
||||
dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000
|
||||
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500
|
||||
|
||||
//Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener);
|
||||
|
||||
//Serial.printf("\trd: %d", roll_dampener);
|
||||
//Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm());
|
||||
rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -66,30 +55,32 @@ clear_yaw_control()
|
|||
yaw_error = 0;
|
||||
}
|
||||
|
||||
|
||||
void output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
Vector3f omega = dcm.get_gyro();
|
||||
|
||||
if(hold){
|
||||
// yaw hold
|
||||
|
||||
if(rate_yaw_flag){
|
||||
// we are still in motion from rate control
|
||||
if(fabs(omega.y) < .15){
|
||||
//Serial.print("trans ");
|
||||
clear_yaw_control();
|
||||
hold = true; // just to be explicit
|
||||
}else{
|
||||
//Serial.print("coast ");
|
||||
// return to rate control until we slow down.
|
||||
hold = false;
|
||||
}
|
||||
}else{
|
||||
//Serial.print("hold ");
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
//Serial.print("rate ");
|
||||
}else{
|
||||
// rate control
|
||||
|
||||
// this indicates we are under rate control, when we enter Yaw Hold and
|
||||
// return to 0° per second, we exit rate control and hold the current Yaw
|
||||
rate_yaw_flag = true;
|
||||
yaw_error = 0;
|
||||
}
|
||||
|
|
@ -100,6 +91,8 @@ void output_yaw_with_hold(boolean hold)
|
|||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
|
||||
|
||||
// Apply PID and save the new angle back to RC_Channel
|
||||
rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .5 * 6000 = 3000
|
||||
|
||||
// We adjust the output by the rate of rotation
|
||||
|
|
@ -110,19 +103,15 @@ void output_yaw_with_hold(boolean hold)
|
|||
rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
|
||||
|
||||
}else{
|
||||
//yaw_error = 0;
|
||||
|
||||
// rate control
|
||||
|
||||
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
|
||||
rate = constrain(rate, -36000, 36000); // limit to something fun!
|
||||
long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000
|
||||
long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
|
||||
// -error = CCW, +error = CW
|
||||
rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, deltaMiliSeconds, 1.0); // .075 * 36000 = 2700
|
||||
rc_4.servo_out = constrain(rc_4.servo_out, -2400, 2400); // limit to 2400
|
||||
|
||||
// this indicates we are under rate control, when we enter Yaw Hold and
|
||||
// return to 0° per second, we exit rate control and hold the current Yaw
|
||||
//rate_yaw_flag = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -138,6 +138,7 @@ void read_EEPROM_PID(void)
|
|||
|
||||
// yaw
|
||||
hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4);
|
||||
init_pids();
|
||||
}
|
||||
|
||||
void save_EEPROM_PID(void)
|
||||
|
|
|
|||
|
|
@ -46,10 +46,10 @@
|
|||
#define CH_AUX2 CH_6
|
||||
|
||||
// motors
|
||||
#define FRONT 0
|
||||
#define RIGHT 1
|
||||
#define BACK 2
|
||||
#define LEFT 3
|
||||
#define RIGHT 0
|
||||
#define LEFT 1
|
||||
#define FRONT 2
|
||||
#define BACK 3
|
||||
|
||||
#define MAX_SERVO_OUTPUT 2700
|
||||
|
||||
|
|
|
|||
|
|
@ -37,9 +37,6 @@ float angle_boost()
|
|||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*************************************************************
|
||||
yaw control
|
||||
****************************************************************/
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ void navigate()
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
void verify_missed_wp()
|
||||
{
|
||||
// check if we have missed the WP
|
||||
|
|
@ -63,6 +64,7 @@ void verify_missed_wp()
|
|||
if (loiter_delta < -170) loiter_delta += 360;
|
||||
loiter_sum += abs(loiter_delta);
|
||||
}
|
||||
*/
|
||||
|
||||
void calc_bearing_error()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -134,13 +134,14 @@ void set_servos_4(void)
|
|||
//Serial.printf("yaw: %d ", rc_4.radio_out);
|
||||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
//Serial.println("+");
|
||||
|
||||
motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out;
|
||||
motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out;
|
||||
motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out;
|
||||
motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out;
|
||||
|
||||
}else if(frame_type == X_FRAME){
|
||||
|
||||
int roll_out = rc_1.pwm_out / 2;
|
||||
int pitch_out = rc_2.pwm_out / 2;
|
||||
|
||||
|
|
@ -149,28 +150,39 @@ 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;
|
||||
|
||||
}else if(frame_type == TRI_FRAME){
|
||||
|
||||
// Tri-copter power distribution
|
||||
|
||||
int roll_out = (float)rc_1.pwm_out * .866;
|
||||
int pitch_out = rc_2.pwm_out / 2;
|
||||
|
||||
// front two motors
|
||||
motor_out[LEFT] = rc_3.radio_out + roll_out + pitch_out;
|
||||
motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out;
|
||||
|
||||
// rear motors
|
||||
motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out;
|
||||
|
||||
// servo Yaw
|
||||
motor_out[FRONT] = rc_4.radio_out;
|
||||
|
||||
}else{
|
||||
/*
|
||||
replace this with Tri-frame control law
|
||||
|
||||
int roll_out = rc_1.pwm_out / 2;
|
||||
int pitch_out = rc_2.pwm_out / 2;
|
||||
Serial.print("frame error");
|
||||
|
||||
motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out;
|
||||
motor_out[LEFT] = 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;
|
||||
*/
|
||||
}
|
||||
|
||||
//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]);
|
||||
|
|
@ -229,15 +241,19 @@ void set_servos_4(void)
|
|||
|
||||
// Send commands to motors
|
||||
if(rc_3.servo_out > 0){
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[RIGHT]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[LEFT]);
|
||||
APM_RC.OutputCh(CH_3, motor_out[FRONT]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[BACK]);
|
||||
|
||||
}else{
|
||||
|
||||
APM_RC.OutputCh(CH_1, rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, rc_3.radio_min);
|
||||
|
||||
}
|
||||
|
||||
// InstantPWM
|
||||
|
|
|
|||
|
|
@ -270,6 +270,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
|||
// stop here
|
||||
}
|
||||
}
|
||||
|
||||
rc_1.radio_min = rc_1.radio_in;
|
||||
rc_2.radio_min = rc_2.radio_in;
|
||||
rc_3.radio_min = rc_3.radio_in;
|
||||
|
|
@ -346,9 +347,14 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
if(frame_type == PLUS_FRAME){
|
||||
Serial.printf_P(PSTR("PLUS"));
|
||||
}else{
|
||||
|
||||
}else if(frame_type == X_FRAME){
|
||||
Serial.printf_P(PSTR("X"));
|
||||
|
||||
}else if(frame_type == TRI_FRAME){
|
||||
Serial.printf_P(PSTR("TRI"));
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR(" Frame\n"));
|
||||
|
||||
while(1){
|
||||
|
|
@ -380,7 +386,8 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
|||
Serial.println("2");
|
||||
}
|
||||
|
||||
}else{
|
||||
}else if(frame_type == PLUS_FRAME){
|
||||
|
||||
// lower right
|
||||
if((rc_1.control_in > 0) && (rc_2.control_in > 0)){
|
||||
motor_out[BACK] = out_min;
|
||||
|
|
@ -400,6 +407,29 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
|||
motor_out[RIGHT] = out_min;
|
||||
Serial.println("0");
|
||||
}
|
||||
|
||||
}else if(frame_type == TRI_FRAME){
|
||||
|
||||
if(rc_1.control_in > 0){
|
||||
motor_out[RIGHT] = out_min;
|
||||
|
||||
}else if(rc_1.control_in < 0){
|
||||
motor_out[LEFT] = out_min;
|
||||
}
|
||||
|
||||
if(rc_2.control_in > 0){
|
||||
motor_out[BACK] = out_min;
|
||||
}
|
||||
|
||||
if(rc_4.control_in > 0){
|
||||
rc_4.servo_out = 2000;
|
||||
|
||||
}else if(rc_4.control_in < 0){
|
||||
rc_4.servo_out = -2000;
|
||||
}
|
||||
|
||||
rc_4.calc_pwm();
|
||||
motor_out[FRONT] = rc_4.radio_out;
|
||||
}
|
||||
|
||||
if(rc_3.control_in > 0){
|
||||
|
|
@ -419,6 +449,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
setup_accel(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
@ -620,7 +651,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
|
|||
}else if(argv[1].i == 3){
|
||||
|
||||
Serial.printf_P(PSTR("Tri "));
|
||||
frame_type = X_FRAME;
|
||||
frame_type = TRI_FRAME;
|
||||
}
|
||||
Serial.printf_P(PSTR("frame\n\n"));
|
||||
save_EEPROM_frame();
|
||||
|
|
|
|||
|
|
@ -94,7 +94,6 @@ void init_ardupilot()
|
|||
|
||||
|
||||
read_EEPROM_startup(); // Read critical config information to start
|
||||
init_pids();
|
||||
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up the timer libs
|
||||
|
|
|
|||
Loading…
Reference in New Issue