Bozza del controllore PID

src/main.cpp

@@ -46,6 +46,8 @@ using namespace mxgui;
 /* This section contains every constant needed for guidance, with every tweakable parameter:
  * you should not need to edit anything behind the end of this section. */
 
+const float CONTROL_SATURATION_THRESHOLD = 0.5; //max output value of the PID controller
+
 const float target_test_lat = 45.54861;  //IN DEGREES: convert the *,",' notation to degrees using an online tool.
 const float target_test_long = 10.46833;  //IN DEGREES: convert the *,",' notation to degrees using an online tool.
 const uint8_t imu_heading_speed = 10;  //IN KPH: defines which speed is considered too slow to use the GPS for heading, and use the IMU instead
@@ -235,6 +237,62 @@ void* inputTest()
   return 0;
 }
 
+float getError(float target_heading, float current_heading)
+{
+  float error = target_heading - current_heading;
+  if(abs(error) > 180)
+  {
+    error = fmod(360+error, 180);
+  }
+  return error / 180; //Errore in [-1 ; 1]
+}
+
+void* controlTest()
+{
+  const float Kp = 0, Ki = 0; //Todo: trovare dei valori sensati
+
+  float last_error = 0;
+  float error_integral = 0;
+  float error;
+
+  float pid_output, control_output;
+
+  float dt = 0;
+
+  while(true) //Todo: add exit condition
+  {
+    uint32_t start = miosix::getTick();
+
+    rogallina = gps1->getGuidanceValues(target_test_lat, target_test_long);
+    if (rogallina.speed < imu_heading_speed || rogallina.gps_fix == 0)  //se sono troppo lento il gps non funziona e devo usare la imu per l'heading (oppure quando NON ho GPS)
+    {
+      error = 0; //Se non abbiamo una misura accurata della direzione, meglio non fare nulla
+      //error_integral = 0; Todo: meglio annullare l'azione integrale?
+    }else{
+      error = getError(rogallina.heading_to_target, rogallina.actual_heading);
+    }
+
+    error_integral += dt * (error + last_error) / 2; //Formula trapezi composita
+
+    pid_output = Kp*error + Ki*error_integral; //Controllore di tipo PI (senza parte derivativa)
+
+    int servo = pid_output > 0 ? right_servo :  left_servo; //In base al segno, scelgo quale servo usare
+
+    control_output = abs(pid_output);
+
+    if(pid_output > CONTROL_SATURATION_THRESHOLD) //Limita l'azione di controllo in modo da evitare lo stallo
+      control_output = CONTROL_SATURATION_THRESHOLD;
+
+    servoGotoPosition(servo, pid_output);
+
+    last_error = error;
+
+    dt = (miosix::getTick() - start) * 1.0f / miosix::TICK_FREQ;
+  }
+
+  return 0;
+}
+
 /**
  * Manda la vela in superstallo, tirando entrambi i cordini al massimo.
  * Poco dopo, rilascia il tutto per vedere se si recupera l'assetto.