preparing for camera stabilization features

git-svn-id: https://arducopter.googlecode.com/svn/trunk@756 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArducopterNG/ArduUser.h

@@ -90,8 +90,18 @@ TODO:
 #define CHANN_CENTER 1500       // Channel center, legacy
 #define MIN_THROTTLE 1040       // Throttle pulse width at minimun...
 
+/* ******************************************************** */
+// Camera related settings
+
 #define CAM_CENT  1500          // Camera center
-#define CAM_SMOOTHING 1000      // Camera movement smoothing
+#define CAM_SMOOTHING 1000      // Camera movement smoothing on pitch axis
+#define CAM_SMOOTHING_ROLL -400 // Camera movement smoothing on roll axis
+
+#define CAM_TILT_OUT   6        // OUTx pin for Tilt servo
+#define CAM_ROLL_OUT   7        // OUTx pin for Roll servo
+#define CAM_YAW_OUT    7        // OUTx pin for Yaw servo (often same as Roll)
+
+#define CAM_TILT_CH  CH_7       // Channel for radio knob to controll tilt "zerolevel" 
 
 /*************************************************************/
 // General definitions

ArducopterNG/Arducopter.h

@@ -251,6 +251,9 @@ boolean SW_DIP2;
 boolean SW_DIP3;
 boolean SW_DIP4;  // closest to header pins
 
+boolean BATTLOW = FALSE;    // We should be always FALSE, if we are TRUE.. it means destruction is close, 
+                            // shut down all secondary systems that uses our precious mAh's
+
 // Attitude PID controls
 float roll_I=0;
 float roll_D;
@@ -312,7 +315,7 @@ int Sonar_Counter=0;
 
 // AP_mode : 1=> Position hold  2=>Stabilization assist mode (normal mode)
 byte AP_mode = 2;  
-byte cam_mode = 0;
+//byte cam_mode = 0;
 
 // Mode LED timers and variables, used to blink LED_Green
 byte gled_status = HIGH;
@@ -340,7 +343,7 @@ int frontMotor;
 int   backMotor;
 int   leftMotor;
 int   rightMotor;
-byte motorArmed = 0;
+byte  motorArmed = 0;                              // 0 = motors disarmed, 1 = motors armed
 int   minThrottle = 0;
 boolean flightOrientation = 0;                    // 0 = +, 1 = x this is read from DIP1 switch during system bootup
 
@@ -491,6 +494,7 @@ float ch_throttle_offset = 0;
 float ch_yaw_offset          = 0;
 float ch_aux_offset          = 0;
 float ch_aux2_offset         = 0;
+byte cam_mode                = 0;
 
 // This function call contains the default values that are set to the ArduCopter
 // when a "Default EEPROM Value" command is sent through serial interface
@@ -552,6 +556,7 @@ void defaultUserConfig() {
   ch_yaw_offset              = 0;
   ch_aux_offset              = 0;
   ch_aux2_offset             = 0;
+  cam_mode                   = 0;
 }
 
 // EEPROM storage addresses
@@ -612,4 +617,5 @@ void defaultUserConfig() {
 #define ch_yaw_offset_ADR      216
 #define ch_aux_offset_ADR      220
 #define ch_aux2_offset_ADR     224
+#define cam_mode_ADR           226
 

ArducopterNG/ArducopterNG.pde

@@ -49,9 +49,10 @@
 //#define IsNEWMTEK   // Do we have MTEK with new firmware
 //#define IsMAG       // Do we have a Magnetometer connected, if have remember to activate it from Configurator
 //#define IsAM        // Do we have motormount LED's. AM = Atraction Mode
+//#define IsCAM       // Do we have camera stabilization in use, If you activate, check OUTPUT pins from ArduUser.h
 
 //#define UseAirspeed  // Quads don't use AirSpeed... Legacy, jp 19-10-10
-//#define UseBMP
+//#define UseBMP       // Use pressure sensor
 //#define BATTERY_EVENT 1   // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
 
 #define CONFIGURATOR
@@ -98,7 +99,7 @@
 //#define MAGORIENTATION  APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD    // If you have soldered Magneto to IMU shield in WIki pictures shows
 
 // To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
-// you need to roll/bank/tilt/yaw/shake etc your ArduCoptet. Don't kick like Jani always does :)
+// you need to roll/bank/tilt/yaw/shake etc your ArduCopter. Don't kick like Jani always does :)
 #define MAGOFFSET 0,0,0
 
 // Declination is a correction factor between North Pole and real magnetic North. This is different on every location
@@ -152,7 +153,7 @@
 //#include <GPS_NMEA.h> 	// ArduPilot NMEA GPS library
 
 /* Software version */
-#define VER 1.51    // Current software version (only numeric values)
+#define VER 1.52    // Current software version (only numeric values)
 
 
 /* ************************************************************ */
@@ -251,8 +252,10 @@ void loop()
     // Send output commands to motor ESCs...
     motor_output();
 
+#ifdef IsCAM
   // Do we have cameras stabilization connected and in use?
   if(!SW_DIP2) camera_output();
+#endif
 
     // Autopilot mode functions
     if (AP_mode == AP_AUTOMATIC_MODE)

ArducopterNG/EEPROM.pde

@@ -114,6 +114,7 @@ void readUserConfig() {
   ch_yaw_offset = readEEPROM(ch_yaw_offset_ADR);
   ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
   ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR);
+  cam_mode = readEEPROM(cam_mode_ADR);
 }
 
 void writeUserConfig() {
@@ -174,4 +175,6 @@ void writeUserConfig() {
   writeEEPROM(ch_yaw_offset, ch_yaw_offset_ADR);
   writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
   writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
+  writeEEPROM(cam_mode, cam_mode_ADR);
+  
 }

ArducopterNG/Functions.pde

@@ -25,7 +25,7 @@
  
  * ************************************************************** *
  ChangeLog:
- 
+ 30-10-10 added basic camera stabilization functions with jumptables
  
  * ************************************************************** *
  TODO:
@@ -73,11 +73,9 @@ void RunningLights(int LightStep) {
 }
 
 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
@@ -133,6 +131,14 @@ boolean APMPinRead(volatile unsigned char &Port, byte Pin)
     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
@@ -140,13 +146,76 @@ boolean APMPinRead(volatile unsigned char &Port, byte Pin)
 // 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
+// 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
 
 
 

ArducopterNG/GCS.pde

@@ -105,7 +105,10 @@ void readSerialCommand() {
       acc_offset_y = readFloatSerial();
       acc_offset_z = readFloatSerial();
       break;
-    case 'K': // Spare
+    case 'K': // Camera mode
+              // 1 = Tilt / Roll without 
+      cam_mode = readFloatSerial();
+      //BATTLOW = readFloatSerial();
       break;      
     case 'M': // Receive debug motor commands
       frontMotor = readFloatSerial();
@@ -271,8 +274,11 @@ void sendSerialTelemetry() {
     AN_OFFSET[5] = acc_offset_z;
     queryType = 'X';
     break;
-  case 'L': // Spare
+  case 'L': // Camera settings and 
-    //    RadioCalibration();
+    SerPri(cam_mode, DEC);
+    tab();
+    SerPri(BATTLOW, DEC);
+    tab();
     queryType = 'X';
     break;
   case 'N': // Send magnetometer config
@@ -354,8 +360,8 @@ void sendSerialTelemetry() {
     comma();
     SerPri(read_adc(3));
     comma();
-    SerPrln(read_adc(5));
+    SerPri(read_adc(5));
-/*    comma();
+    /*    comma();
      SerPri(APM_Compass.Heading, 4);
      comma();
      SerPri(APM_Compass.Heading_X, 4);
@@ -368,7 +374,8 @@ void sendSerialTelemetry() {
      comma();
      SerPri(APM_Compass.Mag_Z);
      comma();
-*/
+     */
+    SerPriln();
     break;
   case 'T': // Spare
     break;
@@ -425,6 +432,29 @@ void sendSerialTelemetry() {
     SerPrln(ch_aux2_offset);
     queryType = 'X';
     break;
+  case '3':  // Jani's debugs
+    SerPri(yaw);
+    tab();
+    SerPri(command_rx_yaw);
+    tab();
+    SerPri(control_yaw);
+    tab();
+    SerPri(err_yaw);
+    tab();
+    SerPri(AN[0]);
+    tab();
+    SerPri(AN[1]);
+    tab();
+    SerPri(AN[2] - gyro_offset_yaw);
+    tab();
+    SerPri(gyro_offset_yaw - AN[2]);
+    tab();
+    SerPri(gyro_offset_yaw);
+    tab();
+    SerPri(1500 - (gyro_offset_yaw - AN[2]));
+    tab();
+    SerPriln();
+    break;
   case '.': // Modify GPS settings, print directly to GPS Port
     Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n");
     break;
@@ -460,3 +490,4 @@ float readFloatSerial() {
   return atof(data);
 }
 
+

ArducopterNG/Sensors.pde

@@ -65,7 +65,7 @@ void calibrateSensors(void) {
   for(i=0;i<600;i++)
   {
     Read_adc_raw();   // Read sensors
-    for(gyro=GYROZ; gyro<=GYROY; gyro++)   
+    for(gyro = GYROZ; gyro <= GYROY; gyro++)   
       aux_float[gyro] = aux_float[gyro] * 0.8 + AN[gyro] * 0.2;     // Filtering  
     Log_Write_Sensor(AN[0], AN[1], AN[2], AN[3], AN[4], AN[5], 0);