stabilization_andi.h

Go to the documentation of this file.

/*
 *
 * Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 *
 */
 
#ifndef STABILIZATION_ANDI_H
#define STABILIZATION_ANDI_H
 
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "generated/airframe.h"
#include <stdio.h>
#include "filters/low_pass_filter.h"
 
 
#ifndef ANDI_NUM_ACT
  #define ANDI_NUM_ACT COMMANDS_NB_REAL
#endif
 
#ifndef ANDI_OUTPUTS
  #error "You must specify the number of controlled axis (outputs)"
#endif
 
struct AttQuat {
  struct FloatQuat att;
  struct FloatRates att_d;
  struct FloatVect3 att_2d;
  struct FloatVect3 att_3d;
};
 
struct AttStateQuat {
  struct FloatQuat att;
  struct FloatRates att_d;
  struct FloatVect3 att_2d;
};
 
struct LinState {
  struct FloatVect3 vel;
  struct FloatVect3 acc;
};
 
struct ThrustRef {
  float thrust;
  float thrust_d;
};
 
struct PolesOrder3Vect3 {
  struct FloatVect3 omega_n;
  struct FloatVect3 zeta;
  struct FloatVect3 omega_a;
};
 
struct PolesOrder2Vect3 {
  struct FloatVect3 omega_a;
  struct FloatVect3 zeta;
};
 
struct GainsOrder2Vect3 {
  struct FloatVect3 k1;
  struct FloatVect3 k2;
};
 
struct GainsOrder3Vect3 {
  struct FloatVect3 k1;
  struct FloatVect3 k2;
  struct FloatVect3 k3;
};
 
/* Declaration of Reference Model and Error Controller Poles */
extern struct PolesOrder2Vect3 andi_p_rate_ec;
extern struct PolesOrder2Vect3 andi_p_rate_rm;
extern struct PolesOrder3Vect3 andi_p_att_ec;
extern struct PolesOrder3Vect3 andi_p_att_rm;
extern float andi_p_thrust_ec;
extern float andi_p_thrust_rm;
 
void stabilization_andi_init(void);
void stabilization_andi_enter(void);
void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint,
                            struct ThrustSetpoint *thrust_setpoint, int32_t *cmd);
// void actuator_debug(bool do_servo_step, bool do_motor_step, float step_rate);
 
struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
                                      const float actuator_state[ANDI_NUM_ACT]);
 
struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body,
                                       const float actuator_state[ANDI_NUM_ACT]);
 
void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT * ANDI_OUTPUTS], const struct FloatRates *rates,
                          const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]);
 
void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body,
                          const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT]);
 
 
float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT]);
#endif // STABILIZATION_ANDI_H