zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Emile's Fixes

mission-4.1.18
Jason Short 13 years ago
parent
4483f19f85
commit
c7c16a5b0f
1 changed files with 87 additions and 93 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 180
      ArduCopter/motors_hexa.pde

180
ArduCopter/motors_hexa.pde
Unescape View File

@ -5,19 +5,19 @@ @@ -5,19 +5,19 @@
static void init_motors_out()
{
#if INSTANT_PWM == 0
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6) );
APM_RC.SetFastOutputChannels(_BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
| _BV(MOT_5) | _BV(MOT_6));
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
}
static void output_motors_armed()
@ -44,7 +44,7 @@ static void output_motors_armed() @@ -44,7 +44,7 @@ static void output_motors_armed()
//left side
motor_out[MOT_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
//right side
motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle
@ -73,26 +73,26 @@ static void output_motors_armed() @@ -73,26 +73,26 @@ static void output_motors_armed()
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[MOT_1] -= 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
// Tridge's stability patch
for (int m = 0; m <= 6; m++) {
int c = ch_of_mot(m);
int c_opp = ch_of_mot(m^1); // m^1 is the opposite motor. c_opp is channel of opposite motor.
if (motor_out[c] > out_max) {
motor_out[c_opp] -= motor_out[c] - out_max;
motor_out[c] = out_max;
}
}
for (int m = 0; m <= 6; m++){
int c = ch_of_mot(m);
int c_opp = ch_of_mot(m ^ 1); // m ^ 1 is the opposite motor. c_opp is channel of opposite motor.
if(motor_out[c] > out_max){
motor_out[c_opp] -= motor_out[c] - out_max;
motor_out[c] = out_max;
}
}
// limit output so motors don't stop
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
#if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors
@ -108,8 +108,8 @@ static void output_motors_armed() @@ -108,8 +108,8 @@ static void output_motors_armed()
// this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue
for(int8_t m = 0; m <= 6; m++ ) {
int c = ch_of_mot(m);
for(int8_t m = 0; m <= 6; m++){
int c = ch_of_mot(m);
if(motor_filtered[c] < motor_out[c]){
motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
}else{
@ -143,7 +143,7 @@ static void output_motors_disarmed() @@ -143,7 +143,7 @@ static void output_motors_disarmed()
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
for (unsigned char i = 0; i < 8; i++){
motor_out[i] = g.rc_3.radio_min;
}
@ -158,7 +158,7 @@ static void output_motors_disarmed() @@ -158,7 +158,7 @@ static void output_motors_disarmed()
static void output_motor_test()
{
motors_output_enable();
motors_output_enable();
motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[MOT_2] = g.rc_3.radio_min;
@ -168,74 +168,68 @@ static void output_motor_test() @@ -168,74 +168,68 @@ static void output_motor_test()
motor_out[MOT_6] = g.rc_3.radio_min;
if(g.frame_orientation == X_FRAME){
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
}
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
} else { /* PLUS_FRAME */
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
delay(4000);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(300);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
delay(2000);
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(300);
}
}
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
@ -244,4 +238,4 @@ static void output_motor_test() @@ -244,4 +238,4 @@ static void output_motor_test()
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
}
#endif
#endif
Write Preview
Loading…
Cancel
Save