Re: [Paparazzi-devel] Add advanced control algorithm to paparazzi

[Mike]

Hi Felix,
    I follow your advice, take
http://docs.paparazziuav.org/latest/module__ctrl_module_demo.html as an
example. I just write a new controler (ADRC control) to replace attitude
loop used by PID control, now I just write a stabilization loop (use ADRC
control) from radio value to output "stabilization_cmd". But what to do
next? How can I make a new mode for rotorcraft to test my own controler and
turn my own parameters in GCS? Is there need to modify any other .c files or
.makefiles?  Here is my new controler codes (ADRC):
  ctrl_module_ADRC.c:
   /**
 * @file modules/ctrl/ctrl_module_ADRC.h
 * @ADRC controller to replace the PID control in attitude loop
 *
 */

#include "modules/ctrl/ctrl_module_ADRC.h"
#include "state.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include
"firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "math/pprz_algebra_int.h"
#include <math.h>
#include "paparazzi.h"
#include "std.h"

#define MAX_ADRC_ERR 4000000
#define Sign(_x) ((_x) > 0 ? 1 : (-1))
#define ABS(val) ((val) < 0 ? -(val) : (val))


struct ctrl_module_demo_struct {
  int rc_t;
} ctrl_module_ADRC;

struct Int32Eulers ADRC_sp;
struct Int32Eulers *ADRC_ltp_to_body_euler = stateGetNedToBodyEulers_i();


float r0 = 100.,h = 0.001;
float B01 = 1000.,B02 = 1000.,B03 = 1000.,r = 500.;
float u_roll,u_pitch,u_yaw;

float ctrl_module_ADRC_b0_gain = 2f;  // Pitch/Roll
float ctrl_module_ADRC_c_gain = 1f;
float ctrl_module_ADRC_h1_gain = 0.005f;   // Yaw

float fhan (float x1,float x2,float r,float h){
        float d,a0,y,a1,a2,Sy,a,Sa;
        d = r*h*h;
        a0 = h*x2;
        y = x1 + a0;
        a1 = sqrt(d*(d+8.*ABS(y));
        a2 = a0 + (Sign(y)*(a1-d))/2.;
        Sy = (Sign(y+d)-Sign(y-d))/2.;
        a = (a0+y-a2)*Sy+a2;
        Sa = (Sign(a+d)-Sign(a-d))/2.;
        return -r*(a/d-Sign(a))*Sa-r*Sign(a);
        }

float fal (float x,float a, float q){
        if (ABS(x)<=q){
                return x/pow(q,1.-a);
                } else {
                return Sign(x)*pow(ABS(x),a);
                }
        )

void ADRC_roll (float roll_sp){
    float fh,roll_1,roll_2,e,z11,z12,z13,fe1,fe2,e1,e2,u_roll_0;
        fh = fhan(roll_1-roll_sp,roll_2,r0,h);
        roll_1 = roll_1 + h*roll_2;
        roll_2 = roll_2 + h*fh;
        e = z11 - ADRC_ltp_to_body_euler.phi;
        fe1 = fal(e,0.5,h);
        fe2 = fal(e,0.25,h);
        z11 = z11 + h*(z12-B01*e);
    z12 = z12 + h*(z13-B02*fe1+ctrl_module_ADRC_b0_gain*u_roll);
        z13 = z13 + h*(-B03*fe2);
        e1 = roll_1 - z11;
        e2 = roll_2 - z12;
        u_roll_0 = 
fhan(e1,ctrl_module_ADRC_c_gain*e2,r,ctrl_module_ADRC_h1_gain);
        u_roll = (u_roll_0-z13)/ctrl_module_ADRC_b0_gain;
                }

void ADRC_pitch (float pitch_sp){
    float fh,pitch_1,pitch_2,e,z21,z22,z23,fe1,fe2,e1,e2,u_pitch_0;
        fh = fhan(pitch_1-pitch_sp,pitch_2,r0,h);
        pitch_1 = pitch_1 + h*roll_2;
        pitch_2 = pitch_2 + h*fh;
        e = z21 - ADRC_ltp_to_body_euler.theta;
        fe1 = fal(e,0.5,h);
        fe2 = fal(e,0.25,h);
        z21 = z21 + h*(z22-B01*e);
    z22 = z22 + h*(z23-B02*fe1+ctrl_module_ADRC_b0_gain*u_pitch);
        z23 = z23 + h*(-B03*fe2);
        e1 = pitch_1 - z21;
        e2 = pitch_2 - z22;
        u_pitch_0 = 
fhan(e1,ctrl_module_ADRC_c_gain*e2,r,ctrl_module_ADRC_h1_gain);
        u_pitch = (u_pitch_0-z23)/ctrl_module_ADRC_b0_gain;
                }

void ADRC_yaw (float yaw_sp){
    float fh,yaw_1,yaw_2,e,z31,z32,z33,fe1,fe2,e1,e2,u_yaw_0;
        fh = fhan(yaw_1-yaw_sp,yaw_2,r0,h);
        yaw_1 = yaw_1 + h*yaw_2;
        yaw_2 = yaw_2 + h*fh;
        e = z31 - ADRC_ltp_to_body_euler.psi;
        fe1 = fal(e,0.5,h);
        fe2 = fal(e,0.25,h);
        z31 = z31 + h*(z32-B01*e);
    z32 = z32 + h*(z33-B02*fe1+ctrl_module_ADRC_b0_gain*u_yaw);
        z33 = z33 + h*(-B03*fe2);
        e1 = yaw_1 - z31;
        e2 = yaw_2 - z32;
        u_yaw_0 = 
fhan(e1,ctrl_module_ADRC_c_gain*e2,r,ctrl_module_ADRC_h1_gain);
        u_yaw = (u_yaw_0-z33)/ctrl_module_ADRC_b0_gain;
                }

                
void ctrl_module_init(void);
void ctrl_module_run(bool_t in_flight);

void ctrl_module_init(void)
{
  radio_control.values[RADIO_ROLL] = 0;
  radio_control.values[RADIO_PITCH] = 0;
  radio_control.values[RADIO_YAW] = 0;
  ctrl_module_demo.rc_t = 0;
}

// simple rate control without reference model nor attitude
void ctrl_module_run(bool_t in_flight)
{
  if (!in_flight) {
    // Reset integrators
    stabilization_cmd[COMMAND_ROLL] = 0;
    stabilization_cmd[COMMAND_PITCH] = 0;
    stabilization_cmd[COMMAND_YAW] = 0;
    stabilization_cmd[COMMAND_THRUST] = 0;
  } else {
        ADRC_roll(ADRC_sp.phi);
        ADRC_pitch(ADRC_sp.theta);
        ADRC_yaw(ADRC_sp.psi);
        
        stabilization_cmd[COMMAND_ROLL]   = u_roll;
    stabilization_cmd[COMMAND_PITCH]  = u_pitch;
    stabilization_cmd[COMMAND_YAW]    = u_yaw;
    stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
    
        BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
    BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
    BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
        BoundAbs(stabilization_cmd[COMMAND_THRUST], MAX_PPRZ);
  }
}


////////////////////////////////////////////////////////////////////
// Call our controller
// Implement own Horizontal loops
void guidance_h_module_enter(void)
{
  ctrl_module_init();
}

void guidance_h_module_read_rc(void)
{
  // Read the values from RC, and transform to eular setpoint
  ADRC_sp->phi = get_rc_roll();
  ADRC_sp->theta = get_rc_pitch();
  ADRC_sp->psi = get_rc_yaw();
  ctrl_module_ADRC.rc_t = radio_control.values[RADIO_THROTTLE];
}

void guidance_h_module_run(bool_t in_flight)
{
  // Call full inner-/outerloop / horizontal-/vertical controller:
  ctrl_module_run(in_flight);
}

// Implement own Vertical loops
void guidance_v_module_enter(void)
{
  // your code that should be executed when entering this vertical mode goes
here
}

void guidance_v_module_run(UNUSED bool_t in_flight)
{
  // your vertical controller goes here
}

ctrl_module_ADRC.h:
/**
 * @file modules/ctrl/ctrl_module_demo.c
 * @brief example empty controller
 *
 * Implements an example simple rate controller in a module.
 */

#ifndef CTRL_MODULE_ADRC_H_
#define CTRL_MODULE_ADRC_H_

#include <std.h>

// Settings
extern float ctrl_module_ADRC_b0_gain;  
extern float ctrl_module_ADRC_c_gain;
extern float ctrl_module_ADRC_h1_gain;  




// Demo with own guidance_h
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE

// and own guidance_v
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE

// Implement own Horizontal loops
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);

// Implement own Vertical loops
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);

#endif /* CTRL_MODULE_DEMO_H_ */

ctrl_module_ADRC.xml:
<!DOCTYPE module SYSTEM &quot;module.dtd&quot;>

<module name="ctrl_module_demo" dir="ctrl">
  <doc>
    <description>
        Demo Control Module.
        Only for rotorcraft firmware.
        Simple rate controler as example on how to integrate write and call
your own controller in a module.
    </description>
  </doc>
  <settings>
    <dl_settings>
      <dl_settings NAME="CtrlModADRC">
        <dl_setting var="ctrl_module_ADRC_b0_gain" min="0" step="0.01"
max="10" module="ctrl/ctrl_module_ADRC" shortname="ADRC_b0"/>
        <dl_setting var="ctrl_module_ADRC_c_gain"  min="0" step="0.01"
max="10" module="ctrl/ctrl_module_ADRC" shortname="ADRC_c"/>
        <dl_setting var="ctrl_module_ADRC_h1_gain"  min="0" step="0.01"
max="1" module="ctrl/ctrl_module_ADRC" shortname="ADRC_h1"/>
      </dl_settings>
    </dl_settings>
  </settings>

  <header>
    <file name="ctrl_module_ADRC.h"/>
  </header>

  <makefile>
    <file name="ctrl_module_ADRC.c"/>
  </makefile>

</module>

Thanks
Mike



--
View this message in context: 
http://lists.paparazziuav.org/Add-advanced-control-algorithm-to-paparazzi-tp16861p16891.html
Sent from the paparazzi-devel mailing list archive at Nabble.com.