Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Some corrections on attitude file. Magnetometer tests 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@814 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-11-09 21:38:54 +00:00
parent a771cbacc0
commit 0ce4e75c79
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArducopterNG/ArducopterNG.pde
View File

@ -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

4
ArducopterNG/Attitude.pde
View File

@ -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