/*
   $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;
}