mirror of https://github.com/ArduPilot/ardupilot
119 lines
5.5 KiB
Plaintext
119 lines
5.5 KiB
Plaintext
/*
|
|
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
|
|
|
|
|