79 changes: 43 additions & 36 deletions conf/airframes/tudelft/rot_wing_v3b.xml
View file
Expand Up @@ -20,16 +20,16 @@
<module name="flight_recorder"/>

<!-- RC switches -->
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/>
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/>
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>

<!-- EKF2 configure inputs -->
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE2_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>

<module name="lidar_tfmini">
<configure name="TFMINI_PORT" value="UART8"/>
Expand All @@ -48,10 +48,11 @@
<module name="fdm" type="jsbsim"/>

<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
<define name="RADIO_CONTROL_THRUST_X" value="0"/>
</target>

<!-- Kongsberg datalink -->
Expand All @@ -61,29 +62,29 @@

<!-- Sensors -->
<module name="mag" type="rm3100">
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
</module>
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
</module>
<module name="airspeed" type="uavcan">
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<module name="gps" type="ublox">
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>

<!-- IMU / INS -->
<module name="imu" type="cube"/>
<module name="ins" type="ekf2" />
<module name="ins" type="ekf2"/>

<!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan">
Expand All @@ -97,11 +98,11 @@
</module>

<!-- Control -->
<module TYPE="indi" NAME="stabilization">
<configure name="INDI_NUM_ACT" value="9"/>
<configure name="INDI_OUTPUTS" value="5"/>
<define name="WLS_N_U" value="9"/>
<define name="WLS_N_V" value="5"/>
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="9"/>
<configure name="INDI_OUTPUTS" value="5"/>
<define name="WLS_N_U" value="9"/>
<define name="WLS_N_V" value="5"/>
</module>

<module name="ctrl_eff_sched_rot_wing"/>
Expand All @@ -115,10 +116,10 @@
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="wing_rotation_controller_servo"/>
<!--module name="rot_wing_automation"/>
<module name="rot_wing_automation"/>
<module name="ground_detect_sensor"/>
<module name="rotwing_state"/>
<module name="preflight_checks"/-->
<module name="preflight_checks"/>
<module name="agl_dist"/>
<module name="approach_moving_target"/>

Expand Down Expand Up @@ -189,13 +190,13 @@

<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="5" name="SERVO_ELEVATOR" min="5000" neutral="5000" max="-5500"/>
<servo no="6" name="SERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
<servo no="0" name="MOTOR_FRONT" min="0" neutral="1000" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="0" max="8191"/>
<servo no="5" name="SERVO_ELEVATOR" min="5000" neutral="5000" max="-5500"/>
<servo no="6" name="SERVO_RUDDER" min="-8191" neutral="0" max="8191"/>
</servos>

<!-- Can bus 2 actuators -->
Expand All @@ -222,11 +223,10 @@
<axis name="THRUST_X" failsafe_value="0"/>
</commands>


<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<let VAR="hover_off" VALUE="$th_hold"/>
<let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/>

<call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/>
Expand Down Expand Up @@ -411,6 +411,13 @@
<define name="ROTWING_V3B" value="1" />
</section>

<section name="WING_ROTATION" prefix="WING_ROTATION_CONTROLLER_">
<define name="POSITION_ADC_0" value="62930"/>
<define name="POSITION_ADC_90" value="41760"/>
<define name="DEADZONE_PPRZ_CMD" value="500"/>
<define name="P_GAIN" value="-50000"/>
<define name="MAX_CMD" value="9600"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<!-- Gains -->
Expand Down
25 changes: 25 additions & 0 deletions conf/modules/rot_wing_automation.xml
View file
@@ -0,0 +1,25 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="rot_wing_automation" dir="rot_wing_drone">
<doc>
<description>Fucntions to automate the navigation and guidance of the rotating wing drone</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="transition">
<dl_setting var="rot_wing_a.trans_accel" min="0.1" max="5.0" step="0.1" shortname="trans_accel"/>
<dl_setting var="rot_wing_a.trans_decel" min="0.1" max="5.0" step="0.1" shortname="trans_decel"/>
<dl_setting var="rot_wing_a.trans_length" min="10" max="500." step="1.0" shortname="trans_distance"/>
<dl_setting var="rot_wing_a.trans_airspeed" min="10.0" max="20.0" step="0.1" shortname="trans_airspeed"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rot_wing_automation.h"/>
</header>
<init fun="init_rot_wing_automation()"/>
<periodic fun="periodic_rot_wing_automation()" freq="10."/>
<makefile>
<file name="rot_wing_automation.c"/>
<test/>
</makefile>
</module>
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);
}
50 changes: 50 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.h
View file
@@ -0,0 +1,50 @@
/*
* 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"
#include "modules/rot_wing_drone/rotwing_state.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;

#define RotWingAutomationReadyForForward() (rot_wing_a.transitioned && (rotwing_state_skewing.wing_angle_deg > 75.0))


#endif // ROT_WING_AUTOMATION_H