mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 18:38:28 -04:00
ed5c7d01b1
Before: -> After Stabilize P –> Stabilize P (Use NG values, or 8.3 x the older AC2 value) Stabilize I –> Stabilize I (Stays same value) Stabilize D –> Rate P (Stays same value) –> Rate I (new) Added a new value – an I term for rate. The old stabilization routines did not use this term. Please refer to the config.h file to read more about the new PIDs. Added framework for using DCM corrected Accelerometer rates. Code is commented out for now. Added set home at Arming. Crosstrack is now a full PID loop, rather than just a P gain for more control. Throttle now slews when switching out of Alt hold or Auto modes for less jarring transitions Sonar and Baro PIDs are now combined into a throttle PID Yaw control is completely re-written. Added Octa_Quad support - Max git-svn-id: https://arducopter.googlecode.com/svn/trunk@2836 f9c3cf11-9bcb-44bc-f272-b75c42450872
136 lines
3.6 KiB
Plaintext
136 lines
3.6 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#if FRAME_CONFIG == Y6_FRAME
|
|
|
|
void output_motors_armed()
|
|
{
|
|
int out_min = g.rc_3.radio_min;
|
|
|
|
// Throttle is 0 to 1000 only
|
|
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
|
|
|
|
if(g.rc_3.servo_out > 0)
|
|
out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;
|
|
|
|
g.rc_1.calc_pwm();
|
|
g.rc_2.calc_pwm();
|
|
g.rc_3.calc_pwm();
|
|
g.rc_4.calc_pwm();
|
|
|
|
int roll_out = (float)g.rc_1.pwm_out * .866;
|
|
int pitch_out = g.rc_2.pwm_out / 2;
|
|
|
|
//left
|
|
motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
|
|
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
|
|
|
//right
|
|
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
|
|
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
|
|
|
//back
|
|
motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
|
|
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
|
|
|
|
// Yaw
|
|
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
|
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
|
motor_out[CH_8] += g.rc_4.pwm_out; // CCW
|
|
|
|
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
|
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
|
|
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
|
|
|
#if CUT_MOTORS == ENABLED
|
|
// Send commands to motors
|
|
if(g.rc_3.servo_out > 0){
|
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
|
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
|
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
|
|
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
|
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
|
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
|
|
|
// InstantPWM
|
|
APM_RC.Force_Out0_Out1();
|
|
APM_RC.Force_Out6_Out7();
|
|
APM_RC.Force_Out2_Out3();
|
|
}else{
|
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
|
}
|
|
#else
|
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
|
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
|
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
|
|
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
|
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
|
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
|
|
|
// InstantPWM
|
|
APM_RC.Force_Out0_Out1();
|
|
APM_RC.Force_Out6_Out7();
|
|
APM_RC.Force_Out2_Out3();
|
|
#endif
|
|
}
|
|
|
|
void output_motors_disarmed()
|
|
{
|
|
if(g.rc_3.control_in > 0){
|
|
// we have pushed up the throttle
|
|
// remove safety
|
|
motor_auto_armed = true;
|
|
}
|
|
|
|
// fill the motor_out[] array for HIL use
|
|
for (unsigned char i = 0; i < 8; i++) {
|
|
motor_out[i] = g.rc_3.radio_min;
|
|
}
|
|
|
|
// Send commands to motors
|
|
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
|
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
|
}
|
|
|
|
void output_motor_test()
|
|
{
|
|
motor_out[CH_1] = g.rc_3.radio_min;
|
|
motor_out[CH_2] = g.rc_3.radio_min;
|
|
motor_out[CH_3] = g.rc_3.radio_min;
|
|
motor_out[CH_4] = g.rc_3.radio_min;
|
|
motor_out[CH_7] = g.rc_3.radio_min;
|
|
motor_out[CH_8] = g.rc_3.radio_min;
|
|
|
|
|
|
if(g.rc_1.control_in > 3000){ // right
|
|
motor_out[CH_1] += 50;
|
|
motor_out[CH_7] += 50;
|
|
}
|
|
|
|
if(g.rc_1.control_in < -3000){ // left
|
|
motor_out[CH_2] += 50;
|
|
motor_out[CH_3] += 50;
|
|
}
|
|
|
|
if(g.rc_2.control_in > 3000){ // back
|
|
motor_out[CH_8] += 50;
|
|
motor_out[CH_4] += 50;
|
|
}
|
|
|
|
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
|
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
|
APM_RC.OutputCh(CH_3, motor_out[CH_4]);
|
|
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
|
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
|
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
|
}
|
|
|
|
#endif |