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

fixed motor arming, fixed sign for pitch control from transmitter 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@44 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
CaranchoEngineering 2010-07-15 07:26:05 +00:00
parent c0edb56fef
commit b61acbfbab
1 changed files with 42 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
Arducopter/Arducopter.pde
View File

@ -194,13 +194,14 @@ int ch_aux2;
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun... #define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
// Motor variables // Motor variables
#define FLIGHT_MODE_+ //#define FLIGHT_MODE_+
//#define FLIGHT_MODE_X #define FLIGHT_MODE_X
int frontMotor; int frontMotor;
int backMotor; int backMotor;
int leftMotor; int leftMotor;
int rightMotor; int rightMotor;
byte motorArmed = 0; byte motorArmed = 0;
int minThrottle = 0;
// Serial communication // Serial communication
#define CONFIGURATOR #define CONFIGURATOR
@ -338,7 +339,7 @@ void Rate_control()
// PITCH CONTROL // PITCH CONTROL
currentPitchRate = read_adc(1); currentPitchRate = read_adc(1);
err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate; err_pitch = ((1500-ch_pitch) * xmitFactor) - currentPitchRate;
pitch_I += err_pitch*G_Dt; pitch_I += err_pitch*G_Dt;
pitch_I = constrain(pitch_I,-20,20); pitch_I = constrain(pitch_I,-20,20);
@ -585,7 +586,7 @@ void loop(){
command_rx_roll = (ch_roll-CHANN_CENTER)/12.0; command_rx_roll = (ch_roll-CHANN_CENTER)/12.0;
command_rx_roll_diff = command_rx_roll-command_rx_roll_old; command_rx_roll_diff = command_rx_roll-command_rx_roll_old;
command_rx_pitch_old = command_rx_pitch; command_rx_pitch_old = command_rx_pitch;
command_rx_pitch = (ch_pitch-CHANN_CENTER)/12.0; command_rx_pitch = (CHANN_CENTER-ch_pitch)/12.0;
command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old; command_rx_pitch_diff = command_rx_pitch-command_rx_pitch_old;
aux_float = (ch_yaw-Neutro_yaw)/180.0; aux_float = (ch_yaw-Neutro_yaw)/180.0;
command_rx_yaw += aux_float; command_rx_yaw += aux_float;
@ -691,64 +692,58 @@ void loop(){
command_rx_yaw = ToDeg(yaw); command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0; command_rx_yaw_diff = 0;
} }
// Arm motor output // Arm motor output
if (ch_throttle < 1200) { if (ch_throttle < 1200) {
control_yaw = 0; control_yaw = 0;
if (ch_yaw > 1800) command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0;
if (ch_yaw > 1800) {
motorArmed = 1; motorArmed = 1;
if (ch_yaw < 1200) minThrottle = 1100;
}
if (ch_yaw < 1200) {
motorArmed = 0; motorArmed = 0;
minThrottle = MIN_THROTTLE;
}
} }
// Quadcopter mix // Quadcopter mix
// Ask Jose if we still need this IF statement, and if we want to do an ESC calibration // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
if (ch_throttle > (MIN_THROTTLE+20)) // Minimun throttle to start control if (motorArmed == 1) {
{ #ifdef FLIGHT_MODE_+
if (motorArmed == 1) { rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
#ifdef FLIGHT_MODE_+ leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
rightMotor = ch_throttle - control_roll - control_yaw; frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000);
leftMotor = ch_throttle + control_roll - control_yaw; backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000);
frontMotor = ch_throttle + control_pitch + control_yaw; #endif
backMotor = ch_throttle - control_pitch + control_yaw; #ifdef FLIGHT_MODE_X
#endif frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor
#ifdef FLIGHT_MODE_X rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor
frontMotor = ch_throttle + control_roll + control_pitch - control_yaw; // front left motor leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000); // rear left motor
rightMotor = ch_throttle - control_roll + control_pitch + control_yaw; // front right motor backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000); // rear right motor
leftMotor = ch_throttle + control_roll - control_pitch + control_yaw; // rear left motor #endif
backMotor = ch_throttle - control_roll - control_pitch - control_yaw; // rear right motor }
#endif if (motorArmed == 0) {
} rightMotor = MIN_THROTTLE;
if (motorArmed == 0) { leftMotor = MIN_THROTTLE;
rightMotor = MIN_THROTTLE; frontMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE; backMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE;
backMotor = MIN_THROTTLE;
}
APM_RC.OutputCh(0, rightMotor); // Right motor
APM_RC.OutputCh(1, leftMotor); // Left motor
APM_RC.OutputCh(2, frontMotor); // Front motor
APM_RC.OutputCh(3, backMotor); // Back motor
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}
else
{
roll_I = 0; // reset I terms of PID controls roll_I = 0; // reset I terms of PID controls
pitch_I = 0; pitch_I = 0;
yaw_I = 0; yaw_I = 0;
APM_RC.OutputCh(0,MIN_THROTTLE); // Motors stoped
APM_RC.OutputCh(1,MIN_THROTTLE);
APM_RC.OutputCh(2,MIN_THROTTLE);
APM_RC.OutputCh(3,MIN_THROTTLE);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
// Initialize yaw command to actual yaw when throttle is down... // Initialize yaw command to actual yaw when throttle is down...
command_rx_yaw = ToDeg(yaw); command_rx_yaw = ToDeg(yaw);
command_rx_yaw_diff = 0; command_rx_yaw_diff = 0;
} }
APM_RC.OutputCh(0, rightMotor); // Right motor
APM_RC.OutputCh(1, leftMotor); // Left motor
APM_RC.OutputCh(2, frontMotor); // Front motor
APM_RC.OutputCh(3, backMotor); // Back motor
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
#ifndef CONFIGURATOR #ifndef CONFIGURATOR
Serial.println(); // Line END Serial.println(); // Line END
#endif #endif