diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index d4826b84be..95c9137f55 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -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 diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index d606b4c743..ba11397988 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -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 diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index b3ef15e9c6..ac023fe7b0 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -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 #include #include #include @@ -143,14 +161,18 @@ #include // I2C Communication library #include // ArduPilot Mega BMP085 Library #include // EEPROM +//#include #include "Arducopter.h" #include "ArduUser.h" +#ifdef IsGPS // GPS library (Include only one library) -#include // ArduPilot MTK GPS Library -//#include // ArduPilot IMU/SIM GPS Library -//#include // ArduPilot Ublox GPS Library -//#include // ArduPilot NMEA GPS library +#include // ArduPilot MTK GPS Library +//#include // ArduPilot IMU/SIM GPS Library +//#include // ArduPilot Ublox GPS Library +//#include // 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) diff --git a/ArducopterNG/EEPROM.pde b/ArducopterNG/EEPROM.pde index 562a9a9636..1ff6b1e844 100644 --- a/ArducopterNG/EEPROM.pde +++ b/ArducopterNG/EEPROM.pde @@ -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); + } diff --git a/ArducopterNG/Events.pde b/ArducopterNG/Events.pde index ed1bec4389..d115476858 100644 --- a/ArducopterNG/Events.pde +++ b/ArducopterNG/Events.pde @@ -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 + + diff --git a/ArducopterNG/Functions.pde b/ArducopterNG/Functions.pde index c17aee8d5b..0b58ef6c36 100644 --- a/ArducopterNG/Functions.pde +++ b/ArducopterNG/Functions.pde @@ -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 diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index 7c0379a152..78d6c77a0b 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -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 . - -* ************************************************************** * -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; } - + +