mirror of https://github.com/ArduPilot/ardupilot
work on progress, updated camera functions, rearranging module configs
git-svn-id: https://arducopter.googlecode.com/svn/trunk@767 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
a5cb22207a
commit
6f70fddea4
|
|
@ -59,10 +59,19 @@ TODO:
|
|||
#define RI_LED AN10 // Mega PH4 pin, OUT5
|
||||
#define LE_LED AN8 // Mega PH5 pin, OUT4
|
||||
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// Special patterns for future use
|
||||
|
||||
/*
|
||||
#define POFF L1\0x00\0x00\0x05
|
||||
#define PALL L1\0xFF\0xFF\0x05
|
||||
|
||||
#define GPS_AM_PAT1 L\0x00\0x00\0x05
|
||||
#define GPS_AM_PAT2 L\0xFF\0xFF\0x05
|
||||
#define GPS_AM_PAT3 L\0xF0\0xF0\0x05
|
||||
|
||||
*/
|
||||
|
||||
/* AM PIN Definitions - END */
|
||||
|
||||
|
|
@ -103,6 +112,8 @@ TODO:
|
|||
|
||||
#define CAM_TILT_CH CH_7 // Channel for radio knob to controll tilt "zerolevel"
|
||||
|
||||
|
||||
|
||||
/*************************************************************/
|
||||
// General definitions
|
||||
//Modes
|
||||
|
|
|
|||
|
|
@ -315,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; // moved to general settings, 31-10-10, jp
|
||||
|
||||
// Mode LED timers and variables, used to blink LED_Green
|
||||
byte gled_status = HIGH;
|
||||
|
|
@ -439,6 +439,15 @@ unsigned long elapsedTime = 0; // for doing custom events
|
|||
// SEVERITY_HIGH
|
||||
// SEVERITY_CRITICAL
|
||||
|
||||
// Different GPS devices,
|
||||
|
||||
#define GPSDEV_DIYMTEK 1
|
||||
#define GPSDEV_DIYUBLOX 2
|
||||
#define GPSDEV_FPUBLOX 3
|
||||
#define GPSDEV_IMU 4
|
||||
#define GPSDEV_NMEA 5
|
||||
|
||||
|
||||
// Following variables stored in EEPROM
|
||||
float KP_QUAD_ROLL;
|
||||
float KI_QUAD_ROLL;
|
||||
|
|
@ -495,6 +504,13 @@ float ch_yaw_offset = 0;
|
|||
float ch_aux_offset = 0;
|
||||
float ch_aux2_offset = 0;
|
||||
byte cam_mode = 0;
|
||||
byte mag_orientation = 0; // mag variables, reserved for future use, 31-10-10, jp
|
||||
float mag_declination = 0.0;
|
||||
float mag_offset_x = 0; // is int enough for offsets.. checkit, 31-10-10, jp
|
||||
float mag_offset_y = 0;
|
||||
float mag_offset_z = 0;
|
||||
//float eeprom_counter; // reserved for eeprom write counter, 31-10-10, jp
|
||||
//float eeprom_checker; // reserved for eeprom checksums, 01-11-10, jp
|
||||
|
||||
// This function call contains the default values that are set to the ArduCopter
|
||||
// when a "Default EEPROM Value" command is sent through serial interface
|
||||
|
|
@ -557,6 +573,11 @@ void defaultUserConfig() {
|
|||
ch_aux_offset = 0;
|
||||
ch_aux2_offset = 0;
|
||||
cam_mode = 0;
|
||||
mag_orientation = 0; // reserved for future, 31-10-10, jp
|
||||
mag_declination = 0.0;
|
||||
mag_offset_x = 0;
|
||||
mag_offset_y = 0;
|
||||
mag_offset_z = 0;
|
||||
}
|
||||
|
||||
// EEPROM storage addresses
|
||||
|
|
@ -618,4 +639,16 @@ void defaultUserConfig() {
|
|||
#define ch_aux_offset_ADR 220
|
||||
#define ch_aux2_offset_ADR 224
|
||||
#define cam_mode_ADR 226
|
||||
#define mag_orientation_ADR 228 // reserved for future, 31-10-10, jp
|
||||
#define mag_declination_ADR 230 // reserved for future, 31-10-10, jp
|
||||
#define mag_offset_x_ADR 232
|
||||
#define mag_offset_y_ADR 234
|
||||
#define mag_offset_z_ADR 236
|
||||
|
||||
|
||||
//#define eeprom_counter_ADR 238 // hmm should i move these?!? , 31-10-10, jp
|
||||
//#define eeprom_checker_ADR 240 // this too... , 31-10-10, jp
|
||||
|
||||
|
||||
|
||||
// end of file
|
||||
|
|
|
|||
|
|
@ -40,10 +40,18 @@
|
|||
/* ************************************************************ */
|
||||
|
||||
/* ************************************************************ */
|
||||
// User definable modules, check them every time you have new
|
||||
// software version at your hand.
|
||||
|
||||
// User MODULES
|
||||
//
|
||||
// Please check your modules settings for every new software downloads you have.
|
||||
// Also check repository / ArduCopter wiki pages for ChangeLogs and software notes
|
||||
//
|
||||
// Comment out with // modules that you are not using
|
||||
//
|
||||
// Do check ArduUser.h settings file too !!
|
||||
//
|
||||
///////////////////////////////////////
|
||||
// Modules Config
|
||||
// --------------------------
|
||||
|
||||
//#define IsGPS // Do we have a GPS connected
|
||||
//#define IsNEWMTEK // Do we have MTEK with new firmware
|
||||
|
|
@ -57,6 +65,14 @@
|
|||
|
||||
#define CONFIGURATOR
|
||||
|
||||
///////////////////////////////////////
|
||||
// GPS Selection
|
||||
|
||||
#define GPSDEVICE GPSDEV_DIYMTEK // For DIY Drones MediaTek
|
||||
//#define GPSDEVICE GPSDEV_DIYUBLOX // For DIY Drones uBlox GPS
|
||||
//#define GPSDEVICE GPSDEV_FPUBLOX // For Fah Pah Special ArduCopter GPS
|
||||
//#define GPSDEVICE GPSDEV_NMEA // For general NMEA compatible GPSEs
|
||||
//#dedine GPSDEVICE GPSDEV_IMU // For IMU Simulations only
|
||||
|
||||
////////////////////
|
||||
// Serial ports & speeds
|
||||
|
|
@ -88,12 +104,13 @@
|
|||
//#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
|
||||
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
|
||||
// 19-10-10 by JP
|
||||
|
||||
// This feature has been disabled for now, if you want to change between flight orientations
|
||||
// just use DIP switch for that. DIP1 down = X, DIP1 up = +
|
||||
|
||||
// Magneto orientation and corrections.
|
||||
// If you don't have magneto actiavted, It is safe to ignore these
|
||||
#ifdef IsMAG
|
||||
// If you don't have magneto activated, It is safe to ignore these
|
||||
//#ifdef IsMAG
|
||||
#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD // This is default solution for ArduCopter
|
||||
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_BACK // Alternative orientation for ArduCopter
|
||||
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD // If you have soldered Magneto to IMU shield in WIki pictures shows
|
||||
|
|
@ -123,7 +140,7 @@
|
|||
//#define DECLINATION -6.08 // Jose, Canary Islands, 6°5'W
|
||||
//#define DECLINATION 0.73 // Tony, Minneapolis, 0°44'E
|
||||
|
||||
#endif
|
||||
//#endif
|
||||
|
||||
|
||||
|
||||
|
|
@ -131,6 +148,7 @@
|
|||
/* **************** MAIN PROGRAM - INCLUDES ******************* */
|
||||
/* ************************************************************ */
|
||||
|
||||
//#include <AP_GPS.h>
|
||||
#include <avr/io.h>
|
||||
#include <avr/eeprom.h>
|
||||
#include <avr/pgmspace.h>
|
||||
|
|
@ -143,14 +161,18 @@
|
|||
#include <Wire.h> // I2C Communication library
|
||||
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
|
||||
#include <EEPROM.h> // EEPROM
|
||||
//#include <AP_GPS.h>
|
||||
#include "Arducopter.h"
|
||||
#include "ArduUser.h"
|
||||
|
||||
#ifdef IsGPS
|
||||
// GPS library (Include only one library)
|
||||
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
|
||||
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
|
||||
//#include <GPS_UBLOX.h> // ArduPilot Ublox GPS Library
|
||||
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
|
||||
#include <GPS_MTK.h> // ArduPilot MTK GPS Library
|
||||
//#include <GPS_IMU.h> // ArduPilot IMU/SIM GPS Library
|
||||
//#include <GPS_UBLOX.h> // ArduPilot Ublox GPS Library
|
||||
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
|
||||
#endif
|
||||
|
||||
|
||||
/* Software version */
|
||||
#define VER 1.52 // Current software version (only numeric values)
|
||||
|
|
@ -262,14 +284,17 @@ void loop()
|
|||
{
|
||||
if (target_position)
|
||||
{
|
||||
#ifdef IsGPS
|
||||
if (GPS.NewData) // New GPS info?
|
||||
{
|
||||
read_GPS_data();
|
||||
Position_control(target_lattitude,target_longitude); // Call GPS position hold routine
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else // First time we enter in GPS position hold we capture the target position as the actual position
|
||||
{
|
||||
#ifdef IsGPS
|
||||
if (GPS.Fix){ // We need a GPS Fix to capture the actual position...
|
||||
target_lattitude = GPS.Lattitude;
|
||||
target_longitude = GPS.Longitude;
|
||||
|
|
@ -279,6 +304,7 @@ void loop()
|
|||
Initial_Throttle = ch_throttle;
|
||||
Reset_I_terms_navigation(); // Reset I terms (in Navigation.pde)
|
||||
}
|
||||
#endif
|
||||
command_gps_roll=0;
|
||||
command_gps_pitch=0;
|
||||
}
|
||||
|
|
@ -290,7 +316,9 @@ void loop()
|
|||
// Medium loop (about 60Hz)
|
||||
if ((currentTime-mediumLoop)>=17){
|
||||
mediumLoop = currentTime;
|
||||
#ifdef IsGPS
|
||||
GPS.Read(); // Read GPS data
|
||||
#endif
|
||||
// Each of the six cases executes at 10Hz
|
||||
switch (medium_loopCounter){
|
||||
case 0: // Magnetometer reading (10Hz)
|
||||
|
|
|
|||
|
|
@ -57,6 +57,8 @@ void writeEEPROM(float value, int address) {
|
|||
}
|
||||
|
||||
void readUserConfig() {
|
||||
// eeprom_counter = readEEPROM(eeprom_counter_ADR);
|
||||
// eeprom_checker = readEEPROM(eeprom_checker_ADR);
|
||||
KP_QUAD_ROLL = readEEPROM(KP_QUAD_ROLL_ADR);
|
||||
KI_QUAD_ROLL = readEEPROM(KI_QUAD_ROLL_ADR);
|
||||
STABLE_MODE_KP_RATE_ROLL = readEEPROM(STABLE_MODE_KP_RATE_ROLL_ADR);
|
||||
|
|
@ -115,9 +117,19 @@ void readUserConfig() {
|
|||
ch_aux_offset = readEEPROM(ch_aux_offset_ADR);
|
||||
ch_aux2_offset = readEEPROM(ch_aux2_offset_ADR);
|
||||
cam_mode = readEEPROM(cam_mode_ADR);
|
||||
mag_orientation = readEEPROM(mag_orientation_ADR);
|
||||
mag_declination = readEEPROM(mag_declination_ADR);
|
||||
mag_offset_x = readEEPROM(mag_offset_x_ADR);
|
||||
mag_offset_y = readEEPROM(mag_offset_y_ADR);
|
||||
mag_offset_z = readEEPROM(mag_offset_z_ADR);
|
||||
|
||||
}
|
||||
|
||||
void writeUserConfig() {
|
||||
// eeprom_counter = readEEPROM(eeprom_counter_ADR);
|
||||
// if(eeprom_counter > 0) eeprom_counter = 0;
|
||||
// eeprom_counter++;
|
||||
// writeEEPROM(eeprom_counter, eeprom_counter_ADR);
|
||||
writeEEPROM(KP_QUAD_ROLL, KP_QUAD_ROLL_ADR);
|
||||
writeEEPROM(KI_QUAD_ROLL, KI_QUAD_ROLL_ADR);
|
||||
writeEEPROM(STABLE_MODE_KP_RATE_ROLL, STABLE_MODE_KP_RATE_ROLL_ADR);
|
||||
|
|
@ -176,5 +188,11 @@ void writeUserConfig() {
|
|||
writeEEPROM(ch_aux_offset, ch_aux_offset_ADR);
|
||||
writeEEPROM(ch_aux2_offset, ch_aux2_offset_ADR);
|
||||
writeEEPROM(cam_mode, cam_mode_ADR);
|
||||
writeEEPROM(mag_orientation, mag_orientation_ADR);
|
||||
writeEEPROM(mag_declination, mag_declination_ADR);
|
||||
writeEEPROM(mag_offset_x, mag_offset_x_ADR);
|
||||
writeEEPROM(mag_offset_y, mag_offset_y_ADR);
|
||||
writeEEPROM(mag_offset_z, mag_offset_z_ADR);
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@
|
|||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
31-10-10 Moved camera stabilization from Functions to here, jp
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
|
@ -34,3 +34,85 @@ 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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -139,83 +139,8 @@ int limitRange(int data, int minLimit, int maxLimit) {
|
|||
}
|
||||
|
||||
|
||||
|
||||
/* **************************************************** */
|
||||
// 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
|
||||
// Camera functions moved to event due it's and event 31-10-10, jp
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -6,9 +6,9 @@
|
|||
File : Navigation.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
|
||||
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
|
||||
|
|
@ -22,24 +22,25 @@
|
|||
|
||||
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:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
- initial functions.
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
- initial functions.
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
void read_GPS_data()
|
||||
{
|
||||
#ifdef IsGPS
|
||||
GPS_timer_old=GPS_timer; // Update GPS timer
|
||||
GPS_timer = millis();
|
||||
GPS_Dt = (GPS_timer-GPS_timer_old)*0.001; // GPS_Dt
|
||||
GPS.NewData=0; // We Reset the flag...
|
||||
|
||||
|
||||
// Write GPS data to DataFlash log
|
||||
Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude,GPS.Altitude,GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
|
||||
|
||||
|
|
@ -48,6 +49,7 @@ void read_GPS_data()
|
|||
digitalWrite(LED_Red,HIGH); // GPS Fix => RED LED
|
||||
else
|
||||
digitalWrite(LED_Red,LOW);
|
||||
#endif
|
||||
}
|
||||
|
||||
/* GPS based Position control */
|
||||
|
|
@ -56,24 +58,25 @@ void Position_control(long lat_dest, long lon_dest)
|
|||
long Lon_diff;
|
||||
long Lat_diff;
|
||||
|
||||
#ifdef IsGPS // it is safe to disable this, if no IsGPS we never come here anyways
|
||||
Lon_diff = lon_dest - GPS.Longitude;
|
||||
Lat_diff = lat_dest - GPS.Lattitude;
|
||||
|
||||
#endif
|
||||
// ROLL
|
||||
//Optimization : cos(yaw) = DCM_Matrix[0][0] ; sin(yaw) = DCM_Matrix[1][0]
|
||||
gps_err_roll = (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[0][0] - (float)Lat_diff * DCM_Matrix[1][0];
|
||||
|
||||
gps_roll_D = (gps_err_roll-gps_err_roll_old) / GPS_Dt;
|
||||
gps_err_roll_old = gps_err_roll;
|
||||
|
||||
|
||||
gps_roll_I += gps_err_roll * GPS_Dt;
|
||||
gps_roll_I = constrain(gps_roll_I, -800, 800);
|
||||
|
||||
command_gps_roll = KP_GPS_ROLL * gps_err_roll + KD_GPS_ROLL * gps_roll_D + KI_GPS_ROLL * gps_roll_I;
|
||||
command_gps_roll = constrain(command_gps_roll, -GPS_MAX_ANGLE, GPS_MAX_ANGLE); // Limit max command
|
||||
|
||||
|
||||
//Log_Write_PID(1,KP_GPS_ROLL*gps_err_roll*10,KI_GPS_ROLL*gps_roll_I*10,KD_GPS_ROLL*gps_roll_D*10,command_gps_roll*10);
|
||||
|
||||
|
||||
// PITCH
|
||||
gps_err_pitch = -(float)Lat_diff * DCM_Matrix[0][0] - (float)Lon_diff * GEOG_CORRECTION_FACTOR * DCM_Matrix[1][0];
|
||||
|
||||
|
|
@ -95,4 +98,5 @@ void Reset_I_terms_navigation()
|
|||
gps_roll_I = 0;
|
||||
gps_pitch_I = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue