ardupilot/archive/ArducopterNG/Events.pde

119 lines
5.5 KiB
Plaintext
Raw Normal View History

/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : Events.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:
31-10-10 Moved camera stabilization from Functions to here, jp
* ************************************************************** *
TODO:
* ************************************************************** */
/* **************************************************** */
// Camera stabilization
//
// Stabilization for three different camera styles
// 1) Camera mounts that have tilt / pan
// 2) Camera mounts that have tilt / roll
// 3) Camera mounts that have tilt / roll / pan (future)
//
// Original code idea from Max Levine / DIY Drones
// You need to initialize proper camera mode by sending Serial command and then save it
// to EEPROM memory. Possible commands are K1, K2, K3, K4
// Look more about different camera type on ArduCopter Wiki
#ifdef IsCAM
void camera_output() {
// cam_mode = 1; // for debugging
// Camera stabilization jump tables
// SW_DIP1 is a multplier, settings
switch ((SW_DIP1 * 4) + cam_mode + (BATTLOW * 10)) {
// Cases 1 & 4 are stabilization for + Mode flying setup
// Cases 5 & 8 are stabilization for x Mode flying setup
// Modes 3/4 + 7/8 needs still proper scaling on yaw movement
// Scaling needs physical test flying with FPV cameras on, 30-10-10 jp
case 1:
// Camera stabilization with Roll / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll) * CAM_SMOOTHING_ROLL), 1000, 2000)); // Roll correction
break;
case 2:
// Camera stabilization with Roll / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll) * CAM_SMOOTHING_ROLL), 1000, 2000)); // Roll correction
break;
case 3:
// Camera stabilization with Yaw / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
case 4:
// Camera stabilization with Yaw / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
// x Mode flying setup
case 5:
// Camera stabilization with Roll / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll + pitch) * CAM_SMOOTHING), 1000, 2000)); // Roll correction
break;
case 6:
// Camera stabilization with Roll / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) + (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_ROLL_OUT, limitRange((CAM_CENT + (roll + pitch) * CAM_SMOOTHING), 1000, 2000)); // Roll correction
break;
case 7:
// Camera stabilization with Yaw / Tilt mounts, NO transmitter input
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((CAM_CENT - (roll - pitch) * CAM_SMOOTHING), 1000, 2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])), 1000, 2000)); // Roll correction
break;
case 8:
// Camera stabilization with Yaw / Tilt mounts, transmitter controls basic "zerolevel"
APM_RC.OutputCh(CAM_TILT_OUT, limitRange((APM_RC.InputCh(CAM_TILT_CH) - (roll - pitch) * CAM_SMOOTHING),1000,2000)); // Tilt correction
APM_RC.OutputCh(CAM_YAW_OUT, limitRange((CAM_CENT - (gyro_offset_yaw - AN[2])),1000,2000)); // Yaw correction
break;
// Only in case of we have switch values over 10
default:
// We should not be here...
break;
}
}
#endif