Ardupilot2/ArducopterNG/Functions.pde

/*
 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 <http://www.gnu.org/licenses/>.
 
 * ************************************************************** *
 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