mirror of https://github.com/ArduPilot/ardupilot
127 lines
4.5 KiB
Plaintext
127 lines
4.5 KiB
Plaintext
/*
|
|
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 < 1200)
|
|
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
|
|
if(!flightOrientation) {
|
|
// 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;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
|