516 changes: 516 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.c
View file

Large diffs are not rendered by default.

59 changes: 59 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.h
View file
@@ -0,0 +1,59 @@
/*
* Copyright (C) 2022 D.C. 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/ctrl/ctrl_eff_sched_rot_wing_v3b.h"
* @author D.C. van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Crtl effectiveness scheduler for thr rotating wing drone V3
*/

#ifndef CTRL_EFF_SCHED_ROT_WING_V3B_H
#define CTRL_EFF_SCHED_ROT_WING_V3B_H

#include "std.h"

// Define settings
extern float lift_d_multiplier;
extern float g1_p_multiplier;
extern float g1_q_multiplier;
extern float g1_r_multiplier;
extern float g1_t_multiplier;

extern float pitch_angle_set;
extern float pitch_angle_range;

extern float rot_wing_aerodynamic_eff_const_g1_p[1];
extern float rot_wing_aerodynamic_eff_const_g1_q[1];
extern float rot_wing_aerodynamic_eff_const_g1_r[1];

extern bool wing_rotation_sched_activated;
extern bool pusher_sched_activated;

extern float pitch_priority_factor;
extern float roll_priority_factor;
extern float thrust_priority_factor;
extern float pusher_priority_factor;

extern bool hover_motors_active;
extern bool bool_disable_hover_motors;

extern void init_eff_scheduling(void);
extern void event_eff_scheduling(void);

#endif // CTRL_EFF_SCHED_ROT_WING_V3_H
150 changes: 150 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.c
View file
@@ -0,0 +1,150 @@
/*
* 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/rot_wing_drone/rot_wing_automation.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#include "modules/rot_wing_drone/rot_wing_automation.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

#ifndef ROT_WING_AUTOMATION_TRANS_ACCEL
#define ROT_WING_AUTOMATION_TRANS_ACCEL 1.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_DECEL
#define ROT_WING_AUTOMATION_TRANS_DECEL 0.5
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_LENGTH
#define ROT_WING_AUTOMATION_TRANS_LENGTH 200.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_AIRSPEED
#define ROT_WING_AUTOMATION_TRANS_AIRSPEED 15.0
#endif

struct rot_wing_automation rot_wing_a;

// declare function
inline void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned);

void init_rot_wing_automation(void)
{
rot_wing_a.trans_accel = ROT_WING_AUTOMATION_TRANS_ACCEL;
rot_wing_a.trans_decel = ROT_WING_AUTOMATION_TRANS_DECEL;
rot_wing_a.trans_length = ROT_WING_AUTOMATION_TRANS_LENGTH;
rot_wing_a.trans_airspeed = ROT_WING_AUTOMATION_TRANS_AIRSPEED;
rot_wing_a.transitioned = false;
}

// periodic function
void periodic_rot_wing_automation(void)
{
float airspeed = stateGetAirspeed_f();
if (airspeed > rot_wing_a.trans_airspeed)
{
rot_wing_a.transitioned = true;
} else {
rot_wing_a.transitioned = false;
}
}

// Update a waypoint such that you can see on the GCS where the drone wants to go
void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned) {

// Update the waypoint
struct EnuCoor_f target_enu;
ENU_OF_TO_NED(target_enu, *target_ned);
waypoint_set_enu(wp_id, &target_enu);

// Send waypoint update every half second
RunOnceEvery(100/2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} );
}

// Function to visualize from flightplan, call repeadely
void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id)
{
//state eulers in zxy order
struct FloatEulers eulers_zxy;

// get heading and cos and sin of heading
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float psi = eulers_zxy.psi;
float cpsi = cosf(psi);
float spsi = sinf(psi);

// get airspeed
float airspeed = stateGetAirspeed_f();
// get groundspeed
float ground_speed = stateGetHorizontalSpeedNorm_f();

// get drone position
struct NedCoor_f *drone_pos = stateGetPositionNed_f();

// Move reference waypoints
// Move end transition waypoint at the correct length
struct FloatVect3 end_transition_rel_pos;
VECT3_COPY(end_transition_rel_pos, *drone_pos);
end_transition_rel_pos.x = cpsi * rot_wing_a.trans_length;
end_transition_rel_pos.y = spsi * rot_wing_a.trans_length;
struct FloatVect3 end_transition_pos;
VECT3_SUM(end_transition_pos, end_transition_rel_pos, *drone_pos);
end_transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_end_id, &end_transition_pos);

// Move transition waypoint
float airspeed_error = rot_wing_a.trans_airspeed - airspeed;
float transition_time = airspeed_error / rot_wing_a.trans_accel;
float average_ground_speed = ground_speed + airspeed_error / 2.;
float transition_distance = average_ground_speed * transition_time;

struct FloatVect3 transition_rel_pos;
VECT3_COPY(transition_rel_pos, *drone_pos);
transition_rel_pos.x = cpsi * transition_distance;
transition_rel_pos.y = spsi * transition_distance;
struct FloatVect3 transition_pos;
VECT3_SUM(transition_pos, transition_rel_pos, *drone_pos);
transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_transition_id, &transition_pos);

// Move decel point
float final_groundspeed = ground_speed + airspeed_error;
float decel_time = final_groundspeed / rot_wing_a.trans_decel;
float decel_distance = (final_groundspeed / 2.) * decel_time;
float decel_distance_from_drone = rot_wing_a.trans_length - decel_distance;

struct FloatVect3 decel_rel_pos;
VECT3_COPY(decel_rel_pos, *drone_pos);
decel_rel_pos.x = cpsi * decel_distance_from_drone;
decel_rel_pos.y = spsi * decel_distance_from_drone;
struct FloatVect3 decel_pos;
VECT3_SUM(decel_pos, decel_rel_pos, *drone_pos);
decel_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_decel_id, &decel_pos);
}
46 changes: 46 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.h
View file
@@ -0,0 +1,46 @@
/*
* 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/rot_wing_drone/rot_wing_automation.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#ifndef ROT_WING_AUTOMATION_H
#define ROT_WING_AUTOMATION_H

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

extern void init_rot_wing_automation(void);
extern void periodic_rot_wing_automation(void);
extern void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id);

struct rot_wing_automation {
float trans_accel;
float trans_decel;
float trans_length;
float trans_airspeed;
bool transitioned;
};

extern struct rot_wing_automation rot_wing_a;

#endif // ROT_WING_AUTOMATION_H
393 changes: 393 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.c
View file
@@ -0,0 +1,393 @@
/*
* 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.c"
* @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.
*/

#include "modules/rot_wing_drone/rotwing_state.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "firmwares/rotorcraft/autopilot_firmware.h"
#include "modules/rot_wing_drone/wing_rotation_controller_v3b.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"

struct RotwingState rotwing_state;

// Quad state identification
#ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0
#endif
#ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER
#define ROTWING_MIN_SKEW_ANGLE_COUNTER 10 // Minimum number of loops the skew angle is below ROTWING_MIN_SKEW_ANGLE_COUNTER
#endif

// Skewing state identification
#ifndef ROTWING_SKEWING_COUNTER
#define ROTWING_SKEWING_COUNTER 10 // Minimum number of loops the skew angle is in between QUAD and FW
#endif

// Half skew state identification
#ifndef ROTWING_HALF_SKEW_ANGLE_DEG
#define ROTWING_HALF_SKEW_ANGLE_DEG 55.0
#endif
#ifndef ROTWING_HALF_SKEW_ANGLE_RANG
#define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0
#endif
#ifndef ROTWING_HALF_SKEW_COUNTER
#define ROTWING_HALF_SKEW_COUNTER 10 // Minimum number of loops the skew angle is at HALF_SKEW_ANGLE_DEG +/- ROTWING_HALF_SKEW_ANGLE_HALF_RANGE to trigger ROTWING_HALF_SKEW_ANGLE state
#endif

// FW state identification
#ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG
#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state
#endif
#ifndef ROTWING_MIN_FW_COUNTER
#define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE
#endif

// FW idle state identification
#ifndef ROTWING_MIN_THRUST_IDLE
#define ROTWING_MIN_THRUST_IDLE 100
#endif
#ifndef ROTWING_MIN_THRUST_IDLE_COUNTER
#define ROTWING_MIN_THRUST_IDLE_COUNTER 10
#endif

// FW hov mot off state identification
#ifndef ROTWING_HOV_MOT_OFF_RPM_TH
#define ROTWING_HOV_MOT_OFF_RPM_TH 50
#endif
#ifndef ROTWING_HOV_MOT_OFF_COUNTER
#define ROTWING_HOV_MOT_OFF_COUNTER 10
#endif

#ifndef ROTWING_STATE_RPM_ID
#define ROTWING_STATE_RPM_ID ABI_BROADCAST
#endif
abi_event rotwing_state_rpm_ev;
static void rotwing_state_rpm_cb(uint8_t sender_id, struct rpm_act_t * rpm_message, uint8_t num_act);
#define ROTWING_STATE_NUM_HOVER_RPM 4
int32_t rotwing_state_hover_rpm[ROTWING_STATE_NUM_HOVER_RPM] = {0, 0, 0, 0};

bool rotwing_state_force_quad = false;

uint8_t rotwing_state_hover_counter = 0;
uint8_t rotwing_state_skewing_counter = 0;
uint8_t rotwing_state_fw_counter = 0;
uint8_t rotwing_state_fw_idle_counter = 0;
uint8_t rotwing_state_fw_m_off_counter = 0;

inline void rotwing_check_set_current_state(void);
inline void rotwing_update_desired_state(void);
inline void rotwing_switch_state(void);

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID,
&rotwing_state.current_state,
&rotwing_state.desired_state,
&wing_rotation.wing_angle_deg,
&wing_rotation.wing_angle_deg_sp,
&wing_rotation.adc_wing_rotation,
&wing_rotation.servo_pprz_cmd);
}
#endif // PERIODIC_TELEMETRY

void init_rotwing_state(void)
{
// Bind ABI messages
AbiBindMsgRPM(ROTWING_STATE_RPM_ID, &rotwing_state_rpm_ev, rotwing_state_rpm_cb);

// Start the drone in a desired hover state
rotwing_state.current_state = ROTWING_STATE_HOVER;
rotwing_state.desired_state = ROTWING_STATE_HOVER;

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state);
#endif
}

void periodic_rotwing_state(void)
{
// Check and set the current state
rotwing_check_set_current_state();

// Check and update desired state
rotwing_update_desired_state();

// Check difference with desired state
// rotwing_switch_state();

//TODO: incorparate motor active / disbaling depending on called flight state
// Switch on motors if flight mode is attitude
if (guidance_h.mode == GUIDANCE_H_MODE_ATTITUDE) {
bool_disable_hover_motors = false;
} else if (guidance_h.mode == GUIDANCE_H_MODE_FORWARD) {
bool_disable_hover_motors = false;
}
}

// Function to request a state
void request_rotwing_state(uint8_t state)
{
if (state == ROTWING_STATE_HOVER) {
rotwing_state.current_state = ROTWING_STATE_HOVER;
} else if (state == ROTWING_STATE_FW) {
rotwing_state.current_state = ROTWING_STATE_FW;
}
}

void rotwing_check_set_current_state(void)
{
// if !in_flight, set state to hover
if (!autopilot.in_flight) {
rotwing_state_hover_counter = 0;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_counter = 0;
rotwing_state_fw_idle_counter = 0;
rotwing_state_fw_m_off_counter = 0;
rotwing_state.current_state = ROTWING_STATE_HOVER;
return;
}

// States can be checked according to wing angle sensor, setpoints .....
uint8_t prev_state = rotwing_state.current_state;
switch (prev_state) {
case ROTWING_STATE_HOVER:
// Check if state needs to be set to skewing
if (wing_rotation.wing_angle_deg > ROTWING_MIN_SKEW_ANGLE_DEG_QUAD)
{
rotwing_state_skewing_counter++;
} else {
rotwing_state_skewing_counter = 0;
}

// Switch state if necessary
if (rotwing_state_skewing_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_SKEWING;
rotwing_state_skewing_counter = 0;
}
break;

case ROTWING_STATE_SKEWING:
// Check if state needs to be set to hover
if (wing_rotation.wing_angle_deg < ROTWING_MIN_SKEW_ANGLE_DEG_QUAD)
{
rotwing_state_hover_counter++;
} else {
rotwing_state_hover_counter = 0;
}

// Check if state needs to be set to fixed wing
if (wing_rotation.wing_angle_deg > ROTWING_MIN_FW_SKEW_ANGLE_DEG)
{
rotwing_state_fw_counter++;
} else {
rotwing_state_fw_counter = 0;
}

// Switch state if necessary
if (rotwing_state_hover_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_HOVER;
rotwing_state_hover_counter = 0;
}

if (rotwing_state_fw_counter > ROTWING_MIN_FW_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW;
rotwing_state_fw_counter = 0;
}
break;

case ROTWING_STATE_FW:
// Check if state needs to be set to skewing
if (wing_rotation.wing_angle_deg < ROTWING_MIN_FW_SKEW_ANGLE_DEG) {
rotwing_state_skewing_counter++;
} else {
rotwing_state_skewing_counter = 0;
}

// Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold)
if (stabilization_cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE) {
rotwing_state_fw_idle_counter++;
} else {
rotwing_state_fw_idle_counter = 0;
}

// Switch state if necessary
if (rotwing_state_skewing_counter > ROTWING_MIN_FW_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_SKEWING;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_idle_counter = 0;
} else if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER
&& rotwing_state_skewing_counter == 0) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_idle_counter = 0;
}
break;

case ROTWING_STATE_FW_HOV_MOT_IDLE:
// Check if state needs to be set to fixed wing with hover motors activated
if (stabilization_cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE) {
rotwing_state_fw_counter++;
} else {
rotwing_state_fw_counter = 0;
}

// Check if state needs to be set to fixed wing with hover motors off (if zero rpm on hover motors)
if (rotwing_state_hover_rpm[0] == 0
&& rotwing_state_hover_rpm[1] == 0
&& rotwing_state_hover_rpm[2] == 0
&& rotwing_state_hover_rpm[3] == 0) {
rotwing_state_fw_m_off_counter++;
} else {
rotwing_state_fw_m_off_counter = 0;
}

// Switch state if necessary
if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW;
rotwing_state_fw_counter = 0;
rotwing_state_fw_m_off_counter = 0;
} else if (rotwing_state_fw_m_off_counter > ROTWING_HOV_MOT_OFF_COUNTER
&& rotwing_state_fw_counter == 0) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_OFF;
rotwing_state_fw_counter = 0;
rotwing_state_fw_m_off_counter = 0;
}
break;

case ROTWING_STATE_FW_HOV_MOT_OFF:
// Check if state needs to be set to fixed wing with hover motors idle (if rpm on hover motors)
if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_OFF_RPM_TH) {
rotwing_state_fw_idle_counter++;
} else {
rotwing_state_fw_idle_counter = 0;
}

// Switch state if necessary
if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE;
rotwing_state_fw_idle_counter = 0;
}
break;

default:
break;
}
}

void rotwing_update_desired_state(void)
{
// If force_forward and nav selected, then the desired state if FW with hover motors off;
switch (guidance_h.mode) {
case GUIDANCE_H_MODE_NAV:
if (rotwing_state_force_quad) {
rotwing_state.desired_state = ROTWING_STATE_HOVER;
} else if (force_forward) {
rotwing_state.desired_state = ROTWING_STATE_FW_HOV_MOT_OFF;
} else {
rotwing_state.desired_state = ROTWING_STATE_FREE;
}
break;

case GUIDANCE_H_MODE_FORWARD:
rotwing_state.desired_state = ROTWING_STATE_FW_HOV_MOT_OFF;
break;

case GUIDANCE_H_MODE_ATTITUDE:
rotwing_state.desired_state = ROTWING_STATE_HOVER;
break;

default:
rotwing_state.desired_state = ROTWING_STATE_FREE;
break;
}
}

// Function that handles settings for switching state(s)
void rotwing_switch_state(void)
{
switch (rotwing_state.current_state) {
case ROTWING_STATE_HOVER:
if (rotwing_state.desired_state > ROTWING_STATE_HOVER) {
// if in hover state, but higher state requested, switch on wing_rotation scheduler
wing_rotation.airspeed_scheduling = true;
} else {
// if hover state desired and at the state, fix wing in quad
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 0;
}
break;

case ROTWING_STATE_SKEWING:
if (rotwing_state.desired_state > ROTWING_STATE_SKEWING) {
// Keep wing rotation scheduler on if skewing to a higher state
wing_rotation.airspeed_scheduling = true;
} else {
// go back to quad state if fulfilling conditions for skewing back (motors on, skewing back not too fast).
}
break;

case ROTWING_STATE_FW:
break;

case ROTWING_STATE_FW_HOV_MOT_IDLE:
if (rotwing_state.desired_state > ROTWING_STATE_FW_HOV_MOT_IDLE) {
// Switch off hover motors if in idle and desired state is higher
bool_disable_hover_motors = true;
} else if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) {
// Allow hover motors to generate thrust when desired state is lower than current state
hover_motors_active = true;
} else {
// if idle desired and already at idle, let motors spin idle
bool_disable_hover_motors = false;
hover_motors_active = false;
}
break;

case ROTWING_STATE_FW_HOV_MOT_OFF:
if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_OFF) {
// Switch on the hover motors when going to a lower state
bool_disable_hover_motors = false;
}
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 90;
break;
}
}

static void rotwing_state_rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED * rpm_message, uint8_t UNUSED num_act_message)
{
for (int i=0; i<num_act_message; i++) {
// Sanity check that index is valid
if (rpm_message[i].actuator_idx < ROTWING_STATE_NUM_HOVER_RPM){
rotwing_state_hover_rpm[rpm_message->actuator_idx] = rpm_message->rpm;
}
}
}
53 changes: 53 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.h
View file
@@ -0,0 +1,53 @@
/*
* 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;
};

extern struct RotwingState rotwing_state;

extern bool rotwing_state_force_quad;

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

#endif // ROTWING_STATE_H
262 changes: 262 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_v3b.c
View file
@@ -0,0 +1,262 @@
/*
* 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_v3b.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_SERVO_IDX
#error "No WING_ROTATION_SERVO_IDX defined"
#endif
*/

#if !USE_NPS

#ifndef ADC_CHANNEL_WING_ROTATION_POSITION
#define ADC_CHANNEL_WING_ROTATION_POSITION ADC_5
#endif

#ifndef ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES
#define ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES 16
#endif

#endif // !USE_NPS

#ifndef WING_ROTATION_POSITION_ADC_0
#error "No WING_ROTATION_POSITION_ADC_0 defined"
#endif

#ifndef WING_ROTATION_POSITION_ADC_90
#error "No WING_ROTATION_POSITION_ADC_90 defined"
#endif

#ifndef WING_ROTATION_FIRST_DYN
#define WING_ROTATION_FIRST_DYN 0.001
#endif

#ifndef WING_ROTATION_SECOND_DYN
#define WING_ROTATION_SECOND_DYN 0.003
#endif

// Parameters
struct wing_rotation_controller wing_rotation;

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_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES);
#endif

// Init wing_rotation_controller struct
wing_rotation.servo_pprz_cmd = 0;
wing_rotation.adc_wing_rotation = 0;
wing_rotation.wing_angle_rad = 0;
wing_rotation.wing_angle_deg = 0;
wing_rotation.wing_angle_rad_sp = 0;
wing_rotation.wing_angle_deg_sp = 0;
wing_rotation.wing_rotation_speed = 0;
wing_rotation.wing_angle_virtual_deg_sp = 45;
wing_rotation.wing_rotation_first_order_dynamics = WING_ROTATION_FIRST_DYN;
wing_rotation.wing_rotation_second_order_dynamics = WING_ROTATION_SECOND_DYN;
wing_rotation.adc_wing_rotation_range = WING_ROTATION_POSITION_ADC_90 - WING_ROTATION_POSITION_ADC_0;
wing_rotation.airspeed_scheduling = false;
wing_rotation.airspeed_scheduling_nav = false;
wing_rotation.force_rotation_angle = false;
wing_rotation.transition_forward = false;
wing_rotation.forward_airspeed = 18.;

// Set wing angle to current wing angle
wing_rotation.initialized = false;
wing_rotation.init_loop_count = 0;
}

void wing_rotation_periodic(void)
{
// your periodic code here.
// freq = 1.0 Hz

// After 5 loops, set current setpoint and enable wing_rotation
if (!wing_rotation.initialized) {
wing_rotation.init_loop_count += 1;
if (wing_rotation.init_loop_count > 4) {
wing_rotation.initialized = true;
wing_rotation.wing_angle_rad_sp = M_PI * 0.25;
wing_rotation.wing_angle_deg_sp = wing_rotation.wing_angle_rad_sp / M_PI * 180.;
}
}
}

void wing_rotation_event(void)
{
// First check if safety switch is triggered
#ifdef WING_ROTATION_RESET_RADIO_CHANNEL
// Update wing_rotation deg setpoint when RESET switch triggered
if (radio_control.values[WING_ROTATION_RESET_RADIO_CHANNEL] > 1750)
{
wing_rotation.airspeed_scheduling = false;
wing_rotation.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.airspeed_scheduling_nav);
}

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

// Update Wing position sensor
wing_rotation_to_rad();

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

if (wing_rotation.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.wing_angle_deg_sp = wing_angle_scheduled_sp_deg;

}

wing_rotation_update_sp();

//int32_t servo_pprz_cmd; // Define pprz cmd

// Control the wing rotation position.
wing_rotation_compute_pprz_cmd();
//servo_pprz_cmd = wing_rotation.wing_angle_deg_sp / 90 * MAX_PPRZ;
//Bound(servo_pprz_cmd, 0, MAX_PPRZ);

//wing_rotation.servo_pprz_cmd = servo_pprz_cmd;
}
}

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

wing_rotation.wing_angle_deg = 0.00247111 * (float)wing_rotation.adc_wing_rotation - 25.635294;
wing_rotation.wing_angle_rad = wing_rotation.wing_angle_deg / 180. * M_PI;

#else
// Copy setpoint as actual angle in simulation
wing_rotation.wing_angle_deg = wing_rotation.wing_angle_virtual_deg_sp;
wing_rotation.wing_angle_rad = wing_rotation.wing_angle_virtual_deg_sp / 180. * M_PI;
#endif
}

void wing_rotation_update_sp(void)
{
wing_rotation.wing_angle_rad_sp = wing_rotation.wing_angle_deg_sp / 180. * M_PI;
}

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

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

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

wing_rotation.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.airspeed_scheduling = true;
} else {
wing_rotation.airspeed_scheduling = false;
if (!wing_rotation.force_rotation_angle) {
wing_rotation.wing_angle_deg_sp = 0;
}
}
return false;
}

bool set_wing_rotation_scheduler_nav(bool rotation_scheduler_on)
{
wing_rotation.airspeed_scheduling_nav = rotation_scheduler_on;
return false;
}
66 changes: 66 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_v3b.h
View file
@@ -0,0 +1,66 @@
/*
* 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.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_V3B_H
#define WING_ROTATION_CONTROLLER_V3B_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 {
int32_t servo_pprz_cmd;
uint16_t adc_wing_rotation;
int16_t adc_wing_rotation_range;
float wing_angle_rad;
float wing_angle_deg;
float wing_angle_rad_sp;
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 float wing_rotation_sched_as_1;
extern float wing_rotation_sched_as_2;
extern float wing_rotation_sched_as_3;
extern float wing_rotation_sched_as_4;

extern struct wing_rotation_controller wing_rotation;

#endif // WING_ROTATION_CONTROLLER_V3B_H