Skip to content

Commit

Permalink
Update _IDX and rebase
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Dec 13, 2023
1 parent 3aaac4c commit ae033fc
Show file tree
Hide file tree
Showing 5 changed files with 44 additions and 41 deletions.
6 changes: 3 additions & 3 deletions conf/airframes/tudelft/rot_wing_v3b.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,9 +385,9 @@
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>

<!-- Actuator dynamics -->
<define name="ACT_DYN" value="{0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.024}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>

<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
Expand Down
67 changes: 35 additions & 32 deletions conf/airframes/tudelft/rot_wing_v3d.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,12 @@
<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"/>
<configure name="USE_TFMINI_AGL" value="TRUE"/>
</module>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>

<!-- Range sensor connected to supercan -->
<module name="range_sensor_uavcan"/>

<!-- <module name="mavlink">
<configure name="MAVLINK_BAUD" value="B115200"/>
Expand All @@ -43,7 +45,6 @@

<!-- Log in high speed (Remove for outdoor flights) -->
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->

</target>

<target name="nps" board="pc">
Expand Down Expand Up @@ -99,7 +100,7 @@
<module name="actuators" type="pwm" >
<define name="SERVO_HZ" value="400"/>
</module>

<!-- Control -->
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="9"/>
Expand Down Expand Up @@ -184,25 +185,20 @@
</module>
</firmware>

<!-- PWM actuators -->
<servos driver="Pwm">
<!--1153 = quad 2104 = fw-->
<servo no="0" name="ROTATION_MECH" min="1153" neutral="1628" max="2104"/>
</servos>

<!-- 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="ROTATION_MECH" min="-2842" neutral="1053" max="4947"/>
</servos>

<!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd">
<servo no="5" name="SERVO_ELEVATOR" min="3250" neutral="3250" max="-3250"/>
<servo no="6" name="SERVO_RUDDER" min="-3250" neutral="0" max="3250"/>
<servo no="6" name="SERVO_ELEVATOR" min="8191" neutral="8191" max="-3250"/>
<servo no="7" name="SERVO_RUDDER" min="-3250" neutral="0" max="3250"/>
</servos>

<!-- Can bus 2 actuators -->
Expand All @@ -211,14 +207,15 @@
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="1000" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="1000" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="1000" max="8191"/>
<servo no="5" name="BROTATION_MECH" min="-2842" neutral="1053" max="4947"/>
</servos>

<!-- CAN BUS 2 command outputs-->
<servos driver="Uavcan2Cmd">
<servo no="7" name="AIL_LEFT" min="-2800" neutral="0" max="2800"/> <!-- min can go up to -9600-->
<servo no="8" name="FLAP_LEFT" min="-2800" neutral="0" max="2800"/> <!-- min can go up to -9600-->
<servo no="9" name="FLAP_RIGHT" min="-2800" neutral="0" max="2800"/> <!-- max can go up to -9600-->
<servo no="10" name="AIL_RIGHT" min="-2800" neutral="0" max="2800"/> <!-- max can go up to -9600-->
<servo no="8" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="9" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="10" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
<servo no="11" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
</servos>

<commands>
Expand All @@ -233,6 +230,8 @@
<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="Or($th_hold, bool_disable_hover_motors)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 20)"/>
<let var="flap_limit_hit" value="LessThan(rotwing_state_skewing.wing_angle_deg, 50)"/>

<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 All @@ -243,16 +242,17 @@
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/>
<set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
<set VALUE="$flap_limit_hit? 0 : ($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_RIGHT"/>

<!-- Backup commands -->
<set VALUE="($hover_off? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/>
<set VALUE="($hover_off? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/>
<set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="BROTATION_MECH"/>
</command_laws>

<section PREFIX="SYS_ID_" NAME="SYS_ID">
Expand Down Expand Up @@ -290,7 +290,7 @@
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-131.497033952591,1.32098226269777,-0.0000291178067445214}"/>

<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
Expand All @@ -299,7 +299,7 @@
<section name="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>

<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="2.0" />
Expand All @@ -325,8 +325,10 @@
</section>

<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-24,-9,18}, .scale={{22586,14285,60640},{2303,1461,6213}}}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-4,-4,49}, .scale={{49201,22321,17833},{10030,4553,3632}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-48,-11,2}, .scale={{37211,48711,18769},{7594,9938,3845}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={-651,103,316}, .scale={{6251,9851,9082},{10162,15845,15697}}, .body_to_sensor={{16384,0,0,0,16384,0,0,0,-16384}}}}"/>
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-24,-9,18}, .scale={{22586,14285,60640},{2303,1461,6213}}, .filter_sample_freq=1139, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-4,-4,49}, .scale={{49201,22321,17833},{10030,4553,3632}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-48,-11,2}, .scale={{37211,48711,18769},{7594,9938,3845}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true, .rotation=true},.neutral={-1518,-1489,512}, .scale={{1124,9185,21218},{1879,15118,36859}}, .body_to_sensor={{16384,0,0,0,16384,0,0,0,-16384}}}}"/>

<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1139, .filter_freq=30}}"/>

<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
Expand Down Expand Up @@ -386,22 +388,23 @@
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>

<!-- Actuator dynamics -->
<define name="ACT_DYN" value="{0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.047}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<define name="ACT_FREQ" value="{20.0, 20.0, 20.0, 20.0, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>

<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_P" value="38.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_P" value="5.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_R" value="3.9"/>

<define name="MAX_R" value="30.0" unit="deg/s"/>

<!-- Filters -->
<define name="FILT_CUTOFF_P" value="20.0"/>
<define name="FILTER_RATES_SECOND_ORDER" value="TRUE"/>
<define name="FILT_CUTOFF_P" value="5.0"/>
<define name="FILT_CUTOFF_Q" value="20.0"/>
<define name="FILT_CUTOFF_R" value="5.0"/>
<define name="FILT_CUTOFF" value="2.0"/>
Expand Down
2 changes: 1 addition & 1 deletion conf/modules/eff_scheduling_rot_wing.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
<file name="eff_scheduling_rot_wing.c"/>
<test>
<define name="ACTUATORS_NB" value="10"/>
<define name="SERVO_ROTATION_MECH" value="8"/>
<define name="SERVO_ROTATION_MECH_IDX" value="8"/>
<define name="SERVO_MOTOR_FRONT" value="0"/>
<define name="SERVO_MOTOR_RIGHT" value="1"/>
<define name="SERVO_MOTOR_BACK" value="2"/>
Expand Down
6 changes: 3 additions & 3 deletions conf/userconf/tudelft/conf.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -577,9 +577,9 @@
airframe="airframes/tudelft/rot_wing_v3b.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHR8.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml modules/wing_rotation_controller_servo.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
/>
<aircraft
Expand Down Expand Up @@ -610,7 +610,7 @@
airframe="airframes/tudelft/rot_wing_v3d.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHR8.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red"
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"

#ifndef SERVO_ROTATION_MECH
#ifndef SERVO_ROTATION_MECH_IDX
#error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH
#endif

Expand Down Expand Up @@ -172,7 +172,7 @@ static abi_event wing_position_ev;
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
{
for (int i=0; i<num_act; i++){
if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH))
if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
{
// Get wing rotation angle from sensor
eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
Expand Down

0 comments on commit ae033fc

Please sign in to comment.