You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
106 lines
3.0 KiB
106 lines
3.0 KiB
/* |
|
www.ArduCopter.com - www.DIYDrones.com |
|
Copyright (c) 2010. All rights reserved. |
|
An Open Source Arduino based multicopter. |
|
|
|
File : Motors.pde |
|
Version : v1.0, Aug 27, 2010 |
|
Author(s): ArduCopter Team |
|
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, |
|
Jani Hirvinen, Ken McEwans, Roberto Navoni, |
|
Sandro Benigno, Chris Anderson |
|
|
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
* ************************************************************** * |
|
ChangeLog: |
|
|
|
|
|
* ************************************************************** * |
|
TODO: |
|
|
|
|
|
* ************************************************************** */ |
|
|
|
|
|
|
|
// Send output commands to ESC´s |
|
void motor_output() |
|
{ |
|
int throttle; |
|
byte throttle_mode=0; |
|
|
|
throttle = ch_throttle; |
|
#if (defined(UseBMP) || defined(IsSONAR)) |
|
if (AP_mode == AP_AUTOMATIC_MODE) |
|
{ |
|
throttle = ch_throttle_altitude_hold; |
|
throttle_mode=1; |
|
} |
|
#endif |
|
|
|
if ((throttle_mode==0)&&(ch_throttle < (MIN_THROTTLE + 100))) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely) |
|
control_yaw = 0; |
|
|
|
// Quadcopter mix |
|
if (motorArmed == 1) { |
|
#ifdef IsAM |
|
digitalWrite(FR_LED, HIGH); // AM-Mode |
|
#endif |
|
|
|
// Quadcopter output mix |
|
rightMotor = constrain(throttle - control_roll + control_yaw, minThrottle, 2000); |
|
leftMotor = constrain(throttle + control_roll + control_yaw, minThrottle, 2000); |
|
frontMotor = constrain(throttle + control_pitch - control_yaw, minThrottle, 2000); |
|
backMotor = constrain(throttle - control_pitch - control_yaw, minThrottle, 2000); |
|
|
|
} else { // MOTORS DISARMED |
|
|
|
#ifdef IsAM |
|
digitalWrite(FR_LED, LOW); // AM-Mode |
|
#endif |
|
digitalWrite(LED_Green,HIGH); // Ready LED on |
|
|
|
rightMotor = MIN_THROTTLE; |
|
leftMotor = MIN_THROTTLE; |
|
frontMotor = MIN_THROTTLE; |
|
backMotor = MIN_THROTTLE; |
|
|
|
// Reset_I_Terms(); |
|
roll_I = 0; // reset I terms of PID controls |
|
pitch_I = 0; |
|
yaw_I = 0; |
|
|
|
// Initialize yaw command to actual yaw when throttle is down... |
|
command_rx_yaw = ToDeg(yaw); |
|
} |
|
|
|
//#if MOTORTYPE == PWM |
|
// Send commands to motors |
|
APM_RC.OutputCh(0, rightMotor); |
|
APM_RC.OutputCh(1, leftMotor); |
|
APM_RC.OutputCh(2, frontMotor); |
|
APM_RC.OutputCh(3, backMotor); |
|
|
|
// InstantPWM => Force inmediate output on PWM signals |
|
APM_RC.Force_Out0_Out1(); |
|
APM_RC.Force_Out2_Out3(); |
|
//#elif MOTORTYPE == I2C |
|
|
|
//#else |
|
//# error You need to define your motor type on ArduUder.pde file |
|
//#endif |
|
} |
|
|
|
|
|
|