637 changes: 637 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.c
View file

Large diffs are not rendered by default.

84 changes: 84 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.h
View file
@@ -0,0 +1,84 @@
/*
* Copyright (C) 2023 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/rotwing/rotwing_state.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.
*/

#ifndef ROTWING_STATE_H
#define ROTWING_STATE_H

#include "std.h"

/** Rotwing States
*/
#define ROTWING_STATE_HOVER 0 // Wing is skewed in 0 degrees (quad)
#define ROTWING_STATE_SKEWING 1 // WIng is skewing
#define ROTWING_STATE_FW 2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority
#define ROTWING_STATE_FW_HOV_MOT_IDLE 3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle
#define ROTWING_STATE_FW_HOV_MOT_OFF 4 // Wing is skewed at 90 degrees (fixed wubg), hover motors are switched off
#define ROTWING_STATE_FREE 5 // This is a desired state for which the controller has to decide the desired state itself

struct RotwingState {
uint8_t current_state;
uint8_t desired_state;
};

#define ROTWING_STATE_WING_QUAD_SETTING 0 // Wing skew at 0 degrees
#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler
#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees

#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover
#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled
#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward

struct RotWingStateSettings {
uint8_t wing_scheduler;
bool hover_motors_active;
bool hover_motors_disable;
bool force_forward;
uint8_t preferred_pitch;
bool stall_protection;
float max_v_climb;
float max_v_descend;
};

struct RotWingStateSkewing {
float wing_angle_deg_sp; // Wing angle setpoint in deg
float wing_angle_deg; // Wing angle from sensor in deg
bool airspeed_scheduling; // Airspeed scheduling on or off
bool force_rotation_angle; // Setting to force wing_angle_deg_sp
};

extern struct RotwingState rotwing_state;
extern struct RotWingStateSettings rotwing_state_settings;
extern struct RotWingStateSkewing rotwing_state_skewing;

extern bool rotwing_state_force_quad;

extern bool hover_motors_active;
extern bool bool_disable_hover_motors;

extern void init_rotwing_state(void);
extern void periodic_rotwing_state(void);
extern void request_rotwing_state(uint8_t state);

#endif // ROTWING_STATE_H
43 changes: 14 additions & 29 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c
View file
Expand Up @@ -24,10 +24,14 @@
*/

#include "modules/rot_wing_drone/wing_rotation_controller_servo.h"
#include "modules/rot_wing_drone/rotwing_state.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "generated/airframe.h"
#include "modules/core/abi.h"
#include "state.h"

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

#if USE_NPS
Expand Down Expand Up @@ -58,6 +62,7 @@
// 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;
Expand All @@ -77,47 +82,27 @@ void wing_rotation_init(void)
#endif

// Init Data
wing_rotation_controller.wing_angle_virtual_deg_sp = 45;
wing_rotation_controller.wing_angle_virtual_deg_sp = 0;
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;
}

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)
{
// Update Wing position sensor
wing_rotation_adc_to_deg();

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

// Setpoint checks
Bound(wing_rotation_controller.wing_angle_deg_sp, 0., 90.);

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

void wing_rotation_adc_to_deg(void)
{
#if !USE_NPS
// Read ADC
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;

wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294;
#else // !USE_NPS
// Copy setpoint as actual angle in simulation
wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp;
Expand All @@ -131,11 +116,11 @@ void wing_rotation_adc_to_deg(void)

// Send ABI message
AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);

}

void wing_rotation_compute_pprz_cmd(void)
{
wing_rotation_controller.wing_angle_deg_sp = rotwing_state_skewing.wing_angle_deg_sp;
// Smooth accerelation and rate limited setpoint
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;
Expand All @@ -148,8 +133,8 @@ void wing_rotation_compute_pprz_cmd(void)
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;

actuators_pprz[SERVO_ROTATION_MECH] = servo_pprz_cmd;
}


#if USE_NPS
actuators_pprz[INDI_NUM_ACT] = servo_pprz_cmd;
#endif
}
4 changes: 0 additions & 4 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h
View file
Expand Up @@ -29,7 +29,6 @@
#include "std.h"

extern void wing_rotation_init(void);
extern void wing_rotation_periodic(void);
extern void wing_rotation_event(void);

// Paramaters
Expand All @@ -44,9 +43,6 @@ struct wing_rotation_controller_t {
float wing_angle_virtual_deg_sp; ///< Rate limiter state variable 2
float wing_rotation_first_order_dynamics; ///< Rate limiter for wing rotation
float wing_rotation_second_order_dynamics; ///< Acceleration limiter for wing rotation

bool initialized; ///< Wing rotation controller initialized
uint8_t init_loop_count; ///< Wing rotation controller initialization loop count
};

// Setters
Expand Down