mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
[vision] add computer vision library in standard modules + 2 modules
- video RTP stream - image download for ARDrone2
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,34 @@
|
|||||||
|
|
||||||
|
<airframe>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<!-- Accelero -->
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="2036"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="2063"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="2050"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="19.8215198025" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="19.3612647775" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="19.6308115492" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto calibration -->
|
||||||
|
<!--define name="MAG_X_NEUTRAL" value="56"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="17"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-64"/>
|
||||||
|
<define name="MAG_X_SENS" value="17.4978698304" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="17.7032280166" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="17.8657801029" integer="16"/-->
|
||||||
|
|
||||||
|
<define name="MAG_X_NEUTRAL" value="21"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-4"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-1"/>
|
||||||
|
<define name="MAG_X_SENS" value="16.510274159" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="16.5086412339" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="17.4503678708" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto current calibration -->
|
||||||
|
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,34 @@
|
|||||||
|
|
||||||
|
<airframe>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<!-- Accelero -->
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="2033"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="2050"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="2045"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="19.4125494887" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="19.5801376185" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="19.4761009738" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto calibration -->
|
||||||
|
<!--define name="MAG_X_NEUTRAL" value="64"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="7"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-28"/>
|
||||||
|
<define name="MAG_X_SENS" value="15.0972908192" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="14.9806854769" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="16.0049628842" integer="16"/-->
|
||||||
|
|
||||||
|
<define name="MAG_X_NEUTRAL" value="48"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-23"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-9"/>
|
||||||
|
<define name="MAG_X_SENS" value="15.3920639363" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="15.3302290802" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="16.236687484" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto current calibration -->
|
||||||
|
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
|
||||||
|
<airframe>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<!-- Accelero -->
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="2046"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="2050"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="2053"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="19.6123236866" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="19.6657552603" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="19.7736759183" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto calibration -->
|
||||||
|
<!--define name="MAG_X_NEUTRAL" value="23"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-13"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-8"/>
|
||||||
|
<define name="MAG_X_SENS" value="15.5472944024" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="15.5179439561" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="16.8234333541" integer="16"/-->
|
||||||
|
|
||||||
|
<!-- Magneto calibration -->
|
||||||
|
<define name="MAG_X_NEUTRAL" value="24"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-20"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-9"/>
|
||||||
|
<define name="MAG_X_SENS" value="15.3774561292" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="15.5082167794" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="16.5687991924" integer="16"/>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Magneto current calibration -->
|
||||||
|
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,27 @@
|
|||||||
|
|
||||||
|
<airframe>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<!-- Accelero -->
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="2061"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="2087"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="2062"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="19.8086810127" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="19.6883612768" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="19.3409944706" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto calibration -->
|
||||||
|
<define name="MAG_X_NEUTRAL" value="46"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-50"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-28"/>
|
||||||
|
<define name="MAG_X_SENS" value="16.1913174133" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="15.2287916857" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="16.5815975397" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Magneto current calibration -->
|
||||||
|
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
|
||||||
|
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,166 @@
|
|||||||
|
<!-- shared control loops gain -->
|
||||||
|
|
||||||
|
<airframe>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="THRUST" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<servos driver="Default">
|
||||||
|
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
|
||||||
|
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
|
||||||
|
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
|
||||||
|
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||||
|
<define name="TRIM_ROLL" value="0"/>
|
||||||
|
<define name="TRIM_PITCH" value="-200"/>
|
||||||
|
<define name="TRIM_YAW" value="0"/>
|
||||||
|
<define name="NB_MOTOR" value="4"/>
|
||||||
|
<define name="SCALE" value="255"/>
|
||||||
|
|
||||||
|
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
|
||||||
|
<define name="ROLL_COEF" value="{ 256, -256, -256, 256 }"/>
|
||||||
|
<define name="PITCH_COEF" value="{ 256, 256, -256, -256 }"/>
|
||||||
|
<define name="YAW_COEF" value="{ -256, 256, -256, 256 }"/>
|
||||||
|
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
||||||
|
<set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
|
||||||
|
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
|
||||||
|
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
|
||||||
|
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<section name="INS_BASE" prefix="INS_">
|
||||||
|
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||||
|
<define name="SONAR_MAX_RANGE" value="4.0"/>
|
||||||
|
<define name="SONAR_MIN_RANGE" value="0.01"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||||
|
<!-- setpoints -->
|
||||||
|
<define name="SP_MAX_P" value="10000"/>
|
||||||
|
<define name="SP_MAX_Q" value="10000"/>
|
||||||
|
<define name="SP_MAX_R" value="10000"/>
|
||||||
|
<define name="DEADBAND_P" value="20"/>
|
||||||
|
<define name="DEADBAND_Q" value="20"/>
|
||||||
|
<define name="DEADBAND_R" value="200"/>
|
||||||
|
<define name="REF_TAU" value="4"/>
|
||||||
|
|
||||||
|
<!-- 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"/>
|
||||||
|
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="DDGAIN_P" value="300"/>
|
||||||
|
<define name="DDGAIN_Q" value="300"/>
|
||||||
|
<define name="DDGAIN_R" value="300"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||||
|
<!-- setpoints -->
|
||||||
|
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||||
|
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||||
|
<define name="SP_MAX_R" value="200" unit="deg/s"/>
|
||||||
|
<define name="DEADBAND_A" value="0"/>
|
||||||
|
<define name="DEADBAND_E" value="0"/>
|
||||||
|
<define name="DEADBAND_R" value="250"/>
|
||||||
|
|
||||||
|
<!-- reference -->
|
||||||
|
<define name="REF_OMEGA_P" value="900" 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="900" 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="600" unit="deg/s"/>
|
||||||
|
<define name="REF_ZETA_R" value="0.9"/>
|
||||||
|
<define name="REF_MAX_R" value="40." unit="deg/s"/>
|
||||||
|
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
|
||||||
|
|
||||||
|
<!-- feedback -->
|
||||||
|
<define name="PHI_PGAIN" value="1100"/>
|
||||||
|
<define name="PHI_DGAIN" value="420"/>
|
||||||
|
<define name="PHI_IGAIN" value="120"/>
|
||||||
|
|
||||||
|
<define name="THETA_PGAIN" value="1100"/>
|
||||||
|
<define name="THETA_DGAIN" value="420"/>
|
||||||
|
<define name="THETA_IGAIN" value="120"/>
|
||||||
|
|
||||||
|
<define name="PSI_PGAIN" value="1000"/>
|
||||||
|
<define name="PSI_DGAIN" value="400"/>
|
||||||
|
<define name="PSI_IGAIN" value="30"/>
|
||||||
|
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="PHI_DDGAIN" value="70"/>
|
||||||
|
<define name="THETA_DDGAIN" value="70"/>
|
||||||
|
<define name="PSI_DDGAIN" value="50"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||||
|
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
|
||||||
|
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
|
||||||
|
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
|
||||||
|
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
|
||||||
|
<define name="MAX_SUM_ERR" value="2000000"/>
|
||||||
|
<define name="REF_MIN_ZDD" value="-0.8*9.81"/>
|
||||||
|
<define name="REF_MAX_ZDD" value=" 0.5*9.81"/>
|
||||||
|
<define name="REF_MIN_ZD" value="-1.5"/>
|
||||||
|
<define name="REF_MAX_ZD" value=" 1.5"/>
|
||||||
|
<define name="HOVER_KP" value="200"/>
|
||||||
|
<define name="HOVER_KD" value="100"/>
|
||||||
|
<define name="HOVER_KI" value="30"/>
|
||||||
|
<define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
|
||||||
|
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||||
|
<define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.5"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||||
|
<define name="REF_MAX_SPEED" value="3." unit="m/s"/>
|
||||||
|
<!-- Good weather -->
|
||||||
|
<define name="MAX_BANK" value="20" unit="deg"/>
|
||||||
|
<!-- Bad weather -->
|
||||||
|
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
|
||||||
|
<define name="PGAIN" value="40"/>
|
||||||
|
<define name="DGAIN" value="60"/>
|
||||||
|
<define name="IGAIN" value="20"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SIMULATOR" prefix="NPS_">
|
||||||
|
<define name="ACTUATOR_NAMES" value="{"nw_motor", "ne_motor", "se_motor", "sw_motor"}"/>
|
||||||
|
<define name="JSBSIM_MODEL" value=""simple_ardrone2""/>
|
||||||
|
<define name="SENSORS_PARAMS" value=""nps_sensors_params_ard_inv.h""/>
|
||||||
|
</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_Z_HOLD"/>
|
||||||
|
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
|
||||||
|
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
|
||||||
|
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
|
||||||
|
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,81 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="ardrone2_digit">
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<define name="DEBUG_VFF_EXTENDED"/>
|
||||||
|
<configure name="HOST" value="192.168.1.$(AC_ID)"/>
|
||||||
|
<target name="ap" board="ardrone2_raw">
|
||||||
|
<define name="USE_SONAR"/>
|
||||||
|
<define name="USE_BARO_MEDIAN_FILTER"/>
|
||||||
|
<define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
|
||||||
|
<subsystem name="telemetry" type="transparent_udp"/>
|
||||||
|
<subsystem name="radio_control" type="datalink"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<subsystem name="fdm" type="jsbsim"/>
|
||||||
|
<subsystem name="radio_control" type="ppm"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<!-- Subsystem section -->
|
||||||
|
<subsystem name="motor_mixing"/>
|
||||||
|
<subsystem name="actuators" type="ardrone2"/>
|
||||||
|
<subsystem name="imu" type="ardrone2"/>
|
||||||
|
<subsystem name="gps" type="ublox"/>
|
||||||
|
<subsystem name="stabilization" type="int_quat"/>
|
||||||
|
<!-- AHRS + INS for indoor or outdoor -->
|
||||||
|
<!--subsystem name="ahrs" type="int_cmpl_euler"/>
|
||||||
|
<subsystem name="ins" type="extended"/-->
|
||||||
|
<!-- INS for outdoor only -->
|
||||||
|
<subsystem name="ins" type="float_invariant"/>
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<modules main_freq="512">
|
||||||
|
<!--load name="gps_ubx_ucenter.xml"/-->
|
||||||
|
<load name="agl_dist.xml">
|
||||||
|
<define name="USE_SONAR"/>
|
||||||
|
</load>
|
||||||
|
<load name="image_nc_send.xml"/>
|
||||||
|
<load name="rotorcraft_cam.xml"/>
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<!-- include common control -->
|
||||||
|
<include href="conf/airframes/ENAC/quadrotor/ard2_base_control.xml"/>
|
||||||
|
<!-- include arframe calibration -->
|
||||||
|
<include href="conf/airframes/ENAC/quadrotor/ard2_$AC_ID.xml"/>
|
||||||
|
|
||||||
|
<!-- local magnetic field -->
|
||||||
|
<!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||||
|
<section name="AHRS" prefix="AHRS_">
|
||||||
|
<!-- 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"/ -->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<!-- 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.387766"/>
|
||||||
|
<define name="H_Y" value="0.00648212"/>
|
||||||
|
<define name="H_Z" value="0.921725"/ -->
|
||||||
|
<!-- trust more the baro over the gps alt -->
|
||||||
|
<define name="INV_NXZ" value="0.3"/>
|
||||||
|
<define name="INV_NH" value="2.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GCS">
|
||||||
|
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
|
||||||
|
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||||
|
<define name="ALT_SHIFT_MINUS" value="-1"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,86 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
|
||||||
|
|
||||||
|
<airframe name="ardrone2_vision">
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<define name="DEBUG_VFF_EXTENDED"/>
|
||||||
|
<configure name="HOST" value="192.168.1.$(AC_ID)"/>
|
||||||
|
<target name="ap" board="ardrone2_raw">
|
||||||
|
<define name="USE_SONAR"/>
|
||||||
|
<define name="USE_BARO_MEDIAN_FILTER"/>
|
||||||
|
<define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
|
||||||
|
<subsystem name="telemetry" type="transparent_udp"/>
|
||||||
|
<subsystem name="radio_control" type="datalink"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<target name="nps" board="pc">
|
||||||
|
<subsystem name="fdm" type="jsbsim"/>
|
||||||
|
<subsystem name="radio_control" type="ppm"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<!-- Subsystem section -->
|
||||||
|
<subsystem name="motor_mixing"/>
|
||||||
|
<subsystem name="actuators" type="ardrone2"/>
|
||||||
|
<subsystem name="imu" type="ardrone2"/>
|
||||||
|
<subsystem name="gps" type="ublox"/>
|
||||||
|
<subsystem name="stabilization" type="int_quat"/>
|
||||||
|
<!-- AHRS + INS for indoor or outdoor -->
|
||||||
|
<!--subsystem name="ahrs" type="int_cmpl_euler"/>
|
||||||
|
<subsystem name="ins" type="extended"/-->
|
||||||
|
<!-- INS for outdoor only -->
|
||||||
|
<subsystem name="ins" type="float_invariant"/>
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<modules main_freq="512">
|
||||||
|
<!--load name="gps_ubx_ucenter.xml"/-->
|
||||||
|
<load name="agl_dist.xml">
|
||||||
|
<define name="USE_SONAR"/>
|
||||||
|
</load>
|
||||||
|
<load name="video_rtp_stream.xml">
|
||||||
|
<define name="VIDEO_SOCK_OUT_OFFSET" value="$(AC_ID)"/>
|
||||||
|
<define name="VIDEO_DOWNSIZE_FACTOR" value="2"/>
|
||||||
|
<define name="VIDEO_QUALITY_FACTOR" value="80"/>
|
||||||
|
<define name="VIDEO_FPS" value="1"/>
|
||||||
|
</load>
|
||||||
|
<load name="rotorcraft_cam.xml"/>
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<!-- include common control -->
|
||||||
|
<include href="conf/airframes/ENAC/quadrotor/ard2_base_control.xml"/>
|
||||||
|
<!-- include arframe calibration -->
|
||||||
|
<include href="conf/airframes/ENAC/quadrotor/ard2_$AC_ID.xml"/>
|
||||||
|
|
||||||
|
<!-- local magnetic field -->
|
||||||
|
<!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||||
|
<section name="AHRS" prefix="AHRS_">
|
||||||
|
<!-- 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"/ -->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<!-- 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.387766"/>
|
||||||
|
<define name="H_Y" value="0.00648212"/>
|
||||||
|
<define name="H_Z" value="0.921725"/ -->
|
||||||
|
<!-- trust more the baro over the gps alt -->
|
||||||
|
<define name="INV_NXZ" value="0.3"/>
|
||||||
|
<define name="INV_NH" value="2.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GCS">
|
||||||
|
<define name="ALT_SHIFT_PLUS_PLUS" value="5"/>
|
||||||
|
<define name="ALT_SHIFT_PLUS" value="1"/>
|
||||||
|
<define name="ALT_SHIFT_MINUS" value="-1"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,34 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="image_nc_send" dir="computer_vision">
|
||||||
|
<doc>
|
||||||
|
<description>Download Images from ARDone 2 with netcat</description>
|
||||||
|
<define name="IMAGE_SOCK_IP" value="192.168.1.255" description="IP server address"/>
|
||||||
|
<define name="IMAGE_DOWNSIZE_FACTOR" value="4" description="Reduction factor"/>
|
||||||
|
<define name="IMAGE_QUALITY_FACTOR" value="50" description="JPEG encoding compression factor [0-99]"/>
|
||||||
|
<define name="IMAGE_FPS" value="4." description="Image send frame rate"/>
|
||||||
|
<define name="IMAGE_SAVE" value="0|1" description="Also save images on internal memory"/>
|
||||||
|
</doc>
|
||||||
|
|
||||||
|
<header>
|
||||||
|
<file name="image_nc_send.h"/>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<periodic fun="image_nc_send_run()" freq="1" start="image_nc_send_start()" stop="image_nc_send_stop()" autorun="FALSE"/>
|
||||||
|
<makefile target="ap">
|
||||||
|
<file name="image_nc_send.c"/>
|
||||||
|
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
|
||||||
|
<file name="video.c" dir="modules/computer_vision/lib/v4l"/>
|
||||||
|
<define name="modules/computer_vision/cv" type="include"/>
|
||||||
|
<define name="modules/computer_vision/lib" type="include"/>
|
||||||
|
<define name="pthread" type="raw"/>
|
||||||
|
<define name="__USE_GNU"/>
|
||||||
|
<flag name="LDFLAGS" value="pthread"/>
|
||||||
|
<flag name="LDFLAGS" value="lrt"/>
|
||||||
|
<flag name="LDFLAGS" value="static"/>
|
||||||
|
</makefile>
|
||||||
|
<makefile target="nps">
|
||||||
|
<file name="image_nc_send_nps.c"/>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
|
|
||||||
@@ -0,0 +1,47 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="video_rtp_stream" dir="computer_vision">
|
||||||
|
<doc>
|
||||||
|
<description>
|
||||||
|
Video stream of ARDone 2 front camera.
|
||||||
|
|
||||||
|
- sends a RTP/UDP stream of the from camera
|
||||||
|
- possibility to save an image on the internal memory (JPEG, full size, best quality, 100 pictures max if nothing else)
|
||||||
|
</description>
|
||||||
|
<define name="VIDEO_SOCK_IP" value="192.168.1.255" description="IP broadcast address"/>
|
||||||
|
<define name="VIDEO_DOWNSIZE_FACTOR" value="4" description="Reduction factor of the video stream"/>
|
||||||
|
<define name="VIDEO_QUALITY_FACTOR" value="50" description="JPEG encoding compression factor [0-99]"/>
|
||||||
|
<define name="VIDEO_FPS" value="4." description="Video stream frame rate"/>
|
||||||
|
</doc>
|
||||||
|
<settings>
|
||||||
|
<dl_settings>
|
||||||
|
<dl_settings name="video">
|
||||||
|
<dl_setting var="viewvideo_shot" min="0" step="1" max="1" shortname="save_shot" module="computer_vision/viewvideo" handler="SaveShot"/>
|
||||||
|
</dl_settings>
|
||||||
|
</dl_settings>
|
||||||
|
</settings>
|
||||||
|
|
||||||
|
<header>
|
||||||
|
<file name="viewvideo.h"/>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<periodic fun="viewvideo_run()" freq="1" start="viewvideo_start()" stop="viewvideo_stop()" autorun="TRUE"/>
|
||||||
|
<makefile target="ap">
|
||||||
|
<file name="viewvideo.c"/>
|
||||||
|
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
|
||||||
|
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
|
||||||
|
<file name="socket.c" dir="modules/computer_vision/lib/udp"/>
|
||||||
|
<file name="video.c" dir="modules/computer_vision/lib/v4l"/>
|
||||||
|
<define name="modules/computer_vision/cv" type="include"/>
|
||||||
|
<define name="modules/computer_vision/lib" type="include"/>
|
||||||
|
<define name="pthread" type="raw"/>
|
||||||
|
<define name="__USE_GNU"/>
|
||||||
|
<flag name="LDFLAGS" value="pthread"/>
|
||||||
|
<flag name="LDFLAGS" value="lrt"/>
|
||||||
|
<flag name="LDFLAGS" value="static"/>
|
||||||
|
</makefile>
|
||||||
|
<makefile target="nps">
|
||||||
|
<file name="viewvideo_nps.c"/>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
|
|
||||||
@@ -0,0 +1,93 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2013
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "image.h"
|
||||||
|
|
||||||
|
inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output);
|
||||||
|
inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output)
|
||||||
|
{
|
||||||
|
uint8_t *source = input->buf;
|
||||||
|
uint8_t *dest = output->buf;
|
||||||
|
source++;
|
||||||
|
|
||||||
|
for (int y=0;y<output->h;y++)
|
||||||
|
{
|
||||||
|
for (int x=0;x<output->w;x++)
|
||||||
|
{
|
||||||
|
// UYVY
|
||||||
|
*dest++ = 127; // U
|
||||||
|
*dest++ = *source; // Y
|
||||||
|
source+=2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
|
||||||
|
inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
|
||||||
|
{
|
||||||
|
int cnt = 0;
|
||||||
|
uint8_t *source = input->buf;
|
||||||
|
uint8_t *dest = output->buf;
|
||||||
|
|
||||||
|
for (int y=0;y<output->h;y++)
|
||||||
|
{
|
||||||
|
for (int x=0;x<output->w;x+=2)
|
||||||
|
{
|
||||||
|
// Color Check:
|
||||||
|
if (
|
||||||
|
// Light
|
||||||
|
(dest[1] >= y_m)
|
||||||
|
&& (dest[1] <= y_M)
|
||||||
|
&& (dest[0] >= u_m)
|
||||||
|
&& (dest[0] <= u_M)
|
||||||
|
&& (dest[2] >= v_m)
|
||||||
|
&& (dest[2] <= v_M)
|
||||||
|
)// && (dest[2] > 128))
|
||||||
|
{
|
||||||
|
cnt ++;
|
||||||
|
// UYVY
|
||||||
|
dest[0] = 64; // U
|
||||||
|
dest[1] = source[1]; // Y
|
||||||
|
dest[2] = 255; // V
|
||||||
|
dest[3] = source[3]; // Y
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// UYVY
|
||||||
|
char u = source[0]-127;
|
||||||
|
u/=4;
|
||||||
|
dest[0] = 127; // U
|
||||||
|
dest[1] = source[1]; // Y
|
||||||
|
u = source[2]-127;
|
||||||
|
u/=4;
|
||||||
|
dest[2] = 127; // V
|
||||||
|
dest[3] = source[3]; // Y
|
||||||
|
}
|
||||||
|
|
||||||
|
dest+=4;
|
||||||
|
source+=4;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return cnt;
|
||||||
|
}
|
||||||
|
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,45 @@
|
|||||||
|
|
||||||
|
#ifndef __MY_JPEG_HEADER__
|
||||||
|
#define __MY_JPEG_HEADER__
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define FOUR_ZERO_ZERO 0
|
||||||
|
#define FOUR_TWO_ZERO 1
|
||||||
|
#define FOUR_TWO_TWO 2
|
||||||
|
#define FOUR_FOUR_FOUR 3
|
||||||
|
#define RGB 4
|
||||||
|
|
||||||
|
unsigned char *encode_image (
|
||||||
|
uint8_t* in,
|
||||||
|
uint8_t* out,
|
||||||
|
uint32_t q, // image quality 1-8
|
||||||
|
uint32_t fmt, // image format code
|
||||||
|
uint32_t width, // image width
|
||||||
|
uint32_t height, // image height
|
||||||
|
uint8_t add_dri_header // data only or full jpeg file
|
||||||
|
);
|
||||||
|
|
||||||
|
unsigned char *encode_image_rtp (
|
||||||
|
uint8_t* in,
|
||||||
|
uint8_t* out,
|
||||||
|
uint32_t q, // image quality 1-8
|
||||||
|
uint32_t fmt, // image format code
|
||||||
|
uint32_t width, // image width
|
||||||
|
uint32_t height, // image height
|
||||||
|
uint8_t add_dri_header // data only or full jpeg file
|
||||||
|
);
|
||||||
|
|
||||||
|
unsigned char *encode_image_std_qt (
|
||||||
|
uint8_t* in,
|
||||||
|
uint8_t* out,
|
||||||
|
uint32_t q, // image quality 0 to 99
|
||||||
|
uint32_t fmt, // image format code
|
||||||
|
uint32_t width, // image width
|
||||||
|
uint32_t height, // image height
|
||||||
|
uint8_t add_dri_header // data only or full jpeg file
|
||||||
|
);
|
||||||
|
|
||||||
|
int create_svs_jpeg_header(unsigned char* buf, int32_t size, int w);
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,196 @@
|
|||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
#include "udp/socket.h"
|
||||||
|
|
||||||
|
#include "rtp.h"
|
||||||
|
|
||||||
|
void send_rtp_packet(struct UdpSocket *sock, uint8_t* Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header);
|
||||||
|
|
||||||
|
// http://www.ietf.org/rfc/rfc3550.txt
|
||||||
|
|
||||||
|
#define KJpegCh1ScanDataLen 32
|
||||||
|
#define KJpegCh2ScanDataLen 56
|
||||||
|
|
||||||
|
// RGB JPEG images as RTP payload - 64x48 pixel
|
||||||
|
uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen] =
|
||||||
|
{
|
||||||
|
0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd,
|
||||||
|
0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
|
||||||
|
};
|
||||||
|
uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] =
|
||||||
|
{
|
||||||
|
0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc,
|
||||||
|
0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2,
|
||||||
|
0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void test_rtp_frame(struct UdpSocket *sock)
|
||||||
|
{
|
||||||
|
static uint32_t framecounter = 0;
|
||||||
|
static uint32_t timecounter = 0;
|
||||||
|
static uint8_t toggle = 0;
|
||||||
|
toggle = ! toggle;
|
||||||
|
|
||||||
|
uint8_t format_code = 0x01;
|
||||||
|
uint8_t quality_code = 0x54;
|
||||||
|
|
||||||
|
if (toggle)
|
||||||
|
{
|
||||||
|
send_rtp_packet(sock, JpegScanDataCh2A,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
send_rtp_packet(sock, JpegScanDataCh2B,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
|
||||||
|
}
|
||||||
|
framecounter++;
|
||||||
|
timecounter+=3600;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
|
||||||
|
{
|
||||||
|
static uint32_t packetcounter = 0;
|
||||||
|
static uint32_t timecounter = 0;
|
||||||
|
uint32_t offset = 0;
|
||||||
|
|
||||||
|
#define MAX_PACKET_SIZE 1400
|
||||||
|
|
||||||
|
if (delta_t <= 0)
|
||||||
|
{
|
||||||
|
struct timeval tv;
|
||||||
|
gettimeofday(&tv, 0);
|
||||||
|
uint32_t t = (tv.tv_sec % (256*256)) * 90000 + tv.tv_usec * 9 / 100;
|
||||||
|
timecounter = t;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Split frame into packets
|
||||||
|
for (;JpegLen > 0;)
|
||||||
|
{
|
||||||
|
uint32_t len = MAX_PACKET_SIZE;
|
||||||
|
uint8_t lastpacket = 0;
|
||||||
|
|
||||||
|
if (JpegLen <= len)
|
||||||
|
{
|
||||||
|
lastpacket = 1;
|
||||||
|
len = JpegLen;
|
||||||
|
}
|
||||||
|
|
||||||
|
send_rtp_packet(sock, Jpeg,len,packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
|
||||||
|
|
||||||
|
JpegLen -= len;
|
||||||
|
Jpeg += len;
|
||||||
|
offset += len;
|
||||||
|
packetcounter++;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (delta_t > 0)
|
||||||
|
{
|
||||||
|
// timestamp = 1 / 90 000 seconds
|
||||||
|
timecounter+=delta_t;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* The RTP timestamp is in units of 90000Hz. The same timestamp MUST
|
||||||
|
appear in each fragment of a given frame. The RTP marker bit MUST be
|
||||||
|
set in the last packet of a frame.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
void send_rtp_packet(
|
||||||
|
struct UdpSocket *sock,
|
||||||
|
uint8_t * Jpeg, int JpegLen,
|
||||||
|
uint32_t m_SequenceNumber, uint32_t m_Timestamp,
|
||||||
|
uint32_t m_offset, uint8_t marker_bit,
|
||||||
|
int w, int h,
|
||||||
|
uint8_t format_code, uint8_t quality_code,
|
||||||
|
uint8_t has_dri_header)
|
||||||
|
{
|
||||||
|
|
||||||
|
#define KRtpHeaderSize 12 // size of the RTP header
|
||||||
|
#define KJpegHeaderSize 8 // size of the special JPEG payload header
|
||||||
|
|
||||||
|
uint8_t RtpBuf[2048];
|
||||||
|
int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize;
|
||||||
|
|
||||||
|
memset(RtpBuf,0x00,sizeof(RtpBuf));
|
||||||
|
|
||||||
|
/*
|
||||||
|
The RTP header has the following format:
|
||||||
|
|
||||||
|
0 1 2 3
|
||||||
|
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
|V=2|P|X| CC |M| PT | sequence number |
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
| timestamp |
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
| synchronization source (SSRC) identifier |
|
||||||
|
+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+=+
|
||||||
|
| contributing source (CSRC) identifiers |
|
||||||
|
| .... |
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
* */
|
||||||
|
|
||||||
|
// Prepare the 12 byte RTP header
|
||||||
|
RtpBuf[0] = 0x80; // RTP version
|
||||||
|
RtpBuf[1] = 0x1a + (marker_bit<<7); // JPEG payload (26) and marker bit
|
||||||
|
RtpBuf[2] = m_SequenceNumber >> 8;
|
||||||
|
RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter
|
||||||
|
RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp
|
||||||
|
RtpBuf[5] = (m_Timestamp & 0x00FF0000) >> 16;
|
||||||
|
RtpBuf[6] = (m_Timestamp & 0x0000FF00) >> 8;
|
||||||
|
RtpBuf[7] = (m_Timestamp & 0x000000FF);
|
||||||
|
RtpBuf[8] = 0x13; // 4 byte SSRC (sychronization source identifier)
|
||||||
|
RtpBuf[9] = 0xf9; // we just an arbitrary number here to keep it simple
|
||||||
|
RtpBuf[10] = 0x7e;
|
||||||
|
RtpBuf[11] = 0x67;
|
||||||
|
|
||||||
|
/* JPEG header", are as follows:
|
||||||
|
*
|
||||||
|
* http://tools.ietf.org/html/rfc2435
|
||||||
|
|
||||||
|
0 1 2 3
|
||||||
|
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
| Type-specific | Fragment Offset |
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
| Type | Q | Width | Height |
|
||||||
|
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Prepare the 8 byte payload JPEG header
|
||||||
|
RtpBuf[12] = 0x00; // type specific
|
||||||
|
RtpBuf[13] = (m_offset & 0x00FF0000) >> 16; // 3 byte fragmentation offset for fragmented images
|
||||||
|
RtpBuf[14] = (m_offset & 0x0000FF00) >> 8;
|
||||||
|
RtpBuf[15] = (m_offset & 0x000000FF);
|
||||||
|
RtpBuf[16] = 0x00; // type: 0 422 or 1 421
|
||||||
|
RtpBuf[17] = 60; // quality scale factor
|
||||||
|
RtpBuf[16] = format_code; // type: 0 422 or 1 421
|
||||||
|
if (has_dri_header)
|
||||||
|
RtpBuf[16] |= 0x40; // DRI flag
|
||||||
|
RtpBuf[17] = quality_code; // quality scale factor
|
||||||
|
RtpBuf[18] = w/8; // width / 8 -> 48 pixel
|
||||||
|
RtpBuf[19] = h/8; // height / 8 -> 32 pixel
|
||||||
|
// append the JPEG scan data to the RTP buffer
|
||||||
|
memcpy(&RtpBuf[20],Jpeg,JpegLen);
|
||||||
|
|
||||||
|
udp_write(sock,RtpBuf,RtpPacketSize);
|
||||||
|
};
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "udp/socket.h"
|
||||||
|
|
||||||
|
void send_rtp_frame(
|
||||||
|
struct UdpSocket *sock, // socket
|
||||||
|
uint8_t * Jpeg, uint32_t JpegLen, // jpeg data
|
||||||
|
int w, int h, // width and height
|
||||||
|
uint8_t format_code, // 0=422, 1=421
|
||||||
|
uint8_t quality_code, // 0-99 of 128 for custom (include
|
||||||
|
uint8_t has_dri_header, // Does Jpeg data include Header Info?
|
||||||
|
uint32_t delta_t // time step 90kHz
|
||||||
|
);
|
||||||
|
|
||||||
|
void test_rtp_frame(struct UdpSocket *sock);
|
||||||
@@ -0,0 +1,35 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2013
|
||||||
|
*
|
||||||
|
* 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 _MY_IMAGE_HEADER_
|
||||||
|
#define _MY_IMAGE_HEADER_
|
||||||
|
|
||||||
|
|
||||||
|
struct img_struct {
|
||||||
|
int seq;
|
||||||
|
double timestamp;
|
||||||
|
unsigned char *buf;
|
||||||
|
int w;
|
||||||
|
int h;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2013
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "image.h"
|
||||||
|
|
||||||
|
inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample);
|
||||||
|
inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample)
|
||||||
|
{
|
||||||
|
uint8_t *source = input->buf;
|
||||||
|
uint8_t *dest = output->buf;
|
||||||
|
|
||||||
|
int pixelskip = downsample-1;
|
||||||
|
for (int y=0;y<output->h;y++)
|
||||||
|
{
|
||||||
|
for (int x=0;x<output->w;x+=2)
|
||||||
|
{
|
||||||
|
// YUYV
|
||||||
|
*dest++ = *source++; // U
|
||||||
|
*dest++ = *source++; // Y
|
||||||
|
// now skip 3 pixels
|
||||||
|
if (pixelskip) source+=(pixelskip+1)*2;
|
||||||
|
*dest++ = *source++; // U
|
||||||
|
*dest++ = *source++; // V
|
||||||
|
if (pixelskip) source+=(pixelskip-1)*2;
|
||||||
|
}
|
||||||
|
// skip 3 rows
|
||||||
|
if (pixelskip) source += pixelskip * input->w * 2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,208 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||||
|
*
|
||||||
|
* This file is part of Paparazzi.
|
||||||
|
*
|
||||||
|
* Paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* Paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/computer_vision/image_nc_send.c
|
||||||
|
*
|
||||||
|
* Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Own header
|
||||||
|
#include "modules/computer_vision/image_nc_send.h"
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/wait.h>
|
||||||
|
|
||||||
|
// Video
|
||||||
|
#include "modules/computer_vision/lib/v4l/video.h"
|
||||||
|
#include "modules/computer_vision/cv/resize.h"
|
||||||
|
#include "modules/computer_vision/cv/encoding/jpeg.h"
|
||||||
|
|
||||||
|
// Threaded computer vision
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
|
// Default netcat server IP (destination)
|
||||||
|
#ifndef IMAGE_SERVER_IP
|
||||||
|
#define IMAGE_SERVER_IP "192.168.1.2"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Default netcat port
|
||||||
|
#ifndef IMAGE_SERVER_PORT
|
||||||
|
#define IMAGE_SERVER_PORT 4900
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Default downsize factor
|
||||||
|
#ifndef IMAGE_DOWNSIZE_FACTOR
|
||||||
|
#define IMAGE_DOWNSIZE_FACTOR 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// JPEG compression quality factor from 0 to 99 (99=high)
|
||||||
|
#ifndef IMAGE_QUALITY_FACTOR
|
||||||
|
#define IMAGE_QUALITY_FACTOR 99
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Frame Per Second
|
||||||
|
#ifndef IMAGE_FPS
|
||||||
|
#define IMAGE_FPS (1./6.)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Save images by default
|
||||||
|
#ifndef IMAGE_SAVE
|
||||||
|
#define IMAGE_SAVE 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void image_nc_send_run(void) {}
|
||||||
|
|
||||||
|
|
||||||
|
/////////////////////////////////////////////////////////////////////////
|
||||||
|
// COMPUTER VISION THREAD
|
||||||
|
|
||||||
|
pthread_t computervision_thread;
|
||||||
|
volatile uint8_t computervision_thread_status = 0;
|
||||||
|
volatile uint8_t computer_vision_thread_command = 0;
|
||||||
|
void *computervision_thread_main(void* data);
|
||||||
|
void *computervision_thread_main(void* data)
|
||||||
|
{
|
||||||
|
// Video Input
|
||||||
|
struct vid_struct vid;
|
||||||
|
vid.device = (char*)"/dev/video1";
|
||||||
|
vid.w=1280;
|
||||||
|
vid.h=720;
|
||||||
|
vid.n_buffers = 4;
|
||||||
|
if (video_init(&vid)<0) {
|
||||||
|
printf("Error initialising video\n");
|
||||||
|
computervision_thread_status = -1;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Frame Grabbing
|
||||||
|
struct img_struct* img_new = video_create_image(&vid);
|
||||||
|
|
||||||
|
// Frame Resizing
|
||||||
|
uint8_t quality_factor = IMAGE_QUALITY_FACTOR;
|
||||||
|
uint8_t dri_jpeg_header = 1;
|
||||||
|
|
||||||
|
struct img_struct small;
|
||||||
|
small.w = vid.w / IMAGE_DOWNSIZE_FACTOR;
|
||||||
|
small.h = vid.h / IMAGE_DOWNSIZE_FACTOR;
|
||||||
|
small.buf = (uint8_t*)malloc(small.w*small.h*2);
|
||||||
|
|
||||||
|
// Commpressed image buffer
|
||||||
|
uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
|
||||||
|
|
||||||
|
// file index (search from 0)
|
||||||
|
int file_index = 0;
|
||||||
|
|
||||||
|
int microsleep = (int)(1000000. / IMAGE_FPS);
|
||||||
|
|
||||||
|
while (computer_vision_thread_command > 0)
|
||||||
|
{
|
||||||
|
usleep(microsleep);
|
||||||
|
video_grab_image(&vid, img_new);
|
||||||
|
|
||||||
|
// Resize
|
||||||
|
resize_uyuv(img_new, &small, IMAGE_DOWNSIZE_FACTOR);
|
||||||
|
|
||||||
|
// JPEG encode the image:
|
||||||
|
uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
|
||||||
|
uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header);
|
||||||
|
uint32_t size = end-(jpegbuf);
|
||||||
|
|
||||||
|
#if IMAGE_SAVE
|
||||||
|
FILE* save;
|
||||||
|
char save_name[128];
|
||||||
|
if (system("mkdir -p /data/video/images") == 0) {
|
||||||
|
// search available index (max is 99)
|
||||||
|
for ( ; file_index < 99; file_index++) {
|
||||||
|
printf("search %d\n",file_index);
|
||||||
|
sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
|
||||||
|
// test if file exists or not
|
||||||
|
if (access(save_name, F_OK) == -1) {
|
||||||
|
printf("access\n");
|
||||||
|
save = fopen(save_name, "w");
|
||||||
|
if (save != NULL) {
|
||||||
|
fwrite(jpegbuf, sizeof(uint8_t), size, save);
|
||||||
|
fclose(save);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("Error when opening file %s\n", save_name);
|
||||||
|
}
|
||||||
|
// leave for loop
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else {printf("file exists\n");}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Fork process
|
||||||
|
int status;
|
||||||
|
pid_t pid = fork();
|
||||||
|
|
||||||
|
if (pid == 0) {
|
||||||
|
// Open process to send using netcat in child process
|
||||||
|
char nc_cmd[64];
|
||||||
|
sprintf(nc_cmd, "nc %s %d", IMAGE_SERVER_IP, IMAGE_SERVER_PORT);
|
||||||
|
FILE* netcat;
|
||||||
|
netcat = popen(nc_cmd, "w");
|
||||||
|
if (netcat != NULL) {
|
||||||
|
fwrite(jpegbuf, sizeof(uint8_t), size, netcat);
|
||||||
|
if (pclose(netcat) == 0) {
|
||||||
|
printf("Sending image succesfully\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("Fail sending image\n");
|
||||||
|
}
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
else if (pid < 0) {
|
||||||
|
printf("Fork failed\n");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Parent is waiting for child to terminate
|
||||||
|
wait(&status);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
printf("Thread Closed\n");
|
||||||
|
video_close(&vid);
|
||||||
|
computervision_thread_status = -100;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void image_nc_send_start(void)
|
||||||
|
{
|
||||||
|
computer_vision_thread_command = 1;
|
||||||
|
int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
|
||||||
|
if(rc) {
|
||||||
|
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void image_nc_send_stop(void)
|
||||||
|
{
|
||||||
|
computer_vision_thread_command = 0;
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,37 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||||
|
*
|
||||||
|
* This file is part of Paparazzi.
|
||||||
|
*
|
||||||
|
* Paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* Paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/computer_vision/image_nc_send.h
|
||||||
|
*
|
||||||
|
* Capture an image on an ARDrone2 and send it to the ground with netcat (nc)
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IMAGE_NC_SEND_H
|
||||||
|
#define IMAGE_NC_SEND_H
|
||||||
|
|
||||||
|
// Module functions
|
||||||
|
extern void image_nc_send_run(void);
|
||||||
|
extern void image_nc_send_start(void);
|
||||||
|
extern void image_nc_send_stop(void);
|
||||||
|
|
||||||
|
#endif /* IMAGE_NC_SEND_H */
|
||||||
|
|
||||||
@@ -0,0 +1,31 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2014 Gautier Hattenberger
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
// Own header
|
||||||
|
#include "image_capture.h"
|
||||||
|
|
||||||
|
void image_capture_run(void) {}
|
||||||
|
|
||||||
|
void image_capture_start(void) {}
|
||||||
|
|
||||||
|
void image_capture_stop(void) {}
|
||||||
|
|
||||||
@@ -0,0 +1,103 @@
|
|||||||
|
#include "socket.h"
|
||||||
|
|
||||||
|
#include <netdb.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <strings.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <netdb.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <netinet/in.h>
|
||||||
|
|
||||||
|
# define ADDR_SIZE_TYPE socklen_t
|
||||||
|
# define SOCKET_ERROR -1
|
||||||
|
# define IO_SOCKET ioctl
|
||||||
|
|
||||||
|
struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast)
|
||||||
|
{
|
||||||
|
|
||||||
|
struct UdpSocket* me = malloc(sizeof(struct UdpSocket));
|
||||||
|
|
||||||
|
int so_reuseaddr = 1;
|
||||||
|
me->socket_out = socket( AF_INET, SOCK_DGRAM, 0);
|
||||||
|
setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR,
|
||||||
|
&so_reuseaddr, sizeof(so_reuseaddr));
|
||||||
|
|
||||||
|
/* only set broadcast option if explicitly enabled */
|
||||||
|
if (broadcast)
|
||||||
|
setsockopt(me->socket_out, SOL_SOCKET, SO_BROADCAST,
|
||||||
|
&broadcast, sizeof(broadcast));
|
||||||
|
|
||||||
|
me->addr_out.sin_family = AF_INET;
|
||||||
|
me->addr_out.sin_port = htons(port_out);
|
||||||
|
me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out);
|
||||||
|
|
||||||
|
me->socket_in = socket( AF_INET, SOCK_DGRAM, 0);
|
||||||
|
setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR,
|
||||||
|
&so_reuseaddr, sizeof(so_reuseaddr));
|
||||||
|
|
||||||
|
me->addr_in.sin_family = AF_INET;
|
||||||
|
me->addr_in.sin_port = htons(port_in);
|
||||||
|
me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||||
|
|
||||||
|
bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in));
|
||||||
|
|
||||||
|
return me;
|
||||||
|
}
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
//#define UDP_MODE MSG_DONTWAIT
|
||||||
|
#define UDP_MODE 0
|
||||||
|
|
||||||
|
int udp_write(struct UdpSocket* me, unsigned char* buf, int len) {
|
||||||
|
sendto(me->socket_out, buf, len, UDP_MODE,
|
||||||
|
(struct sockaddr*)&me->addr_out, sizeof(me->addr_out));
|
||||||
|
//printf("sendto ret=%d\n",ret);
|
||||||
|
return len;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned long MIN(unsigned long a, unsigned long b);
|
||||||
|
unsigned long MIN(unsigned long a, unsigned long b)
|
||||||
|
{
|
||||||
|
if (a<b) return a;
|
||||||
|
return b;
|
||||||
|
}
|
||||||
|
|
||||||
|
int udp_read(struct UdpSocket* me, unsigned char* buf, int len)
|
||||||
|
{
|
||||||
|
unsigned long toread = 0;
|
||||||
|
int btr = 1; // set to >0 in order to start the reading loop
|
||||||
|
int newbytes = 0;
|
||||||
|
|
||||||
|
int status;
|
||||||
|
|
||||||
|
// if socket is connected
|
||||||
|
for (;btr>0;)
|
||||||
|
{
|
||||||
|
// Check Status
|
||||||
|
status = IO_SOCKET(me->socket_in, FIONREAD, &toread);
|
||||||
|
if(status == SOCKET_ERROR) {
|
||||||
|
printf("problem receiving from socket\n");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//printf("UDP has %d bytes\n", toread);
|
||||||
|
if (toread <= 0)
|
||||||
|
break;
|
||||||
|
|
||||||
|
// If status: ok and new data: read it
|
||||||
|
btr = MIN(toread,(unsigned long)len);
|
||||||
|
recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr*)&me->addr_in, (socklen_t *) sizeof(me->addr_in) );
|
||||||
|
newbytes += btr;
|
||||||
|
}
|
||||||
|
return newbytes;
|
||||||
|
}
|
||||||
@@ -0,0 +1,25 @@
|
|||||||
|
#ifndef SOCKET_H
|
||||||
|
#define SOCKET_H
|
||||||
|
|
||||||
|
|
||||||
|
#include <sys/socket.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
|
||||||
|
#define FMS_UNICAST 0
|
||||||
|
#define FMS_BROADCAST 1
|
||||||
|
|
||||||
|
struct UdpSocket {
|
||||||
|
int socket_in;
|
||||||
|
int socket_out;
|
||||||
|
struct sockaddr_in addr_in;
|
||||||
|
struct sockaddr_in addr_out;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
extern struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast);
|
||||||
|
extern int udp_write(struct UdpSocket* me, unsigned char* buf, int len);
|
||||||
|
extern int udp_read(struct UdpSocket* me, unsigned char* buf, int len);
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* SOCKET_H */
|
||||||
|
|
||||||
@@ -0,0 +1,233 @@
|
|||||||
|
/*
|
||||||
|
video.c - video driver
|
||||||
|
|
||||||
|
Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
|
||||||
|
|
||||||
|
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 2 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, write to the Free Software
|
||||||
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||||
|
MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <assert.h>
|
||||||
|
#include <getopt.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <malloc.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
#include <asm/types.h>
|
||||||
|
#include <linux/videodev2.h>
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
|
#include "video.h"
|
||||||
|
|
||||||
|
#define CLEAR(x) memset (&(x), 0, sizeof (x))
|
||||||
|
|
||||||
|
|
||||||
|
pthread_t video_thread;
|
||||||
|
void *video_thread_main(void* data);
|
||||||
|
void *video_thread_main(void* data)
|
||||||
|
{
|
||||||
|
struct vid_struct* vid = (struct vid_struct*)data;
|
||||||
|
printf("video_thread_main started\n");
|
||||||
|
while (1) {
|
||||||
|
fd_set fds;
|
||||||
|
struct timeval tv;
|
||||||
|
int r;
|
||||||
|
|
||||||
|
FD_ZERO(&fds);
|
||||||
|
FD_SET(vid->fd, &fds);
|
||||||
|
|
||||||
|
tv.tv_sec = 2;
|
||||||
|
tv.tv_usec = 0;
|
||||||
|
r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
|
||||||
|
|
||||||
|
if (-1 == r) {
|
||||||
|
if (EINTR == errno) continue;
|
||||||
|
printf("select err\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (0 == r) {
|
||||||
|
fprintf(stderr, "select timeout\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct v4l2_buffer buf;
|
||||||
|
|
||||||
|
CLEAR(buf);
|
||||||
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
buf.memory = V4L2_MEMORY_MMAP;
|
||||||
|
if (ioctl(vid->fd, VIDIOC_DQBUF, &buf) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_DQBUF failed.\n");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
assert(buf.index < vid->n_buffers);
|
||||||
|
|
||||||
|
vid->seq++;
|
||||||
|
|
||||||
|
if(vid->trigger) {
|
||||||
|
|
||||||
|
// todo add timestamp again
|
||||||
|
//vid->img->timestamp = util_timestamp();
|
||||||
|
vid->img->seq = vid->seq;
|
||||||
|
memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2);
|
||||||
|
vid->trigger=0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_QBUF failed.\n");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int video_init(struct vid_struct *vid)
|
||||||
|
{
|
||||||
|
struct v4l2_capability cap;
|
||||||
|
struct v4l2_format fmt;
|
||||||
|
unsigned int i;
|
||||||
|
enum v4l2_buf_type type;
|
||||||
|
|
||||||
|
vid->seq=0;
|
||||||
|
vid->trigger=0;
|
||||||
|
if(vid->n_buffers==0) vid->n_buffers=4;
|
||||||
|
|
||||||
|
vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0);
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_QUERYCAP, &cap) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_QUERYCAP failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//printf("2 driver = %s, card = %s, version = %d, capabilities = 0x%x\n", cap.driver, cap.card, cap.version, cap.capabilities);
|
||||||
|
|
||||||
|
CLEAR(fmt);
|
||||||
|
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
fmt.fmt.pix.width = vid->w;
|
||||||
|
fmt.fmt.pix.height = vid->h;
|
||||||
|
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_S_FMT, &fmt) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_S_FMT failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//image_size = fmt.fmt.pix.width * fmt.fmt.pix.height *3/2;
|
||||||
|
|
||||||
|
struct v4l2_requestbuffers req;
|
||||||
|
|
||||||
|
CLEAR(req);
|
||||||
|
req.count = vid->n_buffers;
|
||||||
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
req.memory = V4L2_MEMORY_MMAP;
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_REQBUFS, &req) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_REQBUFS failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("Buffer count = %d\n", vid->n_buffers);
|
||||||
|
|
||||||
|
vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
|
||||||
|
|
||||||
|
for (i = 0; i < vid->n_buffers; ++i) {
|
||||||
|
struct v4l2_buffer buf;
|
||||||
|
|
||||||
|
CLEAR(buf);
|
||||||
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
buf.memory = V4L2_MEMORY_MMAP;
|
||||||
|
buf.index = i;
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_QUERYBUF, &buf) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_QUERYBUF failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
vid->buffers[i].length = buf.length;
|
||||||
|
printf("buffer%d.length=%d\n",i,buf.length);
|
||||||
|
vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
|
||||||
|
|
||||||
|
if (MAP_FAILED == vid->buffers[i].buf) {
|
||||||
|
printf ("mmap() failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (i = 0; i < vid->n_buffers; ++i) {
|
||||||
|
struct v4l2_buffer buf;
|
||||||
|
|
||||||
|
CLEAR(buf);
|
||||||
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
buf.memory = V4L2_MEMORY_MMAP;
|
||||||
|
buf.index = i;
|
||||||
|
|
||||||
|
if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
|
||||||
|
printf("ioctl() VIDIOC_QBUF failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||||
|
if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
|
||||||
|
printf("ioctl() VIDIOC_STREAMON failed.\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//start video thread
|
||||||
|
int rc = pthread_create(&video_thread, NULL, video_thread_main, vid);
|
||||||
|
if(rc) {
|
||||||
|
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
|
||||||
|
return 202;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void video_close(struct vid_struct *vid)
|
||||||
|
{
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < (int)vid->n_buffers; ++i) {
|
||||||
|
if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n");
|
||||||
|
}
|
||||||
|
close(vid->fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
struct img_struct *video_create_image(struct vid_struct *vid)
|
||||||
|
{
|
||||||
|
struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct));
|
||||||
|
img->w=vid->w;
|
||||||
|
img->h=vid->h;
|
||||||
|
img->buf = (unsigned char*)malloc(vid->h*vid->w*2);
|
||||||
|
return img;
|
||||||
|
}
|
||||||
|
|
||||||
|
pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||||
|
|
||||||
|
void video_grab_image(struct vid_struct *vid, struct img_struct *img) {
|
||||||
|
pthread_mutex_lock(&video_grab_mutex);
|
||||||
|
vid->img = img;
|
||||||
|
vid->trigger=1;
|
||||||
|
// while(vid->trigger) pthread_yield();
|
||||||
|
while(vid->trigger) usleep(1);
|
||||||
|
pthread_mutex_unlock(&video_grab_mutex);
|
||||||
|
}
|
||||||
@@ -0,0 +1,53 @@
|
|||||||
|
/*
|
||||||
|
video.h - video driver
|
||||||
|
|
||||||
|
Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
|
||||||
|
|
||||||
|
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 2 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, write to the Free Software
|
||||||
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||||
|
MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef _VIDEO_H
|
||||||
|
#define _VIDEO_H
|
||||||
|
|
||||||
|
#include "../../cv/image.h"
|
||||||
|
|
||||||
|
struct buffer_struct {
|
||||||
|
void * buf;
|
||||||
|
size_t length;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct vid_struct {
|
||||||
|
char *device;
|
||||||
|
int w;
|
||||||
|
int h;
|
||||||
|
int seq;
|
||||||
|
unsigned int n_buffers;
|
||||||
|
|
||||||
|
//private members
|
||||||
|
int trigger;
|
||||||
|
struct img_struct *img;
|
||||||
|
struct buffer_struct * buffers;
|
||||||
|
int fd;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
int video_init(struct vid_struct *vid);
|
||||||
|
struct img_struct *video_create_image(struct vid_struct *vid);
|
||||||
|
|
||||||
|
void video_grab_image(struct vid_struct *vid, struct img_struct *img);
|
||||||
|
void video_close(struct vid_struct *vid);
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,251 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2014 The Paparazzi Community
|
||||||
|
*
|
||||||
|
* This file is part of Paparazzi.
|
||||||
|
*
|
||||||
|
* Paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* Paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/computer_vision/viewvideo.c
|
||||||
|
*
|
||||||
|
* Get live images from a RTP/UDP stream
|
||||||
|
* and save pictures on internal memory
|
||||||
|
*
|
||||||
|
* Works on Linux platforms
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Own header
|
||||||
|
#include "modules/computer_vision/viewvideo.h"
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
// UDP RTP Images
|
||||||
|
#include "modules/computer_vision/lib/udp/socket.h"
|
||||||
|
// Video
|
||||||
|
#include "modules/computer_vision/lib/v4l/video.h"
|
||||||
|
#include "modules/computer_vision/cv/resize.h"
|
||||||
|
#include "modules/computer_vision/cv/encoding/jpeg.h"
|
||||||
|
#include "modules/computer_vision/cv/encoding/rtp.h"
|
||||||
|
|
||||||
|
// Threaded computer vision
|
||||||
|
#include <pthread.h>
|
||||||
|
|
||||||
|
// Default broadcast IP
|
||||||
|
#ifndef VIDEO_SOCK_IP
|
||||||
|
#define VIDEO_SOCK_IP "192.168.1.255"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Output socket can be defined from an offset
|
||||||
|
#ifdef VIDEO_SOCK_OUT_OFFSET
|
||||||
|
#define VIDEO_SOCK_OUT (5000+VIDEO_SOCK_OUT_OFFSET)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef VIDEO_SOCK_OUT
|
||||||
|
#define VIDEO_SOCK_OUT 5000
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef VIDEO_SOCK_IN
|
||||||
|
#define VIDEO_SOCK_IN 4999
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Downsize factor for video stream
|
||||||
|
#ifndef VIDEO_DOWNSIZE_FACTOR
|
||||||
|
#define VIDEO_DOWNSIZE_FACTOR 4
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// From 0 to 99 (99=high)
|
||||||
|
#ifndef VIDEO_QUALITY_FACTOR
|
||||||
|
#define VIDEO_QUALITY_FACTOR 50
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Frame Per Seconds
|
||||||
|
#ifndef VIDEO_FPS
|
||||||
|
#define VIDEO_FPS 4.
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void viewvideo_run(void) {}
|
||||||
|
|
||||||
|
// take shot flag
|
||||||
|
int viewvideo_shot = 0;
|
||||||
|
|
||||||
|
/////////////////////////////////////////////////////////////////////////
|
||||||
|
// COMPUTER VISION THREAD
|
||||||
|
|
||||||
|
pthread_t computervision_thread;
|
||||||
|
volatile uint8_t computervision_thread_status = 0;
|
||||||
|
volatile uint8_t computer_vision_thread_command = 0;
|
||||||
|
void *computervision_thread_main(void* data);
|
||||||
|
void *computervision_thread_main(void* data)
|
||||||
|
{
|
||||||
|
// Video Input
|
||||||
|
struct vid_struct vid;
|
||||||
|
vid.device = (char*)"/dev/video1";
|
||||||
|
vid.w=1280;
|
||||||
|
vid.h=720;
|
||||||
|
vid.n_buffers = 4;
|
||||||
|
if (video_init(&vid)<0) {
|
||||||
|
printf("Error initialising video\n");
|
||||||
|
computervision_thread_status = -1;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Video Grabbing
|
||||||
|
struct img_struct* img_new = video_create_image(&vid);
|
||||||
|
|
||||||
|
// Video Resizing
|
||||||
|
uint8_t quality_factor = VIDEO_QUALITY_FACTOR;
|
||||||
|
uint8_t dri_jpeg_header = 0;
|
||||||
|
int microsleep = (int)(1000000. / VIDEO_FPS);
|
||||||
|
|
||||||
|
struct img_struct small;
|
||||||
|
small.w = vid.w / VIDEO_DOWNSIZE_FACTOR;
|
||||||
|
small.h = vid.h / VIDEO_DOWNSIZE_FACTOR;
|
||||||
|
small.buf = (uint8_t*)malloc(small.w*small.h*2);
|
||||||
|
|
||||||
|
// Video Compression
|
||||||
|
uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
|
||||||
|
|
||||||
|
// Network Transmit
|
||||||
|
struct UdpSocket* vsock;
|
||||||
|
vsock = udp_socket(VIDEO_SOCK_IP, VIDEO_SOCK_OUT, VIDEO_SOCK_IN, FMS_BROADCAST);
|
||||||
|
|
||||||
|
// Create SPD file and make folder if necessary
|
||||||
|
FILE* sdp;
|
||||||
|
if (system("mkdir -p /data/video/sdp") == 0) {
|
||||||
|
sdp = fopen("/data/video/sdp/x86_config-mjpeg.sdp","w");
|
||||||
|
if (sdp != NULL) {
|
||||||
|
fprintf(sdp, "v=0\n");
|
||||||
|
fprintf(sdp, "m=video %d RTP/AVP 26\n", (int)(VIDEO_SOCK_OUT));
|
||||||
|
fprintf(sdp, "c=IN IP4 0.0.0.0");
|
||||||
|
fclose(sdp);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// file index (search from 0)
|
||||||
|
int file_index = 0;
|
||||||
|
|
||||||
|
// time
|
||||||
|
struct timeval last_time;
|
||||||
|
gettimeofday(&last_time, NULL);
|
||||||
|
|
||||||
|
while (computer_vision_thread_command > 0)
|
||||||
|
{
|
||||||
|
// compute usleep to have a more stable frame rate
|
||||||
|
struct timeval time;
|
||||||
|
gettimeofday(&time, NULL);
|
||||||
|
int dt = (int)(time.tv_sec - last_time.tv_sec)*1000000 + (int)(time.tv_usec - last_time.tv_usec);
|
||||||
|
if (dt < microsleep) usleep(microsleep - dt);
|
||||||
|
last_time = time;
|
||||||
|
|
||||||
|
// Grab new frame
|
||||||
|
video_grab_image(&vid, img_new);
|
||||||
|
|
||||||
|
// Save picture on disk
|
||||||
|
if (computer_vision_thread_command == 2)
|
||||||
|
{
|
||||||
|
uint8_t* end = encode_image (img_new->buf, jpegbuf, 99, FOUR_TWO_TWO, vid.w, vid.h, 1);
|
||||||
|
uint32_t size = end-(jpegbuf);
|
||||||
|
FILE* save;
|
||||||
|
char save_name[128];
|
||||||
|
if (system("mkdir -p /data/video/images") == 0) {
|
||||||
|
// search available index (max is 99)
|
||||||
|
for ( ; file_index < 99; file_index++) {
|
||||||
|
printf("search %d\n",file_index);
|
||||||
|
sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index);
|
||||||
|
// test if file exists or not
|
||||||
|
if (access(save_name, F_OK) == -1) {
|
||||||
|
printf("access\n");
|
||||||
|
save = fopen(save_name, "w");
|
||||||
|
if (save != NULL) {
|
||||||
|
fwrite(jpegbuf, sizeof(uint8_t), size, save);
|
||||||
|
fclose(save);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("Error when opening file %s\n", save_name);
|
||||||
|
}
|
||||||
|
// leave for loop
|
||||||
|
break;
|
||||||
|
} else {
|
||||||
|
printf("file exists\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
computer_vision_thread_command = 1;
|
||||||
|
viewvideo_shot = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Resize
|
||||||
|
resize_uyuv(img_new, &small, VIDEO_DOWNSIZE_FACTOR);
|
||||||
|
|
||||||
|
// JPEG encode the image:
|
||||||
|
uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
|
||||||
|
uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header);
|
||||||
|
uint32_t size = end - (jpegbuf);
|
||||||
|
|
||||||
|
// Send image with RTP
|
||||||
|
printf("Sending an image ...%u\n",size);
|
||||||
|
send_rtp_frame(
|
||||||
|
vsock, // UDP
|
||||||
|
jpegbuf,size, // JPEG
|
||||||
|
small.w, small.h, // Img Size
|
||||||
|
0, // Format 422
|
||||||
|
quality_factor, // Jpeg-Quality
|
||||||
|
dri_jpeg_header, // DRI Header
|
||||||
|
1 // 90kHz time increment
|
||||||
|
);
|
||||||
|
// Extra note: when the time increment is set to 0,
|
||||||
|
// it is automaticaly calculated by the send_rtp_frame function
|
||||||
|
// based on gettimeofday value. This seems to introduce some lag or jitter.
|
||||||
|
// An other way is to compute the time increment and set the correct value.
|
||||||
|
// It seems that a lower value is also working (when the frame is received
|
||||||
|
// the timestamp is always "late" so the frame is displayed immediately).
|
||||||
|
// Here, we set the time increment to the lowest possible value
|
||||||
|
// (1 = 1/90000 s) which is probably stupid but is actually working.
|
||||||
|
}
|
||||||
|
printf("Thread Closed\n");
|
||||||
|
video_close(&vid);
|
||||||
|
computervision_thread_status = -100;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void viewvideo_start(void)
|
||||||
|
{
|
||||||
|
computer_vision_thread_command = 1;
|
||||||
|
int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
|
||||||
|
if(rc) {
|
||||||
|
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void viewvideo_stop(void)
|
||||||
|
{
|
||||||
|
computer_vision_thread_command = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int viewvideo_save_shot(void)
|
||||||
|
{
|
||||||
|
if (computer_vision_thread_command > 0) {
|
||||||
|
computer_vision_thread_command = 2;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2014 The Paparazzi Community
|
||||||
|
*
|
||||||
|
* This file is part of Paparazzi.
|
||||||
|
*
|
||||||
|
* Paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* Paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file modules/computer_vision/viewvideo.h
|
||||||
|
*
|
||||||
|
* Get live images from a RTP/UDP stream
|
||||||
|
* and save pictures on internal memory
|
||||||
|
*
|
||||||
|
* Works on Linux platforms
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef VIEW_VIDEO_H
|
||||||
|
#define VIEW_VIDEO_H
|
||||||
|
|
||||||
|
// Module functions
|
||||||
|
extern void viewvideo_run(void);
|
||||||
|
extern void viewvideo_start(void);
|
||||||
|
extern void viewvideo_stop(void);
|
||||||
|
|
||||||
|
// Save picture on disk at full resolution
|
||||||
|
// can be called from flight plan
|
||||||
|
extern int viewvideo_save_shot(void);
|
||||||
|
|
||||||
|
extern int viewvideo_shot;
|
||||||
|
#define viewvideo_SaveShot(_v) { \
|
||||||
|
viewvideo_shot = 1; \
|
||||||
|
viewvideo_save_shot(); \
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* VIEW_VIDEO_H */
|
||||||
|
|
||||||
@@ -0,0 +1,39 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2012-2013
|
||||||
|
*
|
||||||
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** Dummy C implementation for simulation
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Own header
|
||||||
|
#include "viewvideo.h"
|
||||||
|
|
||||||
|
int viewvideo_shot = 0;
|
||||||
|
|
||||||
|
void viewvideo_run(void) {}
|
||||||
|
void viewvideo_start(void) {}
|
||||||
|
void viewvideo_stop(void) {}
|
||||||
|
|
||||||
|
int viewvideo_save_shot(void)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
Executable
+20
@@ -0,0 +1,20 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
|
||||||
|
# base address for ARDrone2
|
||||||
|
ADDR_BASE=192.168.1.
|
||||||
|
|
||||||
|
# tmp folder for images files (relative path by default)
|
||||||
|
IMAGES_DIR=${PAPARAZZI_HOME=../../../..}/var/images_tmp
|
||||||
|
|
||||||
|
# test if a complete IP address is passed as first argument or just the last digit
|
||||||
|
if [ `grep -c '\.' <<< $1` == 1 ]
|
||||||
|
then
|
||||||
|
ADDR=$1
|
||||||
|
else
|
||||||
|
ADDR=$ADDR_BASE$1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# download images folder from ARDrone
|
||||||
|
mkdir -p $IMAGES_DIR/$ADDR
|
||||||
|
../ardrone2.py --host=$ADDR download_dir $IMAGES_DIR/$ADDR images
|
||||||
|
|
||||||
Executable
+20
@@ -0,0 +1,20 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
|
||||||
|
# base address for ARDrone2
|
||||||
|
ADDR_BASE=192.168.1.
|
||||||
|
|
||||||
|
# test if a complete IP address is passed as first argument or just the last digit
|
||||||
|
if [ `grep -c '\.' <<< $1` == 1 ]
|
||||||
|
then
|
||||||
|
ADDR=$1
|
||||||
|
PORT=$2
|
||||||
|
else
|
||||||
|
ADDR=$ADDR_BASE$1
|
||||||
|
PORT=$((5000+$1))
|
||||||
|
fi
|
||||||
|
|
||||||
|
pid=0
|
||||||
|
|
||||||
|
echo "Start video for $ADDR on port $PORT"
|
||||||
|
/usr/bin/avplay -loglevel quiet -max_delay 50 -fflags nobuffer rtp://$ADDR:$PORT
|
||||||
|
|
||||||
Executable
+50
@@ -0,0 +1,50 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
|
||||||
|
# base address for ARDrone2
|
||||||
|
ADDR_BASE=192.168.1.
|
||||||
|
|
||||||
|
# tmp folder for sdp files (relative path by default)
|
||||||
|
SDP_DIR=${PAPARAZZI_HOME=../../../..}/var/sdp_tmp
|
||||||
|
|
||||||
|
# test if a complete IP address is passed as first argument or just the last digit
|
||||||
|
if [ `grep -c '\.' <<< $1` == 1 ]
|
||||||
|
then
|
||||||
|
ADDR=$1
|
||||||
|
else
|
||||||
|
ADDR=$ADDR_BASE$1
|
||||||
|
fi
|
||||||
|
|
||||||
|
pid=0
|
||||||
|
|
||||||
|
function quit {
|
||||||
|
echo "Stop video"
|
||||||
|
if [ $pid > 0 ]
|
||||||
|
then
|
||||||
|
kill -9 $pid
|
||||||
|
fi
|
||||||
|
exit 0
|
||||||
|
}
|
||||||
|
|
||||||
|
# trap control+c to stop mplayer
|
||||||
|
trap quit SIGINT
|
||||||
|
|
||||||
|
# fetch sdp file on the ARDrone
|
||||||
|
mkdir -p $SDP_DIR/$ADDR
|
||||||
|
../ardrone2.py --host=$ADDR download_file $SDP_DIR/$ADDR/x86_config-mjpeg.sdp sdp
|
||||||
|
|
||||||
|
if [ ! -f $SDP_DIR/$ADDR/x86_config-mjpeg.sdp ];
|
||||||
|
then
|
||||||
|
echo "Unable to download sdp file from $ADDR"
|
||||||
|
exit 0
|
||||||
|
fi
|
||||||
|
|
||||||
|
# start mplayer and respawn if needed
|
||||||
|
echo "Start video"
|
||||||
|
while [ 1 ]
|
||||||
|
do
|
||||||
|
/usr/bin/mplayer -really-quiet $SDP_DIR/$ADDR/x86_config-mjpeg.sdp&
|
||||||
|
pid=$!
|
||||||
|
wait $pid
|
||||||
|
echo "Restart video"
|
||||||
|
done
|
||||||
|
|
||||||
Reference in New Issue
Block a user