1,605 changes: 1,605 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

147 changes: 147 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
@@ -0,0 +1,147 @@
#ifndef EKF_AW_H
#define EKF_AW_H

#ifdef __cplusplus
extern "C" {
#endif

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise

// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise

// Model Based Parameters
float vehicle_mass;

// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];

// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];

// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];

// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};

struct ekfHealth {
bool healthy;
uint16_t crashes_n;
};

extern struct ekfAwParameters ekf_aw_params;

// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);

// Filtering functions
extern void ekf_aw_propagate(struct FloatVect3 *acc, struct FloatRates *gyro, struct FloatEulers *euler,
float *pusher_RPM, float *hover_RPM_array, float *skew, float *elevator_angle, struct FloatVect3 *V_gnd,
struct FloatVect3 *acc_filt, float *V_pitot, float dt);

// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);

extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);

// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);


// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
#endif

#endif /* EKF_AW_H */
468 changes: 468 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file

Large diffs are not rendered by default.

118 changes: 118 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
@@ -0,0 +1,118 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"


// EKF structure
struct ekfAw {

// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;

// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles

#define EKF_AW_RPM_HOVER_NUM 4
int32_t last_RPM_hover[EKF_AW_RPM_HOVER_NUM]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[EKF_AW_RPM_HOVER_NUM]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;


// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed

// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;

// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];

// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];

// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;

};

extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);

extern void set_in_air_status(bool);


extern float tau_filter_high;
extern float tau_filter_low;

extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}

#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */
229 changes: 229 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c
View file
@@ -0,0 +1,229 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@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/>.
*/

/** @file "modules/rot_wing_drone/wing_rotation_controller_v3b.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module to control wing rotation servo command based on prefered angle setpoint
*/

#include "modules/rot_wing_drone/wing_rotation_controller_servo.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"

#include "state.h"

#include <stdlib.h>
#include "mcu_periph/adc.h"

/*
#ifndef WING_ROTATION_CONTROLLER_SERVO_IDX
#error "No WING_ROTATION_CONTROLLER_SERVO_IDX defined"
#endif
*/

#if !USE_NPS

#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION
#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION ADC_5
#endif

#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES
#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES 16
#endif

#endif // !USE_NPS

#ifndef WING_ROTATION_CONTROLLER_POSITION_ADC_0
#error "No WING_ROTATION_CONTROLLER_POSITION_ADC_0 defined"
#endif

#ifndef WING_ROTATION_CONTROLLER_POSITION_ADC_90
#error "No WING_ROTATION_CONTROLLER_POSITION_ADC_90 defined"
#endif

#ifndef WING_ROTATION_CONTROLLER_FIRST_DYN
#define WING_ROTATION_CONTROLLER_FIRST_DYN 0.001
#endif

#ifndef WING_ROTATION_CONTROLLER_SECOND_DYN
#define WING_ROTATION_CONTROLLER_SECOND_DYN 0.003
#endif

// Parameters
struct wing_rotation_controller_t wing_rotation_controller = {0};

bool in_transition = false;

#if !USE_NPS
static struct adc_buf buf_wing_rot_pos;
#endif

#if USE_NPS
#include "modules/actuators/actuators.h"
#endif

// Inline functions
inline void wing_rotation_to_rad(void);
inline void wing_rotation_update_sp(void);
inline void wing_rotation_compute_pprz_cmd(void);

void wing_rotation_init(void)
{
// your init code here
#if !USE_NPS
adc_buf_channel(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES);
#endif

// Init wing_rotation_controller struct
wing_rotation_controller.wing_angle_virtual_deg_sp = 45;
wing_rotation_controller.wing_rotation_first_order_dynamics = WING_ROTATION_CONTROLLER_FIRST_DYN;
wing_rotation_controller.wing_rotation_second_order_dynamics = WING_ROTATION_CONTROLLER_SECOND_DYN;
wing_rotation_controller.forward_airspeed = 18.;
}

void wing_rotation_periodic(void)
{
// After 5 loops, set current setpoint and enable wing_rotation
// freq = 1.0 Hz
if (!wing_rotation_controller.initialized) {
wing_rotation_controller.init_loop_count += 1;
if (wing_rotation_controller.init_loop_count > 4) {
wing_rotation_controller.initialized = true;
wing_rotation_controller.wing_angle_deg_sp = 45.;
}
}
}

void wing_rotation_event(void)
{
// First check if safety switch is triggered
#ifdef WING_ROTATION_CONTROLLER_RESET_RADIO_CHANNEL
// Update wing_rotation deg setpoint when RESET switch triggered
if (radio_control.values[WING_ROTATION_CONTROLLER_RESET_RADIO_CHANNEL] > 1750) {
wing_rotation_controller.airspeed_scheduling = false;
wing_rotation_controller.wing_angle_deg_sp = 0;
}
#endif

if (guidance_h.mode == GUIDANCE_H_MODE_FORWARD) {
set_wing_rotation_scheduler(true);
} else if (guidance_h.mode == GUIDANCE_H_MODE_ATTITUDE) {
set_wing_rotation_scheduler(false);
} else if (guidance_h.mode == GUIDANCE_H_MODE_NAV) {
set_wing_rotation_scheduler(wing_rotation_controller.airspeed_scheduling_nav);
}

if (!wing_rotation_controller.airspeed_scheduling) {
wing_rotation_controller.transition_forward = false;
}

// Update Wing position sensor
wing_rotation_to_rad();

// Run control if initialized
if (wing_rotation_controller.initialized) {

if (wing_rotation_controller.airspeed_scheduling) {
float wing_angle_scheduled_sp_deg = 0;
float airspeed = stateGetAirspeed_f();
if (airspeed < 8) {
in_transition = false;
wing_angle_scheduled_sp_deg = 0;
} else if (airspeed < 10 && in_transition) {
wing_angle_scheduled_sp_deg = 55;
} else if (airspeed > 10) {
wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.;
in_transition = true;
} else {
wing_angle_scheduled_sp_deg = 0;
}

Bound(wing_angle_scheduled_sp_deg, 0., 90.)
wing_rotation_controller.wing_angle_deg_sp = wing_angle_scheduled_sp_deg;

}

wing_rotation_update_sp();

// Control the wing rotation position.
wing_rotation_compute_pprz_cmd();
}
}

void wing_rotation_to_rad(void)
{
#if !USE_NPS
wing_rotation_controller.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample;

wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294;

#else
// Copy setpoint as actual angle in simulation
wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp;
#endif
}

void wing_rotation_update_sp(void)
{
}

void wing_rotation_compute_pprz_cmd(void)
{
float angle_error = wing_rotation_controller.wing_angle_deg_sp - wing_rotation_controller.wing_angle_virtual_deg_sp;
float speed_sp = wing_rotation_controller.wing_rotation_first_order_dynamics * angle_error;
float speed_error = speed_sp - wing_rotation_controller.wing_rotation_speed;
wing_rotation_controller.wing_rotation_speed += wing_rotation_controller.wing_rotation_second_order_dynamics * speed_error;
wing_rotation_controller.wing_angle_virtual_deg_sp += wing_rotation_controller.wing_rotation_speed;

#if !USE_NPS
int32_t servo_pprz_cmd; // Define pprz cmd
servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ);
Bound(servo_pprz_cmd, 0, MAX_PPRZ);

wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd;
#else
int32_t servo_pprz_cmd; // Define pprz cmd
servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_deg_sp / 90. * (float)MAX_PPRZ);
Bound(servo_pprz_cmd, 0, MAX_PPRZ);

wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd;
actuators_pprz[INDI_NUM_ACT] = servo_pprz_cmd;
#endif
}

// Function to call in flightplan to switch scheduler on or off
bool set_wing_rotation_scheduler(bool rotation_scheduler_on)
{
if (rotation_scheduler_on) {
wing_rotation_controller.airspeed_scheduling = true;
} else {
wing_rotation_controller.airspeed_scheduling = false;
if (!wing_rotation_controller.force_rotation_angle) {
wing_rotation_controller.wing_angle_deg_sp = 0;
}
}
return false;
}

bool set_wing_rotation_scheduler_nav(bool rotation_scheduler_on)
{
wing_rotation_controller.airspeed_scheduling_nav = rotation_scheduler_on;
return false;
}
58 changes: 58 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h
View file
@@ -0,0 +1,58 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@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/>.
*/

/** @file "modules/rot_wing_drone/wing_rotation_controller_servo.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module to control wing rotation servo command based on prefered angle setpoint
*/

#ifndef WING_ROTATION_CONTROLLER_SERVO_H
#define WING_ROTATION_CONTROLLER_SERVO_H

#include "std.h"

extern void wing_rotation_init(void);
extern void wing_rotation_periodic(void);
extern void wing_rotation_event(void);
extern bool set_wing_rotation_scheduler(bool rotation_scheduler_on);
extern bool set_wing_rotation_scheduler_nav(bool rotation_scheduler_on);

// Paramaters
struct wing_rotation_controller_t {
int32_t servo_pprz_cmd;
uint16_t adc_wing_rotation;
float wing_angle_deg;
float wing_angle_deg_sp;
float wing_rotation_speed;
float wing_angle_virtual_deg_sp;
float wing_rotation_first_order_dynamics;
float wing_rotation_second_order_dynamics;
bool initialized;
uint8_t init_loop_count;
bool airspeed_scheduling;
bool airspeed_scheduling_nav;
bool force_rotation_angle;
bool transition_forward;
float forward_airspeed;
};

extern struct wing_rotation_controller_t wing_rotation_controller;

#endif // WING_ROTATION_CONTROLLER_SERVO_H