ANGLE MODE WORKINGgit add . Fly test succeed!

Author: liourej

CodeDroneDIY.h

@@ -67,6 +67,10 @@ void InitTimer1() {
 
 void PrintSettings(StateMachine _stateMachine) {
   Serial.println(F("/********* settings *********/"));
+  if ( MAX_POWER == 1860)
+    Serial.println(F("FLYING MODE POWER!!!\t"));
+  else if ( MAX_POWER <= 1300)
+    Serial.println(F("DEBUG MODE POWER!!!\t"));
   Serial.print(F("MAX_POWER:\t")); Serial.println(MAX_POWER);
   if ( _stateMachine.state == angle) {
     Serial.println(F("FLYING_MODE_ANGLE"));
@@ -78,7 +82,7 @@ void PrintSettings(StateMachine _stateMachine) {
     pitchSpeedPID.PrintGains();
     yawSpeedPID.PrintGains();
     Serial.println(F("/********* Complementary filter *********/"));
-    Serial.print("Coefficient:\t");Serial.print(Position.HighPassFilterCoeff); Serial.print("\tTime constant:\t");Serial.println(Position.GetFilterTimeConstant(0.00249));
+    Serial.print("Coefficient:\t"); Serial.print(Position.HighPassFilterCoeff); Serial.print("\tTime constant:\t"); Serial.println(Position.GetFilterTimeConstant(0.00249));
   } else if ( _stateMachine.state == accro) {
     Serial.println(F("FLYING_MODE_ACCRO"));
     Serial.println(F("/********* PID settings *********/"));

CodeDroneDIY.ino

@@ -218,7 +218,6 @@ void loop() {
         pitchSpeedPID.SetGains(angleSpeedPIDParams);
         yawSpeedPID.SetGains(yawSpeedPIDParams);
         stateMachine.statePrev = stateMachine.state;
-        Serial.println("ANGLE MODE");
         PrintSettings(stateMachine);
 
       } else if ( (stateMachine.state != disarmed) && ( stateMachine.state == accro ) ) {
@@ -227,7 +226,6 @@ void loop() {
         yawSpeedPID.SetGains(yawSpeedPIDParams);
 
         stateMachine.statePrev = stateMachine.state;
-        Serial.println("ACCRO MODE");
         PrintSettings(stateMachine);
       } else
         stateMachine.state = starting;

GetPosition.cpp

@@ -151,7 +151,7 @@ void GetPosition::GetCurrPos(MPU6050 _accelgyro, float _pos[], float _speed[], f
 
   Normalize(accRaw);
 
-  /*static int counter = 0;
+ /* static int counter = 0;
   static float pitchGyro = 0.0;
   float pitchAcc = 0.0;
   pitchGyro = pitchGyro + (gyroRaw[1]) * _loop_time;

PID.cpp

@@ -5,7 +5,6 @@ void PID::SetGains(float _params[4]) {
   Kp = _params[1];
   Kd = _params[2];
   Ki = _params[3];
-  PrintGains();
 };
 
 void PID::Reset() {

Settings.h

@@ -6,7 +6,7 @@
 #define GAIN 0.010
 
 // Angle mode
-float anglePosPIDParams[4] = { 0.010, 260, 0.5, 0.3};// G, Kp, Kd, Ki
+float anglePosPIDParams[4] = { 0.010, 268, 0.5, 0.0};// G, Kp, Kd, Ki
 float angleSpeedPIDParams[4] = { 0.010, 192, 0.0, 0.0};
 
 // Accro mode