ardupilot/ArducopterNG/Radio.pde

175 lines
6.0 KiB
Plaintext
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Radio.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:
* ************************************************************** */
#define STICK_TO_ANGLE_FACTOR 12.0 // To convert stick position to absolute angles
#define YAW_STICK_TO_ANGLE_FACTOR 150.0
void read_radio()
{
// Commands from radio Rx
// We apply the Radio calibration parameters (from configurator) except for throttle
ch_roll = channel_filter(APM_RC.InputCh(CH_ROLL) * ch_roll_slope + ch_roll_offset, ch_roll);
ch_pitch = channel_filter(APM_RC.InputCh(CH_PITCH) * ch_pitch_slope + ch_pitch_offset, ch_pitch);
ch_throttle = channel_filter(APM_RC.InputCh(CH_THROTTLE), ch_throttle); // Transmiter calibration not used on throttle
ch_yaw = channel_filter(APM_RC.InputCh(CH_RUDDER) * ch_yaw_slope + ch_yaw_offset, ch_yaw);
ch_aux = APM_RC.InputCh(CH_5) * ch_aux_slope + ch_aux_offset;
ch_aux2 = APM_RC.InputCh(CH_6) * ch_aux2_slope + ch_aux2_offset;
// Flight mode
if(ch_aux2 > 1200)
flightMode = ACRO_MODE; // Force to Acro mode from radio
else
flightMode = STABLE_MODE; // Stable mode (default)
// Autopilot mode (only works on Stable mode)
if (flightMode == STABLE_MODE)
{
if(ch_aux > 1800)
AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude hold
else
AP_mode = AP_NORMAL_MODE; // Normal mode
}
if (flightMode==STABLE_MODE) // IN STABLE MODE we convert stick positions to absoulte angles
{
// In Stable mode stick position defines the desired angle in roll, pitch and yaw
#ifdef FLIGHT_MODE_X
// For X mode we make a mix in the input
float aux_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR;
float aux_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
command_rx_roll = aux_roll - aux_pitch;
command_rx_pitch = aux_roll + aux_pitch;
#else
command_rx_roll = (ch_roll-roll_mid) / STICK_TO_ANGLE_FACTOR; // Convert stick position to absolute angles
command_rx_pitch = (ch_pitch-pitch_mid) / STICK_TO_ANGLE_FACTOR;
#endif
// YAW
if (abs(ch_yaw-yaw_mid)>6) // Take into account a bit of "dead zone" on yaw
{
command_rx_yaw += (ch_yaw-yaw_mid) / YAW_STICK_TO_ANGLE_FACTOR;
command_rx_yaw = Normalize_angle(command_rx_yaw); // Normalize angle to [-180,180]
}
}
// Write Radio data to DataFlash log
Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,ch_aux,ch_aux2);
// Motor arm logic
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
if (ch_throttle < (MIN_THROTTLE + 100)) {
control_yaw = 0;
command_rx_yaw = ToDeg(yaw);
if (ch_yaw > 1850) {
if (Arming_counter > ARM_DELAY){
if(ch_throttle > 800) {
motorArmed = 1;
minThrottle = MIN_THROTTLE+60; // A minimun value for mantain a bit if throttle
}
}
else
Arming_counter++;
}
else
Arming_counter=0;
// To Disarm motor output : Throttle down and full yaw left for more than 2 seconds
if (ch_yaw < 1150) {
if (Disarming_counter > DISARM_DELAY){
motorArmed = 0;
minThrottle = MIN_THROTTLE;
}
else
Disarming_counter++;
}
else
Disarming_counter=0;
}
else{
Arming_counter=0;
Disarming_counter=0;
}
}
// Send output commands to ESC´s
void motor_output()
{
if (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(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
backMotor = constrain(ch_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);
}
// 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();
}