From 0ce4e75c796278b5b134d34c03ff36d100581fea Mon Sep 17 00:00:00 2001
From: jjulio1234 <jjulio1234@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Tue, 9 Nov 2010 21:38:54 +0000
Subject: [PATCH] Some corrections on attitude file. Magnetometer tests

git-svn-id: https://arducopter.googlecode.com/svn/trunk@814 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArducopterNG/ArducopterNG.pde | 8 ++++----
 ArducopterNG/Attitude.pde     | 4 ++++
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde
index b39a5fae7a..4e90ce2088 100644
--- a/ArducopterNG/ArducopterNG.pde
+++ b/ArducopterNG/ArducopterNG.pde
@@ -53,9 +53,9 @@
 //  Modules Config
 // --------------------------
 
-//#define IsGPS       // Do we have a GPS connected
-//#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 IsGPS       // Do we have a GPS connected
+#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
 
@@ -402,4 +402,4 @@ void loop()
 
 }
 
-
+
diff --git a/ArducopterNG/Attitude.pde b/ArducopterNG/Attitude.pde
index 26afe38818..b10ce2b3bf 100644
--- a/ArducopterNG/Attitude.pde
+++ b/ArducopterNG/Attitude.pde
@@ -58,6 +58,7 @@ TODO:
 // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
 void Attitude_control_v3(int command_roll, int command_pitch, int command_yaw)
 {
+  #define MAX_CONTROL_OUTPUT 250
   float stable_roll,stable_pitch,stable_yaw;
   
   // ROLL CONTROL    
@@ -73,6 +74,7 @@ void Attitude_control_v3(int command_roll, int command_pitch, int command_yaw)
   // PD rate control (we use also the bias corrected gyro rates)
   err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
   control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
+  control_roll = constrain(control_roll,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
   
   // PITCH CONTROL
   err_pitch = command_pitch - ToDeg(pitch);
@@ -87,6 +89,7 @@ void Attitude_control_v3(int command_roll, int command_pitch, int command_yaw)
   // P rate control (we use also the bias corrected gyro rates)
   err_pitch = stable_pitch - ToDeg(Omega[1]);
   control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
+  control_pitch = constrain(control_pitch,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
   
   // YAW CONTROL
   err_yaw = command_yaw - ToDeg(yaw);
@@ -104,6 +107,7 @@ void Attitude_control_v3(int command_roll, int command_pitch, int command_yaw)
   // PD rate control (we use also the bias corrected gyro rates)
   err_yaw = stable_yaw - ToDeg(Omega[2]);
   control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw;
+  control_yaw = constrain(control_yaw,-MAX_CONTROL_OUTPUT,MAX_CONTROL_OUTPUT);
 }
 
 // ACRO MODE