/* www.ArduCopter.com - www.DIYDrones.com Copyright (c) 2010. All rights reserved. An Open Source Arduino based multicopter. File : Functions.pde Version : v1.0, Aug 28, 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 . * ************************************************************** * ChangeLog: 30-10-10 added basic camera stabilization functions with jumptables * ************************************************************** * TODO: * ************************************************************** */ // Flash those A,B,C LEDs on IMU Board // // Function: FullBlink(int, int); // int 1 = void FullBlink(int count, int blinkdelay) { for(int i = 0; i <= count; i++) { digitalWrite(LED_Green, HIGH); digitalWrite(LED_Yellow, HIGH); digitalWrite(LED_Red, HIGH); delay(blinkdelay); digitalWrite(LED_Green, LOW); digitalWrite(LED_Yellow, LOW); digitalWrite(LED_Red, LOW); delay(blinkdelay); } } void RunningLights(int LightStep) { if(LightStep == 0) { digitalWrite(LED_Green, HIGH); digitalWrite(LED_Yellow, LOW); digitalWrite(LED_Red, LOW); } else if (LightStep == 1) { digitalWrite(LED_Green, LOW); digitalWrite(LED_Yellow, HIGH); digitalWrite(LED_Red, LOW); } else { digitalWrite(LED_Green, LOW); digitalWrite(LED_Yellow, LOW); digitalWrite(LED_Red, HIGH); } } void LightsOff() { digitalWrite(LED_Green, LOW); digitalWrite(LED_Yellow, LOW); digitalWrite(LED_Red, LOW); } // Funtion to normalize an angle in degrees to -180,180 degrees float Normalize_angle(float angle) { if (angle > 180) return (angle - 360.0); else if (angle < -180) return (angle + 360.0); else return(angle); } // Maximun slope filter for radio inputs... (limit max differences between readings) int channel_filter(int ch, int ch_old) { int diff_ch_old; if (ch_old==0) // ch_old not initialized return(ch); diff_ch_old = ch - ch_old; // Difference with old reading if (diff_ch_old < 0) { if (diff_ch_old <- 60) return(ch_old - 60); // We limit the max difference between readings } else { if (diff_ch_old > 60) return(ch_old + 60); } return((ch + ch_old) >> 1); // Small filtering //return(ch); } // Special APM PinMode settings and others void APMPinMode(volatile unsigned char &Port, byte Pin, boolean Set) { if (Set) { Port |= (1 << Pin); } else { Port &= ~(1 << Pin); } } boolean APMPinRead(volatile unsigned char &Port, byte Pin) { if(Port & (1 << Pin)) return 1; else return 0; } // Faster and smaller replacement for contrain() function int limitRange(int data, int minLimit, int maxLimit) { if (data < minLimit) return minLimit; else if (data > maxLimit) return maxLimit; else return data; } /* **************************************************** */ // 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 case values over 10 default: // We should not be here... break; } } #endif