Add wasm tacle-bench targets

targets/wasm-tacle/parallel/PapaBench/sw/airborne/autopilot/pid.c

@@ -0,0 +1,139 @@
+/*
+   $Id: pid.c,v 1.2 2011-01-21 11:52:44 moellmer Exp $
+
+   Copyright (C) 2003  Pascal Brisset, Antoine Drouin
+
+   This file is part of paparazzi.
+
+   paparazzi is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2, or (at your option)
+   any later version.
+
+   paparazzi is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with paparazzi; see the file COPYING.  If not, write to
+   the Free Software Foundation, 59 Temple Place - Suite 330,
+   Boston, MA 02111-1307, USA.
+
+*/
+
+/** \file pid.c
+    \brief PID controllers (roll, pitch, climb, altitude, course).
+
+*/
+
+//#include <stdlib.h>
+#include <math.h>
+
+#include "pid.h"
+
+#include "autopilot.h"
+#include "infrared.h"
+#include "estimator.h"
+#include "nav.h"
+
+float desired_roll = 0.;
+float desired_pitch = 0.;
+int16_t desired_gaz, desired_aileron, desired_elevator;
+float roll_pgain = ROLL_PGAIN;
+float pitch_pgain = PITCH_PGAIN;
+float pitch_of_roll = PITCH_OF_ROLL;
+
+float pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN;
+float pitch_of_vz = 0.;
+
+
+/** \brief Computes ::desired_aileron and ::desired_elevator from attitude estimation and expected attitude. */
+void roll_pitch_pid_run( void )
+{
+  float err =  estimator_phi - desired_roll;
+  desired_aileron = TRIM_PPRZ( roll_pgain * err );
+  if ( pitch_of_roll < 0. )
+    pitch_of_roll = 0.;
+  err = -( estimator_theta - desired_pitch - pitch_of_roll * fabs(
+             estimator_phi ) );
+  desired_elevator = TRIM_PPRZ( pitch_pgain * err );
+}
+
+float course_pgain = COURSE_PGAIN;
+float desired_course = 0.;
+float max_roll = MAX_ROLL;
+
+void course_pid_run( void )
+{
+  float err = estimator_hspeed_dir - desired_course;
+  //NORM_RAD_ANGLE(err);
+  _Pragma( "loopbound min 0 max 1" )
+  while ( err > M_PI ) err -= 2 * M_PI;
+  _Pragma( "loopbound min 0 max 1" )
+  while ( err < -M_PI ) err += 2 * M_PI;
+  nav_desired_roll = course_pgain * err; //  * fspeed / AIR_SPEED;
+  if ( nav_desired_roll > max_roll )
+    nav_desired_roll = max_roll;
+  else
+    if ( nav_desired_roll < -max_roll )
+      nav_desired_roll = -max_roll;
+}
+
+const float climb_pgain   = CLIMB_PGAIN;
+const float climb_igain   =  CLIMB_IGAIN;
+float desired_climb = 0., pre_climb = 0.;
+static const float level_gaz = CLIMB_LEVEL_GAZ;
+float climb_sum_err  = 0;
+
+float climb_pitch_pgain = CLIMB_PITCH_PGAIN;
+float climb_pitch_igain = CLIMB_PITCH_IGAIN;
+float climb_pitch_sum_err = 0.;
+float max_pitch = MAX_PITCH;
+float min_pitch = MIN_PITCH;
+
+
+#define MAX_CLIMB_SUM_ERR 100
+#define MAX_PITCH_CLIMB_SUM_ERR 100
+
+/** \brief Computes desired_gaz and desired_pitch from desired_climb */
+void climb_pid_run ( void )
+{
+  float err  = estimator_z_dot - desired_climb;
+  float fgaz;
+  if ( auto_pitch ) { /* gaz constant */
+    desired_gaz = nav_desired_gaz;
+    desired_pitch = climb_pitch_pgain * ( err + climb_pitch_igain *
+                                          climb_pitch_sum_err );
+    if ( desired_pitch > max_pitch )
+      desired_pitch = max_pitch;
+    if ( desired_pitch < min_pitch )
+      desired_pitch = min_pitch;
+    climb_pitch_sum_err += err;
+    if ( climb_pitch_sum_err > MAX_PITCH_CLIMB_SUM_ERR )
+      climb_pitch_sum_err = MAX_PITCH_CLIMB_SUM_ERR;
+    if ( climb_pitch_sum_err < - MAX_PITCH_CLIMB_SUM_ERR )
+      climb_pitch_sum_err = - MAX_PITCH_CLIMB_SUM_ERR;
+  } else { /* pitch almost constant */
+    /* pitch offset for climb */
+    pitch_of_vz = ( desired_climb > 0 ) ? desired_climb * pitch_of_vz_pgain : 0.;
+    fgaz = climb_pgain * ( err + climb_igain * climb_sum_err ) + CLIMB_LEVEL_GAZ +
+           CLIMB_GAZ_OF_CLIMB * desired_climb;
+    climb_sum_err += err;
+    if ( climb_sum_err > MAX_CLIMB_SUM_ERR ) climb_sum_err = MAX_CLIMB_SUM_ERR;
+    if ( climb_sum_err < - MAX_CLIMB_SUM_ERR ) climb_sum_err = - MAX_CLIMB_SUM_ERR;
+    desired_gaz = TRIM_UPPRZ( fgaz * MAX_PPRZ );
+    desired_pitch = nav_pitch + pitch_of_vz;
+  }
+}
+
+float altitude_pgain = ALTITUDE_PGAIN;
+
+
+void altitude_pid_run( void )
+{
+  float err = estimator_z - desired_altitude;
+  desired_climb = pre_climb + altitude_pgain * err;
+  if ( desired_climb < -CLIMB_MAX ) desired_climb = -CLIMB_MAX;
+  if ( desired_climb > CLIMB_MAX ) desired_climb = CLIMB_MAX;
+}