mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Merge pull request #2027 from martinmm/master_ins_temp_ctrl
- add temperature control module for Bebop2 - add usb dual cdc board to Bebop2 for serial modem and payload - Sparkfun FIO v3 board adds simultaneous XBee serial modem and SHT75 humidity payload sensor.
This commit is contained in:
Felix Ruess
GitHub
@@ -0,0 +1,262 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- no dampers -->
|
||||
<airframe name="bebop2_lum1_xbee">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="bebop2"/>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="udp"/>
|
||||
</target>
|
||||
|
||||
<define name="UART4_DEV" value="/dev/ttyACM0"/>
|
||||
<define name="UART5_DEV" value="/dev/ttyACM1"/>
|
||||
|
||||
<define name="SENSOR_SYNC_SEND"/>
|
||||
<define name="IMU_TEMP_CTRL_DEBUG"/>
|
||||
|
||||
<module name="telemetry" type="xbee_api">
|
||||
<configure name="MODEM_PORT" value="UART4"/>
|
||||
<configure name="MODEM_BAUD" value="B57600"/>
|
||||
</module>
|
||||
|
||||
<!--module name="telemetry" type="transparent_udp"/-->
|
||||
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="bebop"/>
|
||||
<module name="imu" type="bebop"/>
|
||||
<module name="gps" type="ublox"/>
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
|
||||
</module>
|
||||
<module name="ins" type="extended"/>
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<!--module name="wind_estimator"/-->
|
||||
<module name="geo_mag"/>
|
||||
<module name="air_data"/>
|
||||
<module name="send_imu_mag_current"/>
|
||||
<module name="gps" type="ubx_ucenter"/>
|
||||
<module name="logger_file">
|
||||
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
|
||||
</module>
|
||||
<module name="humid_sht_uart">
|
||||
<configure name="SHT_UART" value="UART5"/>
|
||||
</module>
|
||||
<module name="imu_temp_ctrl"/>
|
||||
</modules>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="6000"/>
|
||||
</commands>
|
||||
|
||||
<servos driver="Default">
|
||||
<servo name="TOP_LEFT" no="0" min="2500" neutral="2500" max="12000"/>
|
||||
<servo name="TOP_RIGHT" no="1" min="2500" neutral="2500" max="12000"/>
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="2500" neutral="2500" max="12000"/>
|
||||
<servo name="BOTTOM_LEFT" no="3" min="2500" neutral="2500" max="12000"/>
|
||||
</servos>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="0"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
<define name="REVERSE" value="TRUE"/>
|
||||
<define name="TYPE" value="QUAD_X"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
|
||||
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
|
||||
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
|
||||
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AIR_DATA" prefix="AIR_DATA_">
|
||||
<define name="CALC_AIRSPEED" value="FALSE"/>
|
||||
<define name="CALC_TAS_FACTOR" value="FALSE"/>
|
||||
<define name="CALC_AMSL_BARO" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Magnetometer still needs to be calibrated -->
|
||||
|
||||
<!--red LM/-->
|
||||
<define name="MAG_X_NEUTRAL" value="-12"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="123"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="239"/>
|
||||
<define name="MAG_X_SENS" value="6.56662079024" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="6.56528562457" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="6.89245853894" integer="16"/>
|
||||
|
||||
<define name= "MAG_X_CURRENT_COEF" value="0.00349278666278"/>
|
||||
<define name= "MAG_Y_CURRENT_COEF" value="-0.00294581023579"/>
|
||||
<define name= "MAG_Z_CURRENT_COEF" value="0.00744261463196"/>
|
||||
|
||||
</section>
|
||||
|
||||
<!-- local magnetic field -->
|
||||
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
|
||||
<!-- Toulouse -->
|
||||
<!--define name="H_X" value="0.513081"/>
|
||||
<define name="H_Y" value="-0.00242783"/>
|
||||
<define name="H_Z" value="0.858336"/-->
|
||||
<!-- Delft -->
|
||||
<!--define name="H_X" value="0.3892503"/>
|
||||
<define name="H_Y" value="0.0017972"/>
|
||||
<define name="H_Z" value="0.9211303"/-->
|
||||
<!-- Gross Lobke -->
|
||||
<!--define name="H_X" value="0.382151"/>
|
||||
<define name="H_Y" value="0.01674"/>
|
||||
<define name="H_Z" value="0.923948"/-->
|
||||
<!-- Longyearbyen -->
|
||||
<!--define name="H_X" value="0.133047"/>
|
||||
<define name="H_Y" value="0.019769"/>
|
||||
<define name="H_Z" value="0.990913"/-->
|
||||
<!-- Andenes HAF -->
|
||||
<!--define name="H_X" value="0.20751"/>
|
||||
<define name="H_Y" value="-0.0244116"/>
|
||||
<define name="H_Z" value="0.977928"/-->
|
||||
|
||||
<!-- Hailuoto -->
|
||||
<define name="H_X" value="0.236568"/>
|
||||
<define name="H_Y" value="0.0424621"/>
|
||||
<define name="H_Z" value="0.970687"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<define name="SONAR_MAX_RANGE" value="2.2"/>
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- setpoint limits for attitude stabilization rc flight -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="300" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- attitude reference generation model -->
|
||||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9"/>
|
||||
<define name="REF_MAX_P" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9"/>
|
||||
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.05"/>
|
||||
<define name="G1_Q" value="0.04"/>
|
||||
<define name="G1_R" value="0.0022"/>
|
||||
<define name="G2_R" value="0.20"/>
|
||||
|
||||
<!-- Here it is assumed that your removed the damping from your bebop2!
|
||||
The dampers do not really damp, but cause oscillation. By removing/
|
||||
fixing them, the bebop2 will fly much better-->
|
||||
<define name="FILTER_ROLL_RATE" value="FALSE"/>
|
||||
<define name="FILTER_PITCH_RATE" value="FALSE"/>
|
||||
<define name="FILTER_YAW_RATE" value="FALSE"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="600.0"/>
|
||||
<define name="REF_ERR_Q" value="600.0"/>
|
||||
<define name="REF_ERR_R" value="600.0"/>
|
||||
<define name="REF_RATE_P" value="28.0"/>
|
||||
<define name="REF_RATE_Q" value="28.0"/>
|
||||
<define name="REF_RATE_R" value="28.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="20.0"/>
|
||||
<define name="FILT_ZETA" value="0.7"/>
|
||||
<define name="FILT_OMEGA_R" value="20.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.7"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_CUTOFF" value="3.2"/>
|
||||
<define name="FILT_CUTOFF_R" value="3.2"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.06"/>
|
||||
<define name="ACT_DYN_Q" value="0.06"/>
|
||||
<define name="ACT_DYN_R" value="0.06"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="350"/>
|
||||
<define name="HOVER_KD" value="85"/>
|
||||
<define name="HOVER_KI" value="20"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="REF_MAX_SPEED" value="2" unit="m/s"/>
|
||||
<define name="MAX_BANK" value="32" unit="deg"/>
|
||||
<define name="PGAIN" value="60"/>
|
||||
<define name="DGAIN" value="170"/>
|
||||
<define name="IGAIN" value="30"/>
|
||||
</section>
|
||||
|
||||
<section name="NAVIGATION" prefix="NAV_">
|
||||
<define name="CLIMB_VSPEED" value="4.5"/>
|
||||
<define name="DESCEND_VSPEED" value="-1.0"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_RC_CLIMB"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="11.0" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="11.1" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="GCS">
|
||||
<define name="AC_ICON" value="quadrotor_xi"/>
|
||||
</section>
|
||||
</airframe>
|
||||
@@ -0,0 +1,98 @@
|
||||
<!-- Logitech F310 controller profile
|
||||
|
||||
Joystick has six axes:
|
||||
axis 0: X on left stick
|
||||
axis 1: Y on left stick
|
||||
axis 2: linear button on left side (LT)
|
||||
axis 3: X on right stick
|
||||
axis 4: Y on right stick
|
||||
axis 5: linear button on right side (RT)
|
||||
axis 6: X on D pad
|
||||
axis 7: Y on D pad
|
||||
|
||||
and 11 buttons:
|
||||
button 0: A, green
|
||||
button 1: B, red
|
||||
button 2: X, blue
|
||||
button 3: Y, orange
|
||||
button 4: LB, left front up
|
||||
button 5: RB, left front up
|
||||
button 6: BACK, left top
|
||||
button 7: START, right top
|
||||
button 8: logitech, top "home" button
|
||||
button 9: left stick press
|
||||
button 10: right stick press
|
||||
|
||||
The "mode" button swaps the axes on the left stick and the d pad.
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="lx" limit="1.00" trim="0"/>
|
||||
<axis index="1" name="ly" trim="0"/>
|
||||
<axis index="2" name="lt" limit="1.00" trim="0"/>
|
||||
<axis index="3" name="rx" limit="1.00" trim="0"/>
|
||||
<axis index="4" name="ry" limit="1.00" trim="0"/>
|
||||
<axis index="5" name="rt" limit="1.00" trim="0"/>
|
||||
<axis index="6" name="dpadx"/>
|
||||
<axis index="7" name="dpady"/>
|
||||
<button index="0" name="b0"/>
|
||||
<button index="1" name="b1"/>
|
||||
<button index="2" name="b2"/>
|
||||
<button index="3" name="b3"/>
|
||||
<button index="4" name="b4"/>
|
||||
<button index="5" name="b5"/>
|
||||
<button index="6" name="b6"/>
|
||||
<button index="7" name="b7"/>
|
||||
<button index="8" name="b8"/>
|
||||
<button index="9" name="b9"/>
|
||||
<button index="10" name="b10"/>
|
||||
<button index="11" name="b11"/>
|
||||
</input>
|
||||
|
||||
<variables>
|
||||
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
|
||||
<var name="mode" default="2"/>
|
||||
<set var="mode" value="0" on_event="b0"/>
|
||||
<set var="mode" value="1" on_event="b1"/>
|
||||
<set var="mode" value="2" on_event="b3"/>
|
||||
</variables>
|
||||
|
||||
<messages period="0.017">
|
||||
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="mode"/>
|
||||
<field name="throttle" value="Fit(0-ry,-127,127,0,127)"/>
|
||||
<field name="roll" value="rx"/>
|
||||
<field name="yaw" value="Fit(lx,-127,127,-64,64)"/>
|
||||
<field name="pitch" value="ly"/>
|
||||
</message>
|
||||
|
||||
<!-- trim commands -->
|
||||
<!-- not needed
|
||||
<message class="trim_plus" name="lx" on_event="b9"/>
|
||||
<message class="trim_minus" name="lx" on_event="b8"/>
|
||||
<message class="trim_plus" name="ly" on_event="b10"/>
|
||||
<message class="trim_minus" name="ly" on_event="b11"/>
|
||||
<message class="trim_plus" name="rx" on_event="b2"/>
|
||||
<message class="trim_minus" name="rx" on_event="b0"/>
|
||||
<message class="trim_minus" name="ry" on_event="b3"/>
|
||||
<message class="trim_plus" name="ry" on_event="b1"/>
|
||||
<message class="trim_save" name="" on_event="b5"/>
|
||||
-->
|
||||
|
||||
<!-- Button "6" / "BACK" for kill -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b6">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="1"/>
|
||||
</message>
|
||||
|
||||
<!-- Button "7" / "START" for resurrect -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b7">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="0"/>
|
||||
</message>
|
||||
|
||||
</messages>
|
||||
|
||||
</joystick>
|
||||
@@ -0,0 +1,98 @@
|
||||
<!-- Logitech F310 controller profile
|
||||
|
||||
Joystick has six axes:
|
||||
axis 0: X on left stick
|
||||
axis 1: Y on left stick
|
||||
axis 2: linear button on left side (LT)
|
||||
axis 3: X on right stick
|
||||
axis 4: Y on right stick
|
||||
axis 5: linear button on right side (RT)
|
||||
axis 6: X on D pad
|
||||
axis 7: Y on D pad
|
||||
|
||||
and 11 buttons:
|
||||
button 0: A, green
|
||||
button 1: B, red
|
||||
button 2: X, blue
|
||||
button 3: Y, orange
|
||||
button 4: LB, left front up
|
||||
button 5: RB, left front up
|
||||
button 6: BACK, left top
|
||||
button 7: START, right top
|
||||
button 8: logitech, top "home" button
|
||||
button 9: left stick press
|
||||
button 10: right stick press
|
||||
|
||||
The "mode" button swaps the axes on the left stick and the d pad.
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="lx" limit="1.00" trim="0"/>
|
||||
<axis index="1" name="ly" trim="0"/>
|
||||
<axis index="2" name="lt" limit="1.00" trim="0"/>
|
||||
<axis index="3" name="rx" limit="1.00" trim="0"/>
|
||||
<axis index="4" name="ry" limit="1.00" trim="0"/>
|
||||
<axis index="5" name="rt" limit="1.00" trim="0"/>
|
||||
<axis index="6" name="dpadx"/>
|
||||
<axis index="7" name="dpady"/>
|
||||
<button index="0" name="b0"/>
|
||||
<button index="1" name="b1"/>
|
||||
<button index="2" name="b2"/>
|
||||
<button index="3" name="b3"/>
|
||||
<button index="4" name="b4"/>
|
||||
<button index="5" name="b5"/>
|
||||
<button index="6" name="b6"/>
|
||||
<button index="7" name="b7"/>
|
||||
<button index="8" name="b8"/>
|
||||
<button index="9" name="b9"/>
|
||||
<button index="10" name="b10"/>
|
||||
<button index="11" name="b11"/>
|
||||
</input>
|
||||
|
||||
<variables>
|
||||
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
|
||||
<var name="mode" default="2"/>
|
||||
<set var="mode" value="0" on_event="b0"/>
|
||||
<set var="mode" value="1" on_event="b1"/>
|
||||
<set var="mode" value="2" on_event="b3"/>
|
||||
</variables>
|
||||
|
||||
<messages period="0.017">
|
||||
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="mode"/>
|
||||
<field name="throttle" value="Fit(0-ly,-127,127,0,127)"/>
|
||||
<field name="roll" value="rx"/>
|
||||
<field name="yaw" value="Fit(lx,-127,127,-64,64)"/>
|
||||
<field name="pitch" value="ry"/>
|
||||
</message>
|
||||
|
||||
<!-- trim commands -->
|
||||
<!-- not needed
|
||||
<message class="trim_plus" name="lx" on_event="b9"/>
|
||||
<message class="trim_minus" name="lx" on_event="b8"/>
|
||||
<message class="trim_plus" name="ly" on_event="b10"/>
|
||||
<message class="trim_minus" name="ly" on_event="b11"/>
|
||||
<message class="trim_plus" name="rx" on_event="b2"/>
|
||||
<message class="trim_minus" name="rx" on_event="b0"/>
|
||||
<message class="trim_minus" name="ry" on_event="b3"/>
|
||||
<message class="trim_plus" name="ry" on_event="b1"/>
|
||||
<message class="trim_save" name="" on_event="b5"/>
|
||||
-->
|
||||
|
||||
<!-- Button "6" / "BACK" for kill -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b6">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="1"/>
|
||||
</message>
|
||||
|
||||
<!-- Button "7" / "START" for resurrect -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b7">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="0"/>
|
||||
</message>
|
||||
|
||||
</messages>
|
||||
|
||||
</joystick>
|
||||
@@ -0,0 +1,26 @@
|
||||
<!--
|
||||
|
||||
thomaspfeifer.net PPM2USB joystick adapter with Futaba transmitter
|
||||
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="mode"/>
|
||||
<axis index="1" name="pitch"/>
|
||||
<axis index="2" name="throttle"/>
|
||||
<axis index="3" name="yaw"/>
|
||||
<axis index="4" name="roll"/>
|
||||
<axis index="5" name="kill"/>
|
||||
</input>
|
||||
|
||||
<messages period="0.05">
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="PprzMode(mode)"/>
|
||||
<field name="throttle" value="Fit(throttle,-100,100,0,127)" />
|
||||
<field name="roll" value="Fit(-roll,-100,100,-127,127)" />
|
||||
<field name="pitch" value="Fit(pitch,-100,100,-127,127)" />
|
||||
<field name="yaw" value="Fit(yaw,-100,100,-127,127)"/>
|
||||
</message>
|
||||
</messages>
|
||||
</joystick>
|
||||
@@ -0,0 +1,25 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="humid_sht_uart" dir="meteo">
|
||||
<doc>
|
||||
<description>
|
||||
SHTxx humidity sensor.
|
||||
This reads the values for humidity and temperature from the SHTxx sensor through an uart.
|
||||
</description>
|
||||
<configure name="SHT_UART" value="UARTX" description="UART on which computer is connected"/>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="humid_sht_uart.h"/>
|
||||
</header>
|
||||
<init fun="humid_sht_uart_init()"/>
|
||||
<periodic fun="humid_sht_uart_periodic()" freq="1."/>
|
||||
<event fun="humid_sht_uart_event()"/>
|
||||
<makefile target="ap">
|
||||
<configure name="SHT_UART" case="upper|lower"/>
|
||||
<file name="humid_sht_uart.c"/>
|
||||
<define name="USE_$(SHT_UART_UPPER)"/>
|
||||
<define name="MET_LINK" value="$(SHT_UART_LOWER)"/>
|
||||
<define name="$(SHT_UART_UPPER)_BAUD" value="B115200"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="imu_temp_ctrl" dir="ins">
|
||||
<doc>
|
||||
<description>
|
||||
Bebop2/Disco INS (MPU6x) sensor temperature control.
|
||||
</description>
|
||||
<define name="IMU_TEMP_CTRL_DEBUG" description="output heater setting in percent and temperature in TMP_STATUS message"/>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="imu_temp_ctrl.h"/>
|
||||
</header>
|
||||
<init fun="imu_temp_ctrl_init()"/>
|
||||
<periodic fun="imu_temp_ctrl_periodic()" freq="20."/>
|
||||
<makefile target="ap">
|
||||
<file name="imu_temp_ctrl.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,265 @@
|
||||
|
||||
|
||||
/* Copyright (c) 2011, Peter Barrett
|
||||
**
|
||||
** Permission to use, copy, modify, and/or distribute this software for
|
||||
** any purpose with or without fee is hereby granted, provided that the
|
||||
** above copyright notice and this permission notice appear in all copies.
|
||||
**
|
||||
** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
|
||||
** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
|
||||
** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR
|
||||
** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES
|
||||
** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
|
||||
** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
|
||||
** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS
|
||||
** SOFTWARE.
|
||||
*/
|
||||
|
||||
/* adapted to support 2nd CDC USB port by Martin Mueller */
|
||||
|
||||
#include <util/atomic.h>
|
||||
#include "USBAPI.h"
|
||||
#include "DUALCDCUSB.h"
|
||||
|
||||
#if defined(USBCON)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
u32 dwDTERate;
|
||||
u8 bCharFormat;
|
||||
u8 bParityType;
|
||||
u8 bDataBits;
|
||||
u8 lineState;
|
||||
} LineInfo;
|
||||
|
||||
static volatile LineInfo _usbLineInfo = { 57600, 0x00, 0x00, 0x00, 0x00 };
|
||||
static volatile int32_t breakValue = -1;
|
||||
|
||||
DUALCDCUSB_ DualCDCUSB;
|
||||
|
||||
bool DUALCDCUSB_::setup(USBSetup& setup)
|
||||
{
|
||||
u8 r = setup.bRequest;
|
||||
u8 requestType = setup.bmRequestType;
|
||||
|
||||
if (REQUEST_DEVICETOHOST_CLASS_INTERFACE == requestType) {
|
||||
if (CDC_GET_LINE_CODING == r) {
|
||||
USB_SendControl(0,(void*)&_usbLineInfo,7);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
if (REQUEST_HOSTTODEVICE_CLASS_INTERFACE == requestType) {
|
||||
if (CDC_SEND_BREAK == r) {
|
||||
breakValue = ((uint16_t)setup.wValueH << 8) | setup.wValueL;
|
||||
}
|
||||
|
||||
if (CDC_SET_LINE_CODING == r) {
|
||||
USB_RecvControl((void*)&_usbLineInfo,7);
|
||||
}
|
||||
|
||||
if (CDC_SET_CONTROL_LINE_STATE == r) {
|
||||
_usbLineInfo.lineState = setup.wValueL;
|
||||
}
|
||||
|
||||
if (CDC_SET_LINE_CODING == r || CDC_SET_CONTROL_LINE_STATE == r) {
|
||||
/* nothing to do here */
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void DUALCDCUSB_::get_status(unsigned char* buf)
|
||||
{
|
||||
}
|
||||
|
||||
int DUALCDCUSB_::getInterface(uint8_t* interfaceCount)
|
||||
{
|
||||
*interfaceCount += 2;
|
||||
|
||||
CDCDescriptor _cdcInterface2 = {
|
||||
/* Interface Association */
|
||||
D_IAD(2,2,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,1),
|
||||
|
||||
/* CDC communication interface */
|
||||
D_INTERFACE(CDC_ACM_INTERFACE2,1,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,0),
|
||||
/* Header (1.10 bcd) */
|
||||
D_CDCCS(CDC_HEADER,0x10,0x01),
|
||||
/* Device handles call management (not) */
|
||||
D_CDCCS(CDC_CALL_MANAGEMENT,1,1),
|
||||
/* SET_LINE_CODING, GET_LINE_CODING, SET_CONTROL_LINE_STATE supported */
|
||||
D_CDCCS4(CDC_ABSTRACT_CONTROL_MANAGEMENT,6),
|
||||
/* Communication interface is master, data interface is slave 0 */
|
||||
D_CDCCS(CDC_UNION,CDC_ACM_INTERFACE2,CDC_DATA_INTERFACE2),
|
||||
D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_ACM2),USB_ENDPOINT_TYPE_INTERRUPT,0x10,0x40),
|
||||
|
||||
/* CDC data interface */
|
||||
D_INTERFACE(CDC_DATA_INTERFACE2,2,CDC_DATA_INTERFACE_CLASS,0,0),
|
||||
D_ENDPOINT(USB_ENDPOINT_OUT(CDC_ENDPOINT_OUT2),USB_ENDPOINT_TYPE_BULK,USB_EP_SIZE,0),
|
||||
D_ENDPOINT(USB_ENDPOINT_IN(CDC_ENDPOINT_IN2),USB_ENDPOINT_TYPE_BULK,USB_EP_SIZE,0)
|
||||
};
|
||||
return USB_SendControl(0, &_cdcInterface2, sizeof(_cdcInterface2));
|
||||
}
|
||||
|
||||
int DUALCDCUSB_::getDescriptor(USBSetup& setup __attribute__((unused)))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t DUALCDCUSB_::getShortName(char* name)
|
||||
{
|
||||
memcpy(name, "Paparazzi", 9);
|
||||
return 9;
|
||||
}
|
||||
|
||||
bool DUALCDCUSB_::begin(void)
|
||||
{
|
||||
}
|
||||
|
||||
DUALCDCUSB_::DUALCDCUSB_(void) : PluggableUSBModule(3, 2, epType)
|
||||
{
|
||||
epType[0] = EP_TYPE_INTERRUPT_IN;
|
||||
epType[1] = EP_TYPE_BULK_OUT;
|
||||
epType[2] = EP_TYPE_BULK_IN;
|
||||
PluggableUSB().plug(this);
|
||||
}
|
||||
|
||||
void Serial__::begin(unsigned long /* baud_count */)
|
||||
{
|
||||
peek_buffer = -1;
|
||||
}
|
||||
|
||||
void Serial__::begin(unsigned long /* baud_count */, byte /* config */)
|
||||
{
|
||||
peek_buffer = -1;
|
||||
}
|
||||
|
||||
void Serial__::end(void)
|
||||
{
|
||||
}
|
||||
|
||||
int Serial__::available(void)
|
||||
{
|
||||
if (peek_buffer >= 0) {
|
||||
return 1 + USB_Available(CDC_RX2);
|
||||
}
|
||||
return USB_Available(CDC_RX2);
|
||||
}
|
||||
|
||||
int Serial__::peek(void)
|
||||
{
|
||||
if (peek_buffer < 0)
|
||||
peek_buffer = USB_Recv(CDC_RX2);
|
||||
return peek_buffer;
|
||||
}
|
||||
|
||||
int Serial__::read(void)
|
||||
{
|
||||
if (peek_buffer >= 0) {
|
||||
int c = peek_buffer;
|
||||
peek_buffer = -1;
|
||||
return c;
|
||||
}
|
||||
return USB_Recv(CDC_RX2);
|
||||
}
|
||||
|
||||
int Serial__::availableForWrite(void)
|
||||
{
|
||||
return USB_SendSpace(CDC_TX2);
|
||||
}
|
||||
|
||||
void Serial__::flush(void)
|
||||
{
|
||||
USB_Flush(CDC_TX2);
|
||||
}
|
||||
|
||||
size_t Serial__::write(uint8_t c)
|
||||
{
|
||||
return write(&c, 1);
|
||||
}
|
||||
|
||||
size_t Serial__::write(const uint8_t *buffer, size_t size)
|
||||
{
|
||||
/* only try to send bytes if the high-level CDC connection itself
|
||||
is open (not just the pipe) - the OS should set lineState when the port
|
||||
is opened and clear lineState when the port is closed.
|
||||
bytes sent before the user opens the connection or after
|
||||
the connection is closed are lost - just like with a UART. */
|
||||
|
||||
// TODO - ZE - check behavior on different OSes and test what happens if an
|
||||
// open connection isn't broken cleanly (cable is yanked out, host dies
|
||||
// or locks up, or host virtual serial port hangs)
|
||||
if (_usbLineInfo.lineState > 0) {
|
||||
int r = USB_Send(CDC_TX2|TRANSFER_RELEASE,buffer,size);
|
||||
if (r > 0) {
|
||||
return r;
|
||||
} else {
|
||||
setWriteError();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
setWriteError();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// This operator is a convenient way for a sketch to check whether the
|
||||
// port has actually been configured and opened by the host (as opposed
|
||||
// to just being connected to the host). It can be used, for example, in
|
||||
// setup() before printing to ensure that an application on the host is
|
||||
// actually ready to receive and display the data.
|
||||
// We add a short delay before returning to fix a bug observed by Federico
|
||||
// where the port is configured (lineState != 0) but not quite opened.
|
||||
Serial__::operator bool() {
|
||||
bool result = false;
|
||||
if (_usbLineInfo.lineState > 0)
|
||||
result = true;
|
||||
delay(10);
|
||||
return result;
|
||||
}
|
||||
|
||||
unsigned long Serial__::baud() {
|
||||
// Disable interrupts while reading a multi-byte value
|
||||
uint32_t baudrate;
|
||||
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
||||
baudrate = _usbLineInfo.dwDTERate;
|
||||
}
|
||||
return baudrate;
|
||||
}
|
||||
|
||||
uint8_t Serial__::stopbits() {
|
||||
return _usbLineInfo.bCharFormat;
|
||||
}
|
||||
|
||||
uint8_t Serial__::paritytype() {
|
||||
return _usbLineInfo.bParityType;
|
||||
}
|
||||
|
||||
uint8_t Serial__::numbits() {
|
||||
return _usbLineInfo.bDataBits;
|
||||
}
|
||||
|
||||
bool Serial__::dtr() {
|
||||
return _usbLineInfo.lineState & 0x1;
|
||||
}
|
||||
|
||||
bool Serial__::rts() {
|
||||
return _usbLineInfo.lineState & 0x2;
|
||||
}
|
||||
|
||||
int32_t Serial__::readBreak() {
|
||||
int32_t ret;
|
||||
// Disable IRQs while reading and clearing breakValue to make
|
||||
// sure we don't overwrite a value just set by the ISR.
|
||||
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
|
||||
ret = breakValue;
|
||||
breakValue = -1;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
Serial__ Serial2;
|
||||
|
||||
|
||||
#endif /* USBCON */
|
||||
@@ -0,0 +1,146 @@
|
||||
|
||||
#ifndef DUALCDCUSB_H
|
||||
#define DUALCDCUSB_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
#if ARDUINO < 10606
|
||||
#error DUALCDCUSB needs Arduino IDE 1.6.6 or higher
|
||||
#endif
|
||||
|
||||
#if !defined(USBCON)
|
||||
#error DUALCDCUSB needs an USB MCU
|
||||
#endif
|
||||
|
||||
#if defined(ARDUINO_ARCH_AVR)
|
||||
#include "PluggableUSB.h"
|
||||
#else
|
||||
#include "USB/PluggableUSB.h"
|
||||
#endif
|
||||
|
||||
#if defined(__SAMD21G18A__)
|
||||
#define USB_SendControl USBDevice.sendControl
|
||||
#define USB_Available USBDevice.available
|
||||
#define USB_Recv USBDevice.recv
|
||||
#define USB_Send USBDevice.send
|
||||
#define USB_Flush USBDevice.flush
|
||||
#define is_write_enabled(x) (1)
|
||||
#endif
|
||||
#define CDC_ENDPOINT_ACM2 (4)
|
||||
#define CDC_ENDPOINT_OUT2 (5)
|
||||
#define CDC_ENDPOINT_IN2 (6)
|
||||
|
||||
#define CDC_ACM_INTERFACE2 2 // CDC ACM
|
||||
#define CDC_DATA_INTERFACE2 3 // CDC Data
|
||||
|
||||
#define CDC_RX2 CDC_ENDPOINT_OUT2
|
||||
#define CDC_TX2 CDC_ENDPOINT_IN2
|
||||
|
||||
#ifndef SERIAL_BUFFER_SIZE
|
||||
#if ((RAMEND - RAMSTART) < 1023)
|
||||
#define SERIAL_BUFFER_SIZE 16
|
||||
#else
|
||||
#define SERIAL_BUFFER_SIZE 64
|
||||
#endif
|
||||
#endif
|
||||
#if (SERIAL_BUFFER_SIZE>256)
|
||||
#error Please lower the CDC Buffer size
|
||||
#endif
|
||||
|
||||
_Pragma("pack(1)")
|
||||
|
||||
|
||||
|
||||
class Serial__ : public Stream
|
||||
{
|
||||
private:
|
||||
int peek_buffer;
|
||||
public:
|
||||
Serial__() { peek_buffer = -1; };
|
||||
void begin(unsigned long);
|
||||
void begin(unsigned long, uint8_t);
|
||||
void end(void);
|
||||
|
||||
virtual int available(void);
|
||||
virtual int peek(void);
|
||||
virtual int read(void);
|
||||
int availableForWrite(void);
|
||||
virtual void flush(void);
|
||||
virtual size_t write(uint8_t);
|
||||
virtual size_t write(const uint8_t*, size_t);
|
||||
using Print::write; // pull in write(str) and write(buf, size) from Print
|
||||
operator bool();
|
||||
|
||||
volatile uint8_t _rx_buffer_head;
|
||||
volatile uint8_t _rx_buffer_tail;
|
||||
unsigned char _rx_buffer[SERIAL_BUFFER_SIZE];
|
||||
|
||||
// This method allows processing "SEND_BREAK" requests sent by
|
||||
// the USB host. Those requests indicate that the host wants to
|
||||
// send a BREAK signal and are accompanied by a single uint16_t
|
||||
// value, specifying the duration of the break. The value 0
|
||||
// means to end any current break, while the value 0xffff means
|
||||
// to start an indefinite break.
|
||||
// readBreak() will return the value of the most recent break
|
||||
// request, but will return it at most once, returning -1 when
|
||||
// readBreak() is called again (until another break request is
|
||||
// received, which is again returned once).
|
||||
// This also mean that if two break requests are received
|
||||
// without readBreak() being called in between, the value of the
|
||||
// first request is lost.
|
||||
// Note that the value returned is a long, so it can return
|
||||
// 0-0xffff as well as -1.
|
||||
int32_t readBreak();
|
||||
|
||||
// These return the settings specified by the USB host for the
|
||||
// serial port. These aren't really used, but are offered here
|
||||
// in case a sketch wants to act on these settings.
|
||||
uint32_t baud();
|
||||
uint8_t stopbits();
|
||||
uint8_t paritytype();
|
||||
uint8_t numbits();
|
||||
bool dtr();
|
||||
bool rts();
|
||||
enum {
|
||||
ONE_STOP_BIT = 0,
|
||||
ONE_AND_HALF_STOP_BIT = 1,
|
||||
TWO_STOP_BITS = 2,
|
||||
};
|
||||
enum {
|
||||
NO_PARITY = 0,
|
||||
ODD_PARITY = 1,
|
||||
EVEN_PARITY = 2,
|
||||
MARK_PARITY = 3,
|
||||
SPACE_PARITY = 4,
|
||||
};
|
||||
|
||||
};
|
||||
extern Serial__ Serial2;
|
||||
|
||||
typedef struct {
|
||||
InterfaceDescriptor DualCDCInterface;
|
||||
} dual_cdc_usb_Descriptor;
|
||||
|
||||
_Pragma("pack()")
|
||||
|
||||
class DUALCDCUSB_ : public PluggableUSBModule {
|
||||
|
||||
protected:
|
||||
int getInterface(uint8_t* interfaceNum);
|
||||
int getDescriptor(USBSetup& setup);
|
||||
bool setup(USBSetup& setup);
|
||||
uint8_t getShortName(char* name);
|
||||
|
||||
public:
|
||||
bool begin(void);
|
||||
void get_status(unsigned char* buf);
|
||||
DUALCDCUSB_(void);
|
||||
|
||||
private:
|
||||
uint8_t epType[3];
|
||||
};
|
||||
|
||||
extern DUALCDCUSB_ DualCDCUSB;
|
||||
|
||||
#endif /* DUALCDCUSB_H */
|
||||
@@ -0,0 +1,5 @@
|
||||
This is a library to create two serial USB ports with ATMEGA32U4 based
|
||||
Arduino boards as the Sparkfun FIO v3. Copy it to your local Ardiono library
|
||||
folder, usually ~/Arduino/libraries
|
||||
|
||||
Tested with Arduino 1.8.1
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Copyright (C) 2017 The Paparazzi Team
|
||||
*
|
||||
* 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 modemhumid.ino
|
||||
*
|
||||
* modem and humidity sensor for Parrot Bebop2 using two serial ports
|
||||
* through a Sparkfun FIO v3 with XBee and SHT75 sensor
|
||||
*
|
||||
* requires DUALCDCUSB library, tested with Arduino v1.8.1
|
||||
*
|
||||
*/
|
||||
|
||||
#include <DUALCDCUSB.h>
|
||||
|
||||
int RXLED = 17;
|
||||
|
||||
void setup()
|
||||
{
|
||||
pinMode(RXLED, OUTPUT); // Set RX LED as an output
|
||||
|
||||
Serial.begin(57600); // USB CDC 0 port
|
||||
Serial1.begin(57600); // serial port
|
||||
Serial2.begin(57600); // USB CDC 1 port
|
||||
|
||||
TXLED1; // set the LED off
|
||||
digitalWrite(RXLED, HIGH); // set the LED off
|
||||
|
||||
humid_sht_init();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
int i, av, ret;
|
||||
size_t re;
|
||||
unsigned char buff[256], device_addr = 0xF0, len = 2;
|
||||
unsigned short tmd_temperature;
|
||||
float ftmd_temperature;
|
||||
static unsigned long ticks = 0, last_ticks = 0;
|
||||
|
||||
while (1) {
|
||||
|
||||
/* USB CDC 0 -> UART */
|
||||
av = Serial.available();
|
||||
if (av > 0) {
|
||||
if (av > 256)
|
||||
av = 256;
|
||||
ret = Serial.readBytes(buff, av);
|
||||
Serial1.write(buff, ret);
|
||||
}
|
||||
|
||||
/* UART -> USB CDC 0*/
|
||||
av = Serial1.available();
|
||||
if (av > 0) {
|
||||
if (av > 256)
|
||||
av = 256;
|
||||
ret = Serial1.readBytes(buff, av);
|
||||
Serial.write(buff, ret);
|
||||
}
|
||||
|
||||
ticks = millis();
|
||||
if (last_ticks == 0) last_ticks = ticks;
|
||||
if (last_ticks + 2 < ticks) {
|
||||
last_ticks = ticks;
|
||||
humid_sht_periodic();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,346 @@
|
||||
|
||||
//#define DEBUG
|
||||
|
||||
#define TRUE true
|
||||
#define FALSE false
|
||||
|
||||
#define noACK 0
|
||||
#define ACK 1
|
||||
#define TEMP 0
|
||||
#define HUMI 1
|
||||
|
||||
//adr command r/w
|
||||
//000 0011 0
|
||||
#define STATUS_REG_W 0x06
|
||||
//000 0011 1
|
||||
#define STATUS_REG_R 0x07
|
||||
//000 0001 1
|
||||
#define MEASURE_TEMP 0x03
|
||||
//000 0010 1
|
||||
#define MEASURE_HUMI 0x05
|
||||
//000 1111 0
|
||||
#define RESET 0x1e
|
||||
|
||||
|
||||
#define SHT_IDLE 0
|
||||
#define SHT_MEASURING_HUMID 1
|
||||
#define SHT_MEASURING_TEMP 2
|
||||
|
||||
/* D4, PD4 */
|
||||
#define SHT_DAT_GPIO 4
|
||||
/* D5, PC6 */
|
||||
#define SHT_SCK_GPIO 5
|
||||
|
||||
#if !defined SHT_DAT_GPIO || !defined SHT_SCK_GPIO
|
||||
#error You need to define SHT_DAT_GPIO and SHT_SCK_GPIO
|
||||
#endif
|
||||
|
||||
/// set data pin to input
|
||||
#define DATA_SET pinMode(SHT_DAT_GPIO, INPUT);
|
||||
/// set data pin to output
|
||||
#define DATA_CLR pinMode(SHT_DAT_GPIO, OUTPUT)
|
||||
/// get data pin
|
||||
#define DATA_IN digitalRead(SHT_DAT_GPIO)
|
||||
|
||||
/// set clock pin to high
|
||||
#define SCK_SET digitalWrite(SHT_SCK_GPIO, HIGH)
|
||||
/// set clock pin to low
|
||||
#define SCK_CLR digitalWrite(SHT_SCK_GPIO, LOW)
|
||||
/// set clock pin to output
|
||||
#define SCK_OUT pinMode(SHT_SCK_GPIO, OUTPUT)
|
||||
|
||||
uint16_t humidsht, tempsht;
|
||||
float fhumidsht, ftempsht;
|
||||
bool humid_sht_available;
|
||||
uint8_t humid_sht_status;
|
||||
|
||||
uint8_t s_write_byte(uint8_t value);
|
||||
uint8_t s_read_byte(uint8_t ack);
|
||||
void s_transstart(void);
|
||||
void s_connectionreset(void);
|
||||
uint8_t s_read_statusreg(uint8_t *p_value, uint8_t *p_checksum);
|
||||
uint8_t s_write_statusreg(uint8_t *p_value);
|
||||
uint8_t s_measure(uint16_t *p_value, uint8_t *p_checksum, uint8_t mode);
|
||||
uint8_t s_start_measure(uint8_t mode);
|
||||
uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum);
|
||||
void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem);
|
||||
uint8_t humid_sht_reset(void);
|
||||
|
||||
|
||||
uint8_t s_write_byte(uint8_t value)
|
||||
{
|
||||
uint8_t i, error = 0;
|
||||
|
||||
for (i = 0x80; i > 0; i /= 2) { //shift bit for masking
|
||||
if (i & value) { DATA_SET; } //masking value with i , write to SENSI-BUS
|
||||
else { DATA_CLR; }
|
||||
SCK_SET; //clk for SENSI-BUS
|
||||
SCK_SET; SCK_SET; SCK_SET; //pulswith approx. 5 us
|
||||
// _nop_();_nop_();_nop_(); //pulswith approx. 5 us
|
||||
SCK_CLR;
|
||||
}
|
||||
DATA_SET; //release DATA-line
|
||||
SCK_SET; //clk #9 for ack
|
||||
error = DATA_IN; //check ack (DATA will be pulled down by SHT11)
|
||||
SCK_CLR;
|
||||
|
||||
return error; //error=1 in case of no acknowledge
|
||||
}
|
||||
|
||||
uint8_t s_read_byte(uint8_t ack)
|
||||
{
|
||||
uint8_t i, val = 0;
|
||||
|
||||
DATA_SET; //release DATA-line
|
||||
for (i = 0x80; i > 0; i /= 2) { //shift bit for masking
|
||||
SCK_SET; //clk for SENSI-BUS
|
||||
if (DATA_IN) { val = (val | i); } //read bit
|
||||
SCK_CLR;
|
||||
}
|
||||
|
||||
if (ack) { DATA_CLR; } //in case of "ack==1" pull down DATA-Line
|
||||
SCK_SET; //clk #9 for ack
|
||||
SCK_SET; SCK_SET; SCK_SET; //pulswith approx. 5 us
|
||||
// _nop_();_nop_();_nop_(); //pulswith approx. 5 us
|
||||
SCK_CLR;
|
||||
DATA_SET; //release DATA-line
|
||||
return val;
|
||||
}
|
||||
|
||||
void s_transstart(void)
|
||||
{
|
||||
// generates a transmission start
|
||||
// _____ ________
|
||||
// DATA: |_______|
|
||||
// ___ ___
|
||||
// SCK : ___| |___| |______
|
||||
|
||||
DATA_SET; SCK_CLR; //Initial state
|
||||
SCK_CLR;// _nop_();
|
||||
SCK_SET;
|
||||
SCK_SET;// _nop_();
|
||||
DATA_CLR;
|
||||
DATA_CLR;// _nop_();
|
||||
SCK_CLR;
|
||||
SCK_CLR; SCK_CLR; SCK_CLR; // _nop_();_nop_();_nop_();
|
||||
SCK_SET;
|
||||
SCK_SET;// _nop_();
|
||||
DATA_SET;
|
||||
DATA_SET;// _nop_();
|
||||
SCK_CLR;
|
||||
}
|
||||
|
||||
void s_connectionreset(void)
|
||||
{
|
||||
// communication reset: DATA-line=1 and at least 9 SCK cycles followed by transstart
|
||||
// _____________________________________________________ ________
|
||||
// DATA: |_______|
|
||||
// _ _ _ _ _ _ _ _ _ ___ ___
|
||||
// SCK : __| |__| |__| |__| |__| |__| |__| |__| |__| |______| |___| |______
|
||||
|
||||
uint8_t i;
|
||||
|
||||
DATA_SET; SCK_CLR; //Initial state
|
||||
for (i = 0; i < 9; i++) { //9 SCK cycles
|
||||
SCK_SET;
|
||||
SCK_CLR;
|
||||
}
|
||||
s_transstart(); //transmission start
|
||||
}
|
||||
|
||||
uint8_t s_read_statusreg(uint8_t *p_value, uint8_t *p_checksum)
|
||||
{
|
||||
// reads the status register with checksum (8-bit)
|
||||
uint8_t error = 0;
|
||||
|
||||
s_transstart(); //transmission start
|
||||
error = s_write_byte(STATUS_REG_R); //send command to sensor
|
||||
*p_value = s_read_byte(ACK); //read status register (8-bit)
|
||||
*p_checksum = s_read_byte(noACK); //read checksum (8-bit)
|
||||
return error; //error=1 in case of no response form the sensor
|
||||
}
|
||||
|
||||
uint8_t s_write_statusreg(uint8_t *p_value)
|
||||
{
|
||||
// writes the status register with checksum (8-bit)
|
||||
uint8_t error = 0;
|
||||
|
||||
s_transstart(); //transmission start
|
||||
error += s_write_byte(STATUS_REG_W); //send command to sensor
|
||||
error += s_write_byte(*p_value); //send value of status register
|
||||
return error; //error>=1 in case of no response form the sensor
|
||||
}
|
||||
|
||||
uint8_t s_measure(uint16_t *p_value, uint8_t *p_checksum, uint8_t mode)
|
||||
{
|
||||
// makes a measurement (humidity/temperature) with checksum
|
||||
uint8_t error = 0;
|
||||
uint32_t i;
|
||||
|
||||
s_transstart(); //transmission start
|
||||
switch (mode) { //send command to sensor
|
||||
case TEMP : error += s_write_byte(MEASURE_TEMP); break;
|
||||
case HUMI : error += s_write_byte(MEASURE_HUMI); break;
|
||||
default : break;
|
||||
}
|
||||
for (i = 0; i < 6665535; i++) if (DATA_IN == 0) { break; } //wait until sensor has finished the measurement
|
||||
if (DATA_IN) { error += 1; } // or timeout (~2 sec.) is reached
|
||||
*(p_value) = s_read_byte(ACK) << 8; //read the first byte (MSB)
|
||||
*(p_value) |= s_read_byte(ACK); //read the second byte (LSB)
|
||||
*p_checksum = s_read_byte(noACK); //read checksum
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
uint8_t s_start_measure(uint8_t mode)
|
||||
{
|
||||
// makes a measurement (humidity/temperature) with checksum
|
||||
uint8_t error = 0;
|
||||
|
||||
s_transstart(); //transmission start
|
||||
switch (mode) { //send command to sensor
|
||||
case TEMP : error += s_write_byte(MEASURE_TEMP); break;
|
||||
case HUMI : error += s_write_byte(MEASURE_HUMI); break;
|
||||
default : break;
|
||||
}
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum)
|
||||
{
|
||||
// reads a measurement (humidity/temperature) with checksum
|
||||
uint8_t error = 0;
|
||||
|
||||
if (DATA_IN) { error += 1; } //still busy?
|
||||
*(p_value) = s_read_byte(ACK) << 8; //read the first byte (MSB)
|
||||
*(p_value) |= s_read_byte(ACK); //read the second byte (LSB)
|
||||
*p_checksum = s_read_byte(noACK); //read checksum
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem)
|
||||
{
|
||||
// calculates temperature [ C] and humidity [%RH]
|
||||
// input : humi [Ticks] (12 bit)
|
||||
// temp [Ticks] (14 bit)
|
||||
// output: humi [%RH]
|
||||
// temp [ C]
|
||||
|
||||
const float C1 = -4.0; // for 12 Bit
|
||||
const float C2 = 0.0405; // for 12 Bit
|
||||
const float C3 = -0.0000028; // for 12 Bit
|
||||
const float T1 = 0.01; // for 14 Bit @ 5V
|
||||
const float T2 = 0.00008; // for 14 Bit @ 5V
|
||||
float rh; // rh: Humidity [Ticks] 12 Bit
|
||||
float t; // t: Temperature [Ticks] 14 Bit
|
||||
float rh_lin; // rh_lin: Humidity linear
|
||||
float rh_true; // rh_true: Temperature compensated humidity
|
||||
float t_C; // t_C : Temperature [ C]
|
||||
|
||||
rh = (float)hum; //converts integer to float
|
||||
t = (float)tem; //converts integer to float
|
||||
|
||||
t_C = t * 0.01 - 39.66; //calc. Temperature from ticks to [°C] @ 3.5V
|
||||
rh_lin = C3 * rh * rh + C2 * rh + C1; //calc. Humidity from ticks to [%RH]
|
||||
rh_true = (t_C - 25) * (T1 + T2 * rh) + rh_lin; //calc. Temperature compensated humidity [%RH]
|
||||
if (rh_true > 100) { rh_true = 100; } //cut if the value is outside of
|
||||
if (rh_true < 0.1) { rh_true = 0.1; } //the physical possible range
|
||||
*ftem = t_C; //return temperature [ C]
|
||||
*fhum = rh_true; //return humidity[%RH]
|
||||
}
|
||||
|
||||
uint8_t humid_sht_reset(void)
|
||||
{
|
||||
// resets the sensor by a softreset
|
||||
uint8_t error = 0;
|
||||
|
||||
s_connectionreset(); //reset communication
|
||||
error += s_write_byte(RESET); //send RESET-command to sensor
|
||||
|
||||
return error; //error=1 in case of no response form the sensor
|
||||
}
|
||||
|
||||
void humid_sht_init(void)
|
||||
{
|
||||
/* Configure DAT/SCL pin as GPIO */
|
||||
DATA_SET;
|
||||
SCK_OUT;
|
||||
SCK_CLR;
|
||||
|
||||
humid_sht_available = FALSE;
|
||||
humid_sht_status = SHT_IDLE;
|
||||
}
|
||||
|
||||
void humid_sht_periodic(void)
|
||||
{
|
||||
uint8_t error = 0, checksum, i;
|
||||
uint8_t data[9] = { 0 };
|
||||
uint16_t chk = 0;
|
||||
|
||||
if (humid_sht_status == SHT_IDLE) {
|
||||
/* init humidity read */
|
||||
s_connectionreset();
|
||||
s_start_measure(HUMI);
|
||||
humid_sht_status = SHT_MEASURING_HUMID;
|
||||
} else if (humid_sht_status == SHT_MEASURING_HUMID) {
|
||||
/* busy? */
|
||||
if (DATA_IN) return;
|
||||
/* get data */
|
||||
error += s_read_measure(&humidsht, &checksum);
|
||||
|
||||
if (error != 0) {
|
||||
s_connectionreset();
|
||||
s_start_measure(HUMI); //restart
|
||||
} else {
|
||||
error += s_start_measure(TEMP);
|
||||
humid_sht_status = SHT_MEASURING_TEMP;
|
||||
}
|
||||
} else if (humid_sht_status == SHT_MEASURING_TEMP) {
|
||||
/* busy? */
|
||||
if (DATA_IN) return;
|
||||
/* get data */
|
||||
error += s_read_measure(&tempsht, &checksum);
|
||||
|
||||
if (error != 0) {
|
||||
s_connectionreset();
|
||||
s_start_measure(TEMP); //restart
|
||||
} else {
|
||||
|
||||
/*
|
||||
[0] 0xFF
|
||||
[1] sht_temp_0
|
||||
[2] sht_temp_1
|
||||
[3] sht_humid_0
|
||||
[4] sht_humid_1
|
||||
[5] checksum
|
||||
*/
|
||||
|
||||
data[0] = 0xFF;
|
||||
data[1] = tempsht & 0xFF;
|
||||
data[2] = (tempsht >> 8) & 0xFF;
|
||||
data[3] = humidsht & 0xFF;
|
||||
data[4] = (humidsht >> 8) & 0xFF;
|
||||
for (i = 1; i < 5; i++)
|
||||
chk += data[i];
|
||||
data[5] = chk & 0xFF;
|
||||
|
||||
humid_sht_available = TRUE;
|
||||
s_connectionreset();
|
||||
s_start_measure(HUMI);
|
||||
humid_sht_status = SHT_MEASURING_HUMID;
|
||||
|
||||
#ifdef DEBUG
|
||||
calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht);
|
||||
Serial2.println(fhumidsht);
|
||||
Serial2.println(ftempsht);
|
||||
#else
|
||||
Serial2.write(data, 6);
|
||||
#endif
|
||||
|
||||
humid_sht_available = FALSE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Copyright (C) 2017 The Paparazzi team
|
||||
* based on code from the Ardupilot project
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/imu_temp_ctrl.c
|
||||
*
|
||||
* INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO
|
||||
*
|
||||
* Controls the heating resistors in the Bebop2 to keep the MPU6050
|
||||
* gyro/accel INS sensors at a constant temperature
|
||||
*
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "imu_temp_ctrl.h"
|
||||
|
||||
uint8_t imu_temp_ctrl_ok = 0;
|
||||
int pwm_heat_duty_fd = 0;
|
||||
|
||||
#ifdef BEBOP_VERSION2
|
||||
#define PWM_HEAT_CHAN PWM_HEAT_CHAN_BEBOP2
|
||||
#elif BOARD==disco
|
||||
#define PWM_HEAT_CHAN PWM_HEAT_CHAN_DISCO
|
||||
#else
|
||||
#error temp control only implemented in Parrot Bebop2 and Disco
|
||||
#endif
|
||||
|
||||
void imu_temp_ctrl_periodic(void)
|
||||
{
|
||||
float temp_current, error;
|
||||
static float sum_error = 0;
|
||||
uint32_t output = 0;
|
||||
|
||||
temp_current = imu_bebop.mpu.temp;
|
||||
|
||||
if (imu_temp_ctrl_ok == 1) {
|
||||
/* minimal PI algo without dt from Ardupilot */
|
||||
error = (float) ((IMU_TEMP_TARGET) - temp_current);
|
||||
|
||||
/* Don't accumulate errors if the integrated error is superior
|
||||
* to the max duty cycle(pwm_period)
|
||||
*/
|
||||
if ((fabsf(sum_error) * IMU_TEMP_CTRL_KI < IMU_TEMP_CTRL_DUTY_MAX)) {
|
||||
sum_error = sum_error + error;
|
||||
}
|
||||
|
||||
output = IMU_TEMP_CTRL_KP * error + IMU_TEMP_CTRL_KI * sum_error;
|
||||
|
||||
if (output > IMU_TEMP_CTRL_DUTY_MAX) {
|
||||
output = IMU_TEMP_CTRL_DUTY_MAX;
|
||||
} else if (output < 0) {
|
||||
output = 0;
|
||||
}
|
||||
|
||||
if (dprintf(pwm_heat_duty_fd, "%u", output) < 0) {
|
||||
printf("[temp-ctrl] could not set duty cycle\n");
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef IMU_TEMP_CTRL_DEBUG
|
||||
uint16_t duty_cycle;
|
||||
duty_cycle = (uint16_t) ((uint32_t) output / (IMU_TEMP_CTRL_DUTY_MAX/100));
|
||||
|
||||
RunOnceEvery(IMU_TEMP_CTRL_PERIODIC_FREQ, DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &duty_cycle, &temp_current));
|
||||
#endif
|
||||
}
|
||||
|
||||
void imu_temp_ctrl_init(void)
|
||||
{
|
||||
int pwm_heat_run_fd, ret;
|
||||
char* path;
|
||||
|
||||
ret = asprintf(&path, "/sys/class/pwm/pwm_%u/run", PWM_HEAT_CHAN);
|
||||
if (ret < 0) {
|
||||
printf("[temp-ctrl] could not create pwm path\n");
|
||||
return;
|
||||
}
|
||||
|
||||
pwm_heat_run_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
|
||||
free(path);
|
||||
if (pwm_heat_run_fd < 0) {
|
||||
printf("[temp-ctrl] could not open pwm run device\n");
|
||||
return;
|
||||
}
|
||||
|
||||
ret = asprintf(&path, "/sys/class/pwm/pwm_%u/duty_ns", PWM_HEAT_CHAN);
|
||||
if (ret < 0) {
|
||||
printf("[temp-ctrl] could not create pwm duty path\n");
|
||||
close(pwm_heat_run_fd);
|
||||
return;
|
||||
}
|
||||
|
||||
pwm_heat_duty_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666);
|
||||
free(path);
|
||||
if (pwm_heat_duty_fd < 0) {
|
||||
printf("[temp-ctrl] could not open pwm duty path\n");
|
||||
close(pwm_heat_run_fd);
|
||||
return;
|
||||
}
|
||||
|
||||
ret = write(pwm_heat_duty_fd, "0", 1);
|
||||
if (ret != 1) {
|
||||
printf("[temp-ctrl] could not set duty cycle\n");
|
||||
goto error;
|
||||
}
|
||||
|
||||
ret = write(pwm_heat_run_fd, "0", 1);
|
||||
if (ret != 1) {
|
||||
printf("[temp-ctrl] could not disable pwm\n");
|
||||
goto error;
|
||||
}
|
||||
|
||||
ret = write(pwm_heat_run_fd, "1", 1);
|
||||
if (ret != 1) {
|
||||
printf("[temp-ctrl] could not enable pwm\n");
|
||||
goto error;
|
||||
}
|
||||
|
||||
imu_temp_ctrl_ok = 1;
|
||||
return;
|
||||
|
||||
error:
|
||||
close(pwm_heat_run_fd);
|
||||
close(pwm_heat_duty_fd);
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Copyright (C) 2017 The Paparazzi team
|
||||
* based on code from the Ardupilot project
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* This program 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 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ins/imu_temp_ctrl.c
|
||||
*
|
||||
* INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO
|
||||
*
|
||||
* Controls the heating resistors in the Bebop2 to keep the MPU6050
|
||||
* gyro/accel INS sensors at a constant temperature
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef IMU_TEMP_CTRL_H
|
||||
#define IMU_TEMP_CTRL_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
void imu_temp_ctrl_init(void);
|
||||
void imu_temp_ctrl_periodic(void);
|
||||
|
||||
#define IMU_TEMP_CTRL_DUTY_MAX 125000
|
||||
|
||||
#ifndef IMU_TEMP_TARGET
|
||||
#define IMU_TEMP_TARGET 50
|
||||
#endif
|
||||
|
||||
#define IMU_TEMP_CTRL_KP 20000
|
||||
#define IMU_TEMP_CTRL_KI 60
|
||||
|
||||
#define PWM_HEAT_CHAN_BEBOP2 6
|
||||
#define PWM_HEAT_CHAN_DISCO 10
|
||||
|
||||
#endif /* IMU_TEMP_CTRL_H */
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2017 The Paparazzi team
|
||||
*
|
||||
* 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 modules/meteo/humid_sht_uart.c
|
||||
*
|
||||
* SHTxx sensor interface
|
||||
*
|
||||
* This reads the values for humidity and temperature from the SHTxx sensor through an uart.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
#include "mcu_periph/gpio.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "humid_sht_uart.h"
|
||||
|
||||
uint16_t humidsht, tempsht;
|
||||
float fhumidsht, ftempsht;
|
||||
bool humid_sht_available;
|
||||
|
||||
void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem);
|
||||
void humid_sht_uart_parse(uint8_t c);
|
||||
|
||||
void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem)
|
||||
{
|
||||
// calculates temperature [ C] and humidity [%RH]
|
||||
// input : humi [Ticks] (12 bit)
|
||||
// temp [Ticks] (14 bit)
|
||||
// output: humi [%RH]
|
||||
// temp [ C]
|
||||
|
||||
const float C1 = -4.0; // for 12 Bit
|
||||
const float C2 = 0.0405; // for 12 Bit
|
||||
const float C3 = -0.0000028; // for 12 Bit
|
||||
const float T1 = 0.01; // for 14 Bit @ 5V
|
||||
const float T2 = 0.00008; // for 14 Bit @ 5V
|
||||
float rh; // rh: Humidity [Ticks] 12 Bit
|
||||
float t; // t: Temperature [Ticks] 14 Bit
|
||||
float rh_lin; // rh_lin: Humidity linear
|
||||
float rh_true; // rh_true: Temperature compensated humidity
|
||||
float t_C; // t_C : Temperature [ C]
|
||||
|
||||
rh = (float)hum; //converts integer to float
|
||||
t = (float)tem; //converts integer to float
|
||||
|
||||
t_C = t * 0.01 - 39.66; //calc. Temperature from ticks to [°C] @ 3.5V
|
||||
rh_lin = C3 * rh * rh + C2 * rh + C1; //calc. Humidity from ticks to [%RH]
|
||||
rh_true = (t_C - 25) * (T1 + T2 * rh) + rh_lin; //calc. Temperature compensated humidity [%RH]
|
||||
if (rh_true > 100) { rh_true = 100; } //cut if the value is outside of
|
||||
if (rh_true < 0.1) { rh_true = 0.1; } //the physical possible range
|
||||
*ftem = t_C; //return temperature [ C]
|
||||
*fhum = rh_true; //return humidity[%RH]
|
||||
}
|
||||
|
||||
void humid_sht_uart_periodic(void)
|
||||
{
|
||||
}
|
||||
|
||||
/* airspeed_otf_parse */
|
||||
void humid_sht_uart_parse(uint8_t c)
|
||||
{
|
||||
static uint8_t msg_cnt = 0;
|
||||
static uint8_t data[6];
|
||||
uint16_t i, chk = 0;
|
||||
|
||||
if (msg_cnt > 0) {
|
||||
data[msg_cnt++] = c;
|
||||
if (msg_cnt == 6) {
|
||||
tempsht = data[1] | (data[2] << 8);
|
||||
humidsht = data[3] | (data[4] << 8);
|
||||
for (i = 1; i < 5; i++)
|
||||
chk += data[i];
|
||||
if (data[5] == (chk & 0xFF)) {
|
||||
calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht);
|
||||
DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht);
|
||||
}
|
||||
msg_cnt = 0;
|
||||
}
|
||||
}
|
||||
else if (c == 0xFF)
|
||||
msg_cnt = 1;
|
||||
}
|
||||
|
||||
void humid_sht_uart_init(void)
|
||||
{
|
||||
}
|
||||
|
||||
void humid_sht_uart_event(void)
|
||||
{
|
||||
while (MetBuffer()) {
|
||||
uint8_t ch = MetGetch();
|
||||
humid_sht_uart_parse(ch);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2017 The Paparazzi team
|
||||
*
|
||||
* 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 modules/meteo/humid_sht_uart.h
|
||||
*
|
||||
* SHTxx sensor interface.
|
||||
*
|
||||
* This reads the values for humidity and temperature from the SHTxx sensor through an uart.
|
||||
*/
|
||||
|
||||
#ifndef HUMID_SHT_H
|
||||
#define HUMID_SHT_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#ifndef SITL
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
#define MetLinkDevice (&(MET_LINK).device)
|
||||
|
||||
#define MetBuffer() MetLinkDevice->char_available(MetLinkDevice->periph)
|
||||
#define MetGetch() MetLinkDevice->get_byte(MetLinkDevice->periph)
|
||||
#define ReadMetBuffer() { while (MetBuffer()&&!met_msg_received) parse_met_buffer(MetGetch()); }
|
||||
#define MetSend1(c) MetLinkDevice->put_byte(MetLinkDevice->periph, 0, c)
|
||||
#define MetUartSend1(c) MetSend1(c)
|
||||
#define MetSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) MetSend1(_dat[i]); };
|
||||
#define MetUartSetBaudrate(_b) uart_periph_set_baudrate(&(MET_LINK), _b)
|
||||
#define MetUartRunning (MET_LINK).tx_running
|
||||
|
||||
#endif /** !SITL */
|
||||
|
||||
extern uint16_t humidsht, tempsht;
|
||||
extern float fhumidsht, ftempsht;
|
||||
extern bool humid_sht_available;
|
||||
extern uint8_t humid_sht_status;
|
||||
|
||||
void humid_sht_uart_init(void);
|
||||
void humid_sht_uart_periodic(void);
|
||||
void humid_sht_uart_event(void);
|
||||
|
||||
#endif /* HUMID_SHT_H */
|
||||
Reference in New Issue
Block a user