Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-06 11:12:43 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[actuators] Add UAVCAN actuators for chibios (#2511)

Browse Source 
* [actuators] Add UAVCAN actuators for chibios

* [conf] Fixes for Nederdrone

* [conf] Cleanup nederdrone

* [boards] Fix FMUv5 i2c DMA

* [conf] Fixes for Nederdrone4

* [actuators] Add uavcan simulator

* [uavcan] Rewrite to seperate module for actuators and sensors
This commit is contained in:
Freek van Tienen
2021-04-20 14:10:25 +02:00
committed by GitHub
parent fdd82745cb
commit 627ecce31b
21 changed files with 2228 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@@ -50,3 +50,6 @@
[submodule "sw/ext/matrix"]
path = sw/ext/matrix
url = https://github.com/PX4/Matrix.git
[submodule "sw/ext/libcanard"]
path = sw/ext/libcanard
url = https://github.com/UAVCAN/libcanard.git

457
conf/airframes/tudelft/neddrone4.xml Normal file
View File

@@ -0,0 +1,457 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- This is a Nedderdrone
* Autopilot: Pixhawk 4
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="Neddrone4">
<description>Neddrone4</description>
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_5.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/>
</module>
<module name="airspeed_ets.xml">
<define name="USE_I2C4"/>
<configure name="AIRSPEED_ETS_I2C_DEV" value="I2C4"/>
<define name="AIRSPEED_ETS_SYNC_SEND"/>
<define name="USE_AIRSPEED_ETS" value="TRUE"/>
<define name="AIRSPEED_ETS_SCALE" value="1.24"/>
</module>
<!-- TODO: add this module -->
<!--module name="scheduling_indi_simple"/-->
<!-- Forward FuelCell data back to the GCS -->
<!--module name="generic_uart_sensor"/-->
<!-- Logger -->
<module name="tlsf"/>
<module name="pprzlog"/>
<module name="logger" type="sd_chibios"/>
<module name="flight_recorder"/>
<define name="ADC_CURRENT_DISABLE" value="TRUE"/>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_FLAPS" value="RADIO_AUX4"/> <!-- Flaps control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_BACK_THOLD" value="RADIO_AUX5"/>
</target>
<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<!--module name="scheduling_indi_simple"/-->
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--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_FLAPS" value="0"/> <!-- Flaps control -->
<define name="RADIO_KILL_SWITCH" value="6"/>
<define name="RADIO_BACK_THOLD" value="0"/>
</target>
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
</module>
<module name="imu" type="mpu6000"/>
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<!--<module name="stabilization" type="int_quat"/>-->
<!--<module name="gain_scheduling"/>-->
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ins" type="ekf2" />
<!--module name="airspeed" type="ms45xx_i2c">
<define name="USE_I2C4"/>
<define name="MS45XX_I2C_DEV" value="i2c4"/>
</module-->
<module name="air_data"/>
<module name="send_imu_mag_current"/>
<!-- External GPS -->
<module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<!--define name="LIS3MDL_MAG_TO_IMU_PHI" value="0.0"/>
<define name="LIS3MDL_MAG_TO_IMU_THETA" value="0.0"/>
<define name="LIS3MDL_MAG_TO_IMU_PSI" value="180.0"/-->
</module>
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->
<!--<module name="guidance" type="hybrid"/>-->
<module name="guidance" type="indi_hybrid">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.0"/>
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/>
<define name="GUIDANCE_INDI_LIFT_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/>
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="2500"/>
</module>
<module name="motor_mixing"/>
<!--module name="motor_vs_flap_slider"/-->
</firmware>
<!-- CAN BUS 1 (Front Wing) -->
<servos driver="Uavcan1">
<servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_3" no="2" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_4" no="3" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_5" no="4" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
<servo name="AIL_1" no="6" min="6000" neutral="0" max="-6000"/>
<servo name="FLAP_1" no="7" min="6000" neutral="0" max="-6000"/>
<servo name="FLAP_2" no="8" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_2" no="9" min="-6000" neutral="0" max="6000"/>
</servos>
<!-- CAN BUS 2 (Back Wing) -->
<servos driver="Uavcan2">
<servo name="MOTOR_7" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_8" no="1" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_9" no="2" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
<servo name="FLAP_3" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_3" no="7" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_4" no="8" min="6000" neutral="0" max="-6000"/>
<servo name="FLAP_4" no="9" min="6000" neutral="0" max="-6000"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
<axis name="FLAPS" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@YAW" />
<set command="PITCH" value="@PITCH/2" />
<set command="YAW" value="-@ROLL/4" />
<set command="FLAPS" value="@FLAPS" />
</rc_commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- frontleft left (CW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CCW) -->
<!-- backright right (CCW), brackright mid (CW), brackright left (CCW), backleft right (CW), backleft mid (CCW), backleft left (CW) -->
<define name="NB_MOTOR" value="12"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{ 256, 164, 70, -70, -164, -256, -256, -164, -70, 70, 164, 256}"/>
<define name="PITCH_COEF" value="{ 256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{ 238, -248, 242, -242, 248, -238, 250, -240, 246, -246, 240, -250}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{ 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set servo="MOTOR_1" value="($th_hold? -9600 : motor_mixing.commands[0])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : motor_mixing.commands[1])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : motor_mixing.commands[2])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : motor_mixing.commands[3])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : motor_mixing.commands[4])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : motor_mixing.commands[5])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : motor_mixing.commands[6])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : motor_mixing.commands[7])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : motor_mixing.commands[8])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : motor_mixing.commands[9])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : motor_mixing.commands[10])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : motor_mixing.commands[11])"/>
<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="-2*@PITCH + ( 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="AIL_2" value="-2*@PITCH + (- 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="AIL_3" value=" 2*@PITCH + (- 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="AIL_4" value=" 2*@PITCH + ( 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="FLAP_1" value="-2*@PITCH + ( 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="FLAP_2" value="-2*@PITCH + (- 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="FLAP_3" value=" 2*@PITCH + (- 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
<set servo="FLAP_4" value=" 2*@PITCH + ( 2*@YAW) + RadioControlValues(RADIO_FLAPS)"/>
</command_laws>
<section name="MISC">
<define name="WINDTUNNEL_TO_BODY_THETA" value="-90." unit="deg"/>
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.5"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.9024749557 * adc)"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
</section>
<section name="FORWARD">
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
<!-- For RC coordinated turns, lower because the yawing was too slow -->
<define name="MAX_FWD_SPEED" value="20.0"/>
<!-- For hybrid guidance -->
<define name="MAX_AIRSPEED" value="20.0"/>
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Rotate the IMU (for Pixhawk 4) -->
<define name="MPU_CHAN_X" value="1"/>
<define name="MPU_CHAN_Y" value="0"/>
<define name="MPU_CHAN_Z" value="2"/>
<define name="MPU_X_SIGN" value="1"/>
<define name="MPU_Y_SIGN" value="1"/>
<define name="MPU_Z_SIGN" value="-1"/>
<!-- Calibrated in the MAVLab 14-05-2020 -->
<define name="ACCEL_X_NEUTRAL" value="-337"/>
<define name="ACCEL_Y_NEUTRAL" value="64"/>
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
<define name="ACCEL_X_SENS" value="4.670307671109528" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.9016250738902425" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.846689188075245" integer="16"/>
<!--define name= "MAG_X_CURRENT_COEF" value="-1.7139708082316483"/>
<define name= "MAG_Y_CURRENT_COEF" value="-0.7696784114750511"/>
<define name= "MAG_Z_CURRENT_COEF" value="1.1626106815908253"/-->
<!-- Calibrated at valkenburg 20-05-2020 -->
<define name="MAG_X_NEUTRAL" value="866"/>
<define name="MAG_Y_NEUTRAL" value="-1530"/>
<define name="MAG_Z_NEUTRAL" value="-3313"/>
<define name="MAG_X_SENS" value="0.6067461130451115" integer="16"/>
<define name="MAG_Y_SENS" value="0.6544292255627779" integer="16"/>
<define name="MAG_Z_SENS" value="0.6352539557433349" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
<define name="USE_GPS_HEADING" value="0"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="140" unit="deg/s"/>
<define name="SP_MAX_Q" value="140" unit="deg/s"/>
<define name="SP_MAX_R" value="140" unit="deg/s"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="80." unit="deg"/>
<define name="SP_MAX_R" value="90." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="400" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="100." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(3000.)"/>
<define name="REF_OMEGA_Q" value="400" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
<define name="REF_OMEGA_R" value="300" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.85"/>
<define name="REF_MAX_R" value="180." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="800"/>
<define name="PHI_DGAIN" value="350"/>
<define name="PHI_IGAIN" value="40"/>
<define name="THETA_PGAIN" value="800"/>
<define name="THETA_DGAIN" value="350"/>
<define name="THETA_IGAIN" value="40"/>
<define name="PSI_PGAIN" value="1250"/>
<define name="PSI_DGAIN" value="320"/>
<define name="PSI_IGAIN" value="10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="0"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.0030"/>
<define name="G1_Q" value="0.0035"/>
<define name="G1_R" value="0.0004"/>
<define name="G2_R" value="0.00015"/>
<define name="FORWARD_G1_P" value="0.0020"/>
<define name="FORWARD_G1_Q" value="0.0077"/>
<define name="FORWARD_G1_R" value="0.004"/>
<!--P effectiveness if center props are switched off-->
<define name="FORWARD_G1_P_FEW_PROPS" value="0.0020"/>
<!--Part of yaw eff that comes from motors-->
<define name="MOT_YAW_EFF" value="0.00"/>
<define name="MOT_PITCH_EFF" value="0.003"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="30.0"/>
<define name="REF_ERR_R" value="20.0"/>
<define name="REF_RATE_P" value="6.0"/>
<define name="REF_RATE_Q" value="6.0"/>
<define name="REF_RATE_R" value="6.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="50.0" unit="deg/s"/>
<!-- Maximum rate setpoint in rate control mode -->
<define name="MAX_RATE" value="3.0" unit="rad/s"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_R" value="0.5"/>
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
<define name="FILTER_YAW_RATE" value="TRUE"/>
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.0354"/>
<define name="ACT_DYN_Q" value="0.0354"/>
<define name="ACT_DYN_R" value="0.0354"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GAIN_SETS">
<define name="NUMBER_OF_GAINSETS" value="2"/>
<define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
<define name="SCHEDULING_POINTS" value="{4, 10}"/>
<define name="SCHEDULING_VARIABLE_FRAC" value="8"/>
<define name="PHI_P" value="{800, 600}"/>
<define name="PHI_D" value="{350, 250}"/>
<define name="PHI_I" value="{40, 20}"/>
<define name="PHI_DD" value="{0, 0}"/>
<define name="THETA_P" value="{800, 400}"/>
<define name="THETA_D" value="{350, 150}"/>
<define name="THETA_I" value="{40, 100}"/>
<define name="THETA_DD" value="{0, 0}"/>
<define name="PSI_P" value="{1250, 1250}"/>
<define name="PSI_D" value="{320, 320}"/>
<define name="PSI_I" value="{10, 10}"/>
<define name="PSI_DD" value="{0, 0}"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="30" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
</section>
</airframe>

19
conf/modules/actuators_uavcan.xml Normal file
View File

@@ -0,0 +1,19 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="actuators_uavcan" dir="actuators" task="actuators">
<doc>
<description>
UAVCan actuators
</description>
</doc>
<dep>
<depends>uavcan</depends>
</dep>
<header>
<file name="actuators_uavcan.h" dir="subsystems/actuators"/>
</header>
<makefile target="ap|nps">
<file name="actuators_uavcan.c" dir="subsystems/actuators"/>
</makefile>
</module>

32
conf/modules/uavcan.xml Normal file
View File

@@ -0,0 +1,32 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="uavcan" dir="uavcan">
<doc>
<description>
UAVCan interface for transmitting/receiving uavcan messages on CAN busses
</description>
<configure name="UAVCAN_USE_CAN1" value="FALSE" description="Enable UAVCAN on CAN1 interface"/>
<configure name="UAVCAN_USE_CAN2" value="FALSE" description="Enable UAVCAN on CAN2 interface"/>
<define name="UAVCAN_NODE_ID" value="100" description="The UAVCAN node ID of the AP"/>
<define name="UAVCAN_BAUDRATE" value="1000000" description="The baudrate of the CAN interface"/>
</doc>
<header>
<file name="uavcan.h" dir="modules/uavcan"/>
</header>
<init fun="uavcan_init()"/>
<makefile target="ap">
<!-- Enable the CAN busses if needed -->
<define name="USE_CAN1" cond="ifeq ($(UAVCAN_USE_CAN1), TRUE)"/>
<define name="USE_CAN2" cond="ifeq ($(UAVCAN_USE_CAN2), TRUE)"/>
<define name="UAVCAN_USE_CAN1" value="$(UAVCAN_USE_CAN1)"/>
<define name="UAVCAN_USE_CAN2" value="$(UAVCAN_USE_CAN2)"/>
<!-- Load canard -->
<include name="$(PAPARAZZI_SRC)/sw/ext/libcanard"/>
<file name="canard.c" dir="$(PAPARAZZI_SRC)/sw/ext/libcanard"/>
<!-- Load uavcan itself -->
<file_arch name="uavcan.c" dir="modules/uavcan"/>
</makefile>
</module>

80
conf/simulator/jsbsim/aircraft/Systems/aerodynamics_nederdrone.xml Normal file
View File

@@ -0,0 +1,80 @@
<aerodynamics>
<axis name="LIFT">
<function name="aero/force/Lift_alpha">
<description>Lift due to alpha</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-deg</independentVar>
<tableData>
-180 0.14
-140 -0.80
-110 -0.63
-102 -0.74
-98 -0.64
-90 0.07
-79 1.15
-76 1.2
-72 1.1
-45 0.8
0 0.0
45 0.8
90 0.0
135 -0.8
180 0.0
</tableData>
</table>
</product>
</function>
</axis>
<axis name="DRAG">
<function name="aero/force/Drag_basic">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<table>
<independentVar lookup="row">aero/alpha-deg</independentVar>
<tableData>
-180 1.7
-160 1.62
-100 0.4
-95 0.27
-92 0.24
-88 0.225
-83 0.235
-78 0.29
-65 0.6128
-20 1.36
-4 1.45
15 1.35
30 1.12
</tableData>
</table>
<value>1.2</value>
</product>
</function>
</axis>
<axis name="SIDE">
<function name="aero/force/Side_beta">
<description>Side force due to beta</description>
<product>
<property>aero/qbar-psf</property>
<property>metrics/Sw-sqft</property>
<property>aero/beta-rad</property>
<value>-4</value>
</product>
</function>
</axis>
</aerodynamics>

606
conf/simulator/jsbsim/aircraft/nederdrone.xml Normal file
View File

File diff suppressed because it is too large Load Diff

230
conf/telemetry/highspeed_rotorcraft.xml Normal file
View File

@@ -0,0 +1,230 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="0.7"/>
<message name="ROTORCRAFT_STATUS" period="0.4"/>
<message name="ROTORCRAFT_FP" period="0.20"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".2"/>
<message name="INS" period=".2"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="0.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.6"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
<message name="VISUALTARGET" period="0.10"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="DIVERGENCE" period="0.05"/>
<message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="INS_EKF2_EXT" period="4.25"/>
<message name="FBW_STATUS" period="2.1"/>
<message name="WIND_INFO_RET" period="9.2"/>
<message name="AHRS_BIAS" period="7.5"/>
<message name="HOVER_LOOP" period="0.3"/>
<message name="GUIDANCE_H_REF_INT" period="0.31"/>
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/>
<!--message name="WINDTUNNEL_MEAS" period="0.1"/-->
<message name="AHRS_REF_QUAT" period="0.1"/>
</mode>
<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_INDI" period=".25"/>
<message name="INDI_G" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>
</process>
<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="GPS" period="11.1"/>
<message name="DL_VALUE" period="2.5"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.02"/>
<message name="ROTORCRAFT_CMD" period=".002"/>
<message name="COMMANDS" period=".02"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".002"/>
<message name="INS_Z" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="0.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="0.02"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".002"/>
<message name="IMU_ACCEL_SCALED" period=".002"/>
<message name="IMU_MAG_SCALED" period=".0125"/>
<message name="LIDAR" period="0.05"/>
<message name="INS_EKF2" period="0.05"/>
<message name="INS_EKF2_EXT" period="0.1"/>
<message name="WIND_INFO_RET" period="0.2"/>
<message name="AHRS_BIAS" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.01"/>
<message name="GUIDANCE_H_REF_INT" period="0.02"/>
<message name="ESC" period="0.02"/>
<message name="STAB_ATTITUDE_INDI" period="0.002"/>
<message name="PPM" period="0.05"/>
<message name="INDI_G" period="0.1"/>
<!--<message name="WINDTUNNEL_MEAS" period="0.005"/>-->
</mode>
</process>
</telemetry>

11
conf/userconf/tudelft/conf.xml
View File

@@ -285,6 +285,17 @@
gui_color="white"
release="997fa535902c4d8f73bc34c02c862652cd47cae5"
/>
<aircraft
name="Neddrone4"
ac_id="14"
airframe="airframes/tudelft/neddrone4.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/guidance_indi_hybrid.xml modules/air_data.xml modules/ins_ekf2.xml modules/stabilization_indi_simple.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="blue"
/>
<aircraft
name="OrigamiMXS_wifi_indi_stereocam"
ac_id="15"

290
sw/airborne/arch/chibios/modules/uavcan/uavcan.c Normal file
View File

@@ -0,0 +1,290 @@
/*
* Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/chibios/modules/uavcan/uavcan.c
* Interface from actuators to ChibiOS CAN driver using UAVCan
*
*/
#include "uavcan.h"
#ifndef UAVCAN_NODE_ID
#define UAVCAN_NODE_ID 100
#endif
#ifndef UAVCAN_BAUDRATE
#define UAVCAN_BAUDRATE 1000000
#endif
static uavcan_event *uavcan_event_hd = NULL;
#if UAVCAN_USE_CAN1
#ifndef UAVCAN_CAN1_NODE_ID
#define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID
#endif
#ifndef UAVCAN_CAN1_BAUDRATE
#define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
#endif
static THD_WORKING_AREA(uavcan1_rx_wa, 1024*2);
static THD_WORKING_AREA(uavcan1_tx_wa, 1024*2);
struct uavcan_iface_t uavcan1 = {
.can_driver = &CAND1,
.can_cfg = {
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1/18)/UAVCAN_CAN1_BAUDRATE - 1)
},
.thread_rx_wa = uavcan1_rx_wa,
.thread_rx_wa_size = sizeof(uavcan1_rx_wa),
.thread_tx_wa = uavcan1_tx_wa,
.thread_tx_wa_size = sizeof(uavcan1_tx_wa),
.node_id = UAVCAN_CAN1_NODE_ID,
.transfer_id = 0,
.initialized = false
};
#endif
#if UAVCAN_USE_CAN2
#ifndef UAVCAN_CAN2_NODE_ID
#define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
#endif
#ifndef UAVCAN_CAN2_BAUDRATE
#define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
#endif
static THD_WORKING_AREA(uavcan2_rx_wa, 1024*2);
static THD_WORKING_AREA(uavcan2_tx_wa, 1024*2);
struct uavcan_iface_t uavcan2 = {
.can_driver = &CAND2,
.can_cfg = {
CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1/18)/UAVCAN_CAN2_BAUDRATE - 1)
},
.thread_rx_wa = uavcan2_rx_wa,
.thread_rx_wa_size = sizeof(uavcan2_rx_wa),
.thread_tx_wa = uavcan2_tx_wa,
.thread_tx_wa_size = sizeof(uavcan2_tx_wa),
.node_id = UAVCAN_CAN2_NODE_ID,
.transfer_id = 0,
.initialized = false
};
#endif
/*
* Receiver thread.
*/
static THD_FUNCTION(uavcan_rx, p) {
event_listener_t el;
CANRxFrame rx_msg;
CanardCANFrame rx_frame;
struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
chRegSetThreadName("uavcan_rx");
chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0));
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
chMtxLock(&iface->mutex);
// Wait until a CAN message is received
while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) {
// Process message.
const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX());
memcpy(rx_frame.data, rx_msg.data8, 8);
rx_frame.data_len = rx_msg.DLC;
if(rx_msg.IDE) {
rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID;
} else {
rx_frame.id = rx_msg.SID;
}
// Let canard handle the frame
canardHandleRxFrame(&iface->canard, &rx_frame, timestamp);
}
chMtxUnlock(&iface->mutex);
}
chEvtUnregister(&iface->can_driver->rxfull_event, &el);
}
/*
* Transmitter thread.
*/
static THD_FUNCTION(uavcan_tx, p) {
event_listener_t txc, txe, txr;
struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
uint8_t err_cnt = 0;
chRegSetThreadName("uavcan_tx");
chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0));
chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1));
chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2));
while (true) {
eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100));
// Succesfull transmit
if (evts == 0)
continue;
// Transmit error
if(evts & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&txe);
continue;
}
chMtxLock(&iface->mutex);
for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
CANTxFrame tx_msg;
tx_msg.DLC = txf->data_len;
memcpy(tx_msg.data8, txf->data, 8);
tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
tx_msg.IDE = CAN_IDE_EXT;
tx_msg.RTR = CAN_RTR_DATA;
if (canTransmit(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg, TIME_IMMEDIATE) == MSG_OK) {
err_cnt = 0;
canardPopTxQueue(&iface->canard);
} else {
// After 5 retries giveup
if(err_cnt >= 5) {
err_cnt = 0;
canardPopTxQueue(&iface->canard);
continue;
}
// Timeout - just exit and try again later
chMtxUnlock(&iface->mutex);
err_cnt++;
canardPopTxQueue(&iface->canard); //FIXME (This needs to be here, don't know why)
chThdSleepMilliseconds(err_cnt * 5);
chMtxLock(&iface->mutex);
continue;
}
}
chMtxUnlock(&iface->mutex);
}
}
/**
* Whenever a valid and 'accepted' transfer is received
*/
static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) {
struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
// Go through all registered callbacks and call function callback if found
for(uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
if(transfer->data_type_id == ev->data_type_id)
ev->cb(iface, transfer);
}
}
/**
* If we should accept this transfer
*/
static bool shouldAcceptTransfer(const CanardInstance* ins __attribute__((unused)),
uint64_t* out_data_type_signature,
uint16_t data_type_id,
CanardTransferType transfer_type __attribute__((unused)),
uint8_t source_node_id __attribute__((unused))) {
// Go through all registered callbacks and return signature if found
for(uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
if(data_type_id == ev->data_type_id) {
*out_data_type_signature = ev->data_type_signature;
return true;
}
}
// No callback found return
return false;
}
/**
* Initialize uavcan interface
*/
static void uavcanInitIface(struct uavcan_iface_t *iface) {
// Initialize mutexes/events for multithread locking
chMtxObjectInit(&iface->mutex);
chEvtObjectInit(&iface->tx_request);
// Initialize canard
canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
onTransferReceived, shouldAcceptTransfer, iface);
// Update the uavcan node ID
canardSetLocalNodeID(&iface->canard, iface->node_id);
// Start the can interface
canStart(iface->can_driver, &iface->can_cfg);
// Start the receiver and transmitter thread
chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void*)iface);
chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void*)iface);
iface->initialized = true;
}
/**
* Initialize all uavcan interfaces
*/
void uavcan_init(void)
{
#if UAVCAN_USE_CAN1
uavcanInitIface(&uavcan1);
#endif
#if UAVCAN_USE_CAN2
uavcanInitIface(&uavcan2);
#endif
}
/**
* Bind to a receiving message from uavcan
*/
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
{
// Configure the event
ev->data_type_id = data_type_id,
ev->data_type_signature = data_type_signature,
ev->cb = cb,
ev->next = uavcan_event_hd;
// Switch the head
uavcan_event_hd = ev;
}
/**
* Broadcast an uavcan message to a specific interface
*/
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void* payload,
uint16_t payload_len) {
if(!iface->initialized) return;
chMtxLock(&iface->mutex);
canardBroadcast(&iface->canard,
data_type_signature,
data_type_id, &iface->transfer_id,
priority, payload, payload_len);
chMtxUnlock(&iface->mutex);
chEvtBroadcast(&iface->tx_request);
}

81
sw/airborne/arch/chibios/modules/uavcan/uavcan.h Normal file
View File

@@ -0,0 +1,81 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/chibios/modules/uavcan/uavcan.h
* Interface with uavcan using the Chibios can interfaces.
* This uses multithreading and starts a transmit and receive thread per interface.
*/
#ifndef MODULES_UAVCAN_ARCH_H
#define MODULES_UAVCAN_ARCH_H
#include <hal.h>
#include <canard.h>
#include <string.h>
/** uavcan interface structure */
struct uavcan_iface_t {
CANDriver *can_driver;
CANConfig can_cfg;
event_source_t tx_request;
mutex_t mutex;
void *thread_rx_wa;
void *thread_tx_wa;
void *thread_uavcan_wa;
size_t thread_rx_wa_size;
size_t thread_tx_wa_size;
size_t thread_uavcan_wa_size;
uint8_t node_id;
CanardInstance canard;
uint8_t canard_memory_pool[1024*2];
uint8_t transfer_id;
bool initialized;
};
/** Generic uavcan callback definition */
typedef void (*uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer* transfer);
/** Main uavcan event structure for registering/calling callbacks */
struct uavcan_event_t {
uint16_t data_type_id;
uint64_t data_type_signature;
uavcan_callback cb;
struct uavcan_event_t *next;
};
typedef struct uavcan_event_t uavcan_event;
/** uavcan interfaces */
#if UAVCAN_USE_CAN1
extern struct uavcan_iface_t uavcan1;
#endif
#if UAVCAN_USE_CAN2
extern struct uavcan_iface_t uavcan2;
#endif
/** uavcan external functions */
void uavcan_init(void);
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb);
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority,
const void* payload, uint16_t payload_len);
#endif /* MODULES_UAVCAN_ARCH_H */

41
sw/airborne/arch/sim/modules/uavcan/uavcan.c Normal file
View File

@@ -0,0 +1,41 @@
/*
* Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/chibios/subsystems/actuators/actuators_uavcan_arch.c
* Interface from actuators to ChibiOS CAN driver using UAVCan
*
*/
#include "uavcan.h"
/** uavcan interfaces */
#if UAVCAN_USE_CAN1
struct uavcan_iface_t uavcan1;
#endif
#if UAVCAN_USE_CAN2
struct uavcan_iface_t uavcan2;
#endif
/** uavcan external functions */
void uavcan_init(void) {}
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb) {}
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority,
const void* payload, uint16_t payload_len) {}

57
sw/airborne/arch/sim/modules/uavcan/uavcan.h Normal file
View File

@@ -0,0 +1,57 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file arch/sim/modules/uavcan/uavcan.h
* Stub interface for simulation
*/
#ifndef MODULES_UAVCAN_ARCH_H
#define MODULES_UAVCAN_ARCH_H
#include <std.h>
#include <string.h>
/** uavcan interface structure */
struct uavcan_iface_t {};
/** Generic uavcan callback definition */
struct CanardRxTransfer_t {};
typedef struct CanardRxTransfer_t CanardRxTransfer;
typedef void (*uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer* transfer);
/** Main uavcan event structure for registering/calling callbacks */
struct uavcan_event_t {};
typedef struct uavcan_event_t uavcan_event;
/** uavcan interfaces */
#if UAVCAN_USE_CAN1
extern struct uavcan_iface_t uavcan1;
#endif
#if UAVCAN_USE_CAN2
extern struct uavcan_iface_t uavcan2;
#endif
/** uavcan external functions */
void uavcan_init(void);
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb);
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority,
const void* payload, uint16_t payload_len);
#endif /* MODULES_UAVCAN_ARCH_H */

31
sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.c Normal file
View File

@@ -0,0 +1,31 @@
/*
* Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file arch/sim/subsystems/actuators/actuators_uavcan_arch.c
* dummy servos handling for sim
*/
#include "subsystems/actuators/actuators_uavcan_arch.h"
void actuators_uavcan_arch_init(void)
{
}

36
sw/airborne/arch/sim/subsystems/actuators/actuators_uavcan_arch.h Normal file
View File

@@ -0,0 +1,36 @@
/*
* Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file arch/sim/subsystems/actuators/actuators_uavcan_arch.h
* dummy servos handling for sim
*/
#ifndef ACTUATORS_UAVCAN_ARCH_H
#define ACTUATORS_UAVCAN_ARCH_H
#define SERVOS_TICS_OF_USEC(_v) (_v)
#define ActuatorUavcanSet(_i, _v) {}
#define ActuatorsUavcanCommit() {}
extern void actuators_uavcan_arch_init(void);
#endif /* ACTUATORS_UAVCAN_ARCH_H */

2
sw/airborne/boards/px4fmu/chibios/v5.0/mcuconf.h
View File

@@ -238,7 +238,7 @@
#endif
#define STM32_I2C_BUSY_TIMEOUT 50
#define STM32_I2C_I2C1_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 0)
#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 6)
#define STM32_I2C_I2C1_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_I2C_I2C2_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)
#define STM32_I2C_I2C2_TX_DMA_STREAM STM32_DMA_STREAM_ID(1, 7)
#define STM32_I2C_I2C3_RX_DMA_STREAM STM32_DMA_STREAM_ID(1, 2)

7
sw/airborne/math/pprz_algebra_int.h
View File

@@ -612,6 +612,13 @@ static inline void int32_vect_zero(int32_t *a, const int n)
for (i = 0; i < n; i++) { a[i] = 0.; }
}
/** a = 0 */
static inline void int16_vect_zero(int16_t *a, const int n)
{
int i;
for (i = 0; i < n; i++) { a[i] = 0.; }
}
/** a = v * ones(n,1) */
static inline void int32_vect_set_value(int32_t *a, const int32_t v, const int n)
{

145
sw/airborne/subsystems/actuators/actuators_uavcan.c Normal file
View File

@@ -0,0 +1,145 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/actuators/actuators_uavcan.c
* UAVCan actuators using RAWCOMMAND message and ESC_STATUS telemetry
*
*/
#include "actuators_uavcan.h"
#include "subsystems/electrical.h"
/* uavcan ESC status telemetry structure */
struct actuators_uavcan_telem_t {
float voltage;
float current;
float temperature;
int32_t rpm;
uint32_t energy;
};
#ifdef SERVOS_UAVCAN1_NB
int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB];
static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB];
#endif
#ifdef SERVOS_UAVCAN2_NB
int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB];
#endif
/* uavcan EQUIPMENT_ESC_STATUS message definition */
#define UAVCAN_EQUIPMENT_ESC_STATUS_ID 1034
#define UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE (0xA9AF28AEA2FBB254ULL)
#define UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE ((110 + 7)/8)
/* uavcan EQUIPMENT_ESC_RAWCOMMAND message definition */
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID 1030
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE (0x217F5C87D7EC951DULL)
#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE ((285 + 7)/8)
static bool actuators_uavcan_initialized = false;
static uavcan_event esc_status_ev;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void actuators_uavcan_send_esc(struct transport_tx *trans, struct link_device *dev)
{
/*static uint8_t esc_idx = 0;
float power = uavcan_telem[esc_idx].current * uavcan_telem[esc_idx].voltage;
float rpm = uavcan_telem[esc_idx].rpm;
float energy = uavcan_telem[esc_idx].energy;
pprz_msg_send_ESC(trans, dev, AC_ID, &uavcan_telem[esc_idx].current, &electrical.vsupply, &power,
&rpm, &uavcan_telem[esc_idx].voltage, &energy, &esc_idx);
esc_idx++;
if(esc_idx >= ACTUATORS_UAVCAN_NB)
esc_idx = 0;*/
}
#endif
/**
* Whevener an ESC_STATUS message from the EQUIPMENT group is received
*/
static void actuators_uavcan_esc_status_cb(struct uavcan_iface_t *iface, CanardRxTransfer* transfer) {
/*uint8_t esc_idx;
uint16_t tmp_float;
canardDecodeScalar(transfer, 105, 5, false, (void*)&esc_idx);
if(iface == &can2_iface)
esc_idx += 10;
if(esc_idx >= ACTUATORS_UAVCAN_NB)
break;
canardDecodeScalar(transfer, 0, 32, false, (void*)&uavcan_telem[esc_idx].energy);
canardDecodeScalar(transfer, 32, 16, true, (void*)&tmp_float);
uavcan_telem[esc_idx].voltage = canardConvertFloat16ToNativeFloat(tmp_float);
canardDecodeScalar(transfer, 48, 16, true, (void*)&tmp_float);
uavcan_telem[esc_idx].current = canardConvertFloat16ToNativeFloat(tmp_float);
canardDecodeScalar(transfer, 64, 16, true, (void*)&tmp_float);
uavcan_telem[esc_idx].temperature = canardConvertFloat16ToNativeFloat(tmp_float);
canardDecodeScalar(transfer, 80, 18, true, (void*)&uavcan_telem[esc_idx].rpm);
// Update total current
electrical.current = 0;
for(uint8_t i = 0; i < ACTUATORS_UAVCAN_NB; ++i)
electrical.current += uavcan_telem[i].current;*/
}
/**
* Initialize an uavcan interface
*/
void actuators_uavcan_init(struct uavcan_iface_t* iface __attribute__((unused)))
{
// Check if not already initialized (for multiple interfaces, needs only 1)
if(actuators_uavcan_initialized) return;
// Bind uavcan ESC_STATUS message from EQUIPMENT
uavcan_bind(UAVCAN_EQUIPMENT_ESC_STATUS_ID, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, &esc_status_ev, &actuators_uavcan_esc_status_cb);
// Configure telemetry
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, actuators_uavcan_send_esc);
#endif
// Set initialization
actuators_uavcan_initialized = true;
}
/**
* Commit actuator values to the uavcan interface
*/
void actuators_uavcan_commit(struct uavcan_iface_t* iface, int16_t *values, uint8_t nb)
{
uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
uint32_t offset = 0;
// Encode the values as 14-bit signed integers
for(uint8_t i = 0; i < nb; i++) {
canardEncodeScalar(buffer, offset, 14, (void *)&values[i]);
offset += 14;
}
// Broadcast the raw command message on the interface
uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID,
CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset+7)/8);
}

32
sw/airborne/subsystems/actuators/actuators_uavcan.h Normal file
View File

@@ -0,0 +1,32 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef ACTUATORS_UAVCAN_H
#define ACTUATORS_UAVCAN_H
#include "modules/uavcan/uavcan.h"
#include BOARD_CONFIG
/* External functions */
extern void actuators_uavcan_init(struct uavcan_iface_t* iface);
extern void actuators_uavcan_commit(struct uavcan_iface_t* iface, int16_t *values, uint8_t nb);
#endif /* ACTUATORS_UAVCAN_H */

34
sw/airborne/subsystems/actuators/actuators_uavcan1.h Normal file
View File

@@ -0,0 +1,34 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef ACTUATORS_UAVCAN1_H
#define ACTUATORS_UAVCAN1_H
#include "actuators_uavcan.h"
/** Stub file needed per uavcan interface because of generator */
extern int16_t actuators_uavcan1_values[SERVOS_UAVCAN1_NB];
#define ActuatorsUavcan1Init() actuators_uavcan_init(&uavcan1)
#define ActuatorUavcan1Set(_i, _v) { actuators_uavcan1_values[_i] = _v; }
#define ActuatorsUavcan1Commit() actuators_uavcan_commit(&uavcan1, actuators_uavcan1_values, SERVOS_UAVCAN1_NB)
#endif /* ACTUATORS_UAVCAN1_H */

34
sw/airborne/subsystems/actuators/actuators_uavcan2.h Normal file
View File

@@ -0,0 +1,34 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef ACTUATORS_UAVCAN2_H
#define ACTUATORS_UAVCAN2_H
#include "actuators_uavcan.h"
/** Stub file needed per interface because of generator */
extern int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
#define ActuatorsUavcan2Init() actuators_uavcan_init(&uavcan2)
#define ActuatorUavcan2Set(_i, _v) { actuators_uavcan2_values[_i] = _v; }
#define ActuatorsUavcan2Commit() actuators_uavcan_commit(&uavcan2, actuators_uavcan2_values, SERVOS_UAVCAN2_NB)
#endif /* ACTUATORS_UAVCAN2_H */

1
sw/ext/libcanard Submodule

Submodule sw/ext/libcanard added at 4244b8c0b3