mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-30 19:47:50 +08:00
Felix Ruess
+4
-1
@@ -42,7 +42,8 @@ CFLAGS += -fno-short-enums
|
||||
# CFLAGS += -malignment-traps
|
||||
CFLAGS += -Wall -Wcast-qual -Wimplicit -Wcast-align
|
||||
CFLAGS += -Wpointer-arith -Wswitch
|
||||
CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused -Wno-unused-result
|
||||
CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
|
||||
#-Wno-unused-result
|
||||
#CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
|
||||
#CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
@@ -97,6 +98,7 @@ endif
|
||||
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/usbserial.ko /$(SUB_DIR)/usbserial.ko"; \
|
||||
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/pl2303.ko /$(SUB_DIR)/pl2303.ko"; \
|
||||
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/ftdi-sio.ko /$(SUB_DIR)/ftdi-sio.ko"; \
|
||||
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cp210x.ko /$(SUB_DIR)/cp210x.ko"; \
|
||||
echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \
|
||||
echo "quit"; \
|
||||
} | ftp -n $(HOST)
|
||||
@@ -106,6 +108,7 @@ endif
|
||||
echo "cd $(TARGET_DIR)"; \
|
||||
echo "insmod cdc-acm.ko"; \
|
||||
echo "insmod usbserial.ko"; \
|
||||
echo "insmod cp210x.ko"; \
|
||||
echo "insmod pl2303.ko"; \
|
||||
echo "insmod ftdi-sio.ko"; \
|
||||
echo "chmod 777 $(TARGET).elf"; \
|
||||
|
||||
@@ -0,0 +1,239 @@
|
||||
<!DOCTYPE airframe SYSTEM "airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2_raw">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2_raw">
|
||||
<define name="USE_INS_NAV_INIT" />
|
||||
<!--configure name="USE_NEW_I2C_DRIVER" value="1"/ -->
|
||||
<configure name="USE_MAGNETOMETER" value="0" />
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="200" />
|
||||
<define name="AHRS_PROPAGATE_FREQUENCY" value="200" />
|
||||
<define name="USE_BAROMETER" />
|
||||
<subsystem name="telemetry" type="wifi" />
|
||||
<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 name="motor_mixing" />
|
||||
|
||||
<subsystem name="actuators" type="ardrone2" />
|
||||
<subsystem name="imu" type="ardrone2" />
|
||||
<subsystem name="gps" type="sirf" />
|
||||
<subsystem name="stabilization" type="int_quat" />
|
||||
<subsystem name="ahrs" type="int_cmpl_quat" />
|
||||
<subsystem name="ins" />
|
||||
|
||||
</firmware>
|
||||
|
||||
<!-- <modules main_freq="512"> <load name="sys_mon.xml" /> </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="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_LEFT" no="3" min="0" neutral="1" max="500" />
|
||||
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500" />
|
||||
</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="NB_MOTOR" value="4" />
|
||||
<define name="SCALE" value="255" />
|
||||
<!-- Time cross layout (X), as documented in paparazzi -->
|
||||
<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[SERVO_TOP_LEFT]" />
|
||||
<set servo="TOP_RIGHT" value="motor_mixing.commands[SERVO_TOP_RIGHT]" />
|
||||
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[SERVO_BOTTOM_LEFT]" />
|
||||
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[SERVO_BOTTOM_RIGHT]" />
|
||||
</command_laws>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="ACCEL_X_NEUTRAL" value="2072" />
|
||||
<define name="ACCEL_Y_NEUTRAL" value="2040" />
|
||||
<define name="ACCEL_Z_NEUTRAL" value="2047" />
|
||||
<define name="ACCEL_X_SENS" value="19.1079036954" integer="16" />
|
||||
<define name="ACCEL_Y_SENS" value="19.5393623518" integer="16" />
|
||||
<define name="ACCEL_Z_SENS" value="19.6539180847" integer="16" />
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1" />
|
||||
<define name="ACCEL_Y_SIGN" value="-1" />
|
||||
<define name="ACCEL_Z_SIGN" value="-1" />
|
||||
|
||||
<define name="GYRO_P_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_P_SENS_DEN" value="1000" />
|
||||
<define name="GYRO_Q_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_Q_SENS_DEN" value="1000" />
|
||||
<define name="GYRO_R_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_R_SENS_DEN" value="1000" />
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1" />
|
||||
<define name="GYRO_Q_SIGN" value="-1" />
|
||||
<define name="GYRO_R_SIGN" value="-1" />
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="118" />
|
||||
<define name="MAG_Y_NEUTRAL" value="-65" />
|
||||
<define name="MAG_Z_NEUTRAL" value="110" />
|
||||
<define name="MAG_X_SENS" value="14.6416865144" integer="16" />
|
||||
<define name="MAG_Y_SENS" value="14.5167340835" integer="16" />
|
||||
<define name="MAG_Z_SENS" value="15.1670335124" integer="16" />
|
||||
|
||||
<define name="MAG_X_SIGN" value="-1" />
|
||||
<define name="MAG_Y_SIGN" value="1" />
|
||||
<define name="MAG_Z_SIGN" value="-1" />
|
||||
|
||||
<!-- roll -->
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
|
||||
<!-- pitch -->
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
|
||||
<!-- yaw -->
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg" />
|
||||
</section>
|
||||
|
||||
<!-- local magnetic field, calculated for: 52°3'56", 4°31'24" -->
|
||||
<!-- http://paparazzi.enac.fr/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<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_">
|
||||
<define name="BARO_SENS" value="22.3" integer="16" />
|
||||
</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="15" unit="deg" />
|
||||
<define name="SP_MAX_THETA" value="15" unit="deg" />
|
||||
<define name="SP_MAX_R" value="15" 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="800" unit="deg/s" />
|
||||
<define name="REF_ZETA_P" value="0.85" />
|
||||
<define name="REF_MAX_P" value="400." unit="deg/s" />
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)" />
|
||||
|
||||
<define name="REF_OMEGA_Q" value="800" unit="deg/s" />
|
||||
<define name="REF_ZETA_Q" value="0.85" />
|
||||
<define name="REF_MAX_Q" value="400." unit="deg/s" />
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)" />
|
||||
|
||||
<define name="REF_OMEGA_R" value="500" unit="deg/s" />
|
||||
<define name="REF_ZETA_R" value="0.85" />
|
||||
<define name="REF_MAX_R" value="180." unit="deg/s" />
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" />
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="592" />
|
||||
<define name="PHI_DGAIN" value="303" />
|
||||
<define name="PHI_IGAIN" value="0" />
|
||||
|
||||
<define name="THETA_PGAIN" value="606" />
|
||||
<define name="THETA_DGAIN" value="303" />
|
||||
<define name="THETA_IGAIN" value="0" />
|
||||
|
||||
<define name="PSI_PGAIN" value="529" />
|
||||
<define name="PSI_DGAIN" value="353" />
|
||||
<define name="PSI_IGAIN" value="0" />
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0" />
|
||||
<define name="THETA_DDGAIN" value="0" />
|
||||
<define name="PSI_DDGAIN" value="52" />
|
||||
</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="HOVER_KP" value="283" />
|
||||
<define name="HOVER_KD" value="18" />
|
||||
<define name="HOVER_KI" value="13" />
|
||||
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
|
||||
<define name="RC_CLIMB_COEF" value="163" />
|
||||
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
|
||||
<define name="RC_CLIMB_DEAD_BAND" value="160000" />
|
||||
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive
|
||||
estimation -->
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.4" />
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="20" unit="deg" />
|
||||
<define name="PGAIN" value="100" />
|
||||
<define name="DGAIN" value="100" />
|
||||
<define name="IGAIN" value="0" />
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES"
|
||||
value="{"top_left_motor", "top_right_motor", "bottom_right_motor", "bottom_left_motor"}" />
|
||||
<define name="INITIAL_CONDITITONS" value=""reset00"" />
|
||||
<define name="SENSORS_PARAMS" value=""nps_sensors_params_default.h"" />
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<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_HOVER_Z_HOLD" />
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<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,154 @@
|
||||
<!DOCTYPE airframe SYSTEM "airframe.dtd">
|
||||
|
||||
<airframe name="ardrone2_sdk">
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="ardrone2_sdk">
|
||||
<define name="USE_INS_NAV_INIT" />
|
||||
<define name="ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED"/>
|
||||
<!-- <define name="USE_GPS_HEIGHT" /> -->
|
||||
<!-- <define name="ARDRONE_FLIGHT_INDOOR" /> -->
|
||||
<!-- <define name="ARDRONE_WITHOUT_SHELL" /> -->
|
||||
<!-- <define name="ARDRONE_OWNER_MAC" value="00:24:d7:47:f0:f4" /> -->
|
||||
</target>
|
||||
|
||||
<subsystem name="radio_control" type="datalink" />
|
||||
<subsystem name="telemetry" type="wifi" />
|
||||
<subsystem name="gps" type="sirf" />
|
||||
<subsystem name="ahrs" type="ardrone2" />
|
||||
<subsystem name="ins" type="ardrone2" />
|
||||
<subsystem name="actuators" type="ardrone2" />
|
||||
<subsystem name="stabilization" type="passthrough"/>
|
||||
<subsystem name="imu" type="ardrone2" />
|
||||
|
||||
</firmware>
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="sys_mon.xml" />
|
||||
</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="0" />
|
||||
</commands>
|
||||
|
||||
<servos driver="Default" />
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<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="30." unit="deg" />
|
||||
<define name="SP_MAX_THETA" value="30." unit="deg" />
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s" />
|
||||
<define name="DEADBAND_A" value="0" />
|
||||
<define name="DEADBAND_E" value="0" />
|
||||
<define name="DEADBAND_R" value="250" />
|
||||
</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="HOVER_KP" value="425" />
|
||||
<define name="HOVER_KD" value="200" />
|
||||
<define name="HOVER_KI" value="125" />
|
||||
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
|
||||
<define name="RC_CLIMB_COEF" value="163" />
|
||||
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
|
||||
<define name="RC_CLIMB_DEAD_BAND" value="160000" />
|
||||
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive
|
||||
estimation -->
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.5" />
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="MAX_BANK" value="30." unit="deg" />
|
||||
<define name="PGAIN" value="7" />
|
||||
<define name="DGAIN" value="2" />
|
||||
<define name="IGAIN" value="0" />
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT" />
|
||||
<define name="MODE_AUTO1" value="AP_MODE_HOVER_Z_HOLD" />
|
||||
<define name="MODE_AUTO2" value="AP_MODE_NAV" />
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value=".2" unit="V" />
|
||||
<define name="CRITIC_BAT_LEVEL" value="1.9" unit="V" />
|
||||
<define name="LOW_BAT_LEVEL" value="2.5" unit="V" />
|
||||
<define name="MAX_BAT_LEVEL" value="10.0" unit="V" />
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="ACCEL_X_NEUTRAL" value="2072" />
|
||||
<define name="ACCEL_Y_NEUTRAL" value="2040" />
|
||||
<define name="ACCEL_Z_NEUTRAL" value="2047" />
|
||||
<define name="ACCEL_X_SENS" value="19.1079036954" integer="16" />
|
||||
<define name="ACCEL_Y_SENS" value="19.5393623518" integer="16" />
|
||||
<define name="ACCEL_Z_SENS" value="19.6539180847" integer="16" />
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1" />
|
||||
<define name="ACCEL_Y_SIGN" value="-1" />
|
||||
<define name="ACCEL_Z_SIGN" value="-1" />
|
||||
|
||||
<define name="GYRO_P_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_P_SENS_DEN" value="1000" />
|
||||
<define name="GYRO_Q_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_Q_SENS_DEN" value="1000" />
|
||||
<define name="GYRO_R_SENS_NUM" value="4359" />
|
||||
<define name="GYRO_R_SENS_DEN" value="1000" />
|
||||
|
||||
<define name="GYRO_P_SIGN" value="1" />
|
||||
<define name="GYRO_Q_SIGN" value="-1" />
|
||||
<define name="GYRO_R_SIGN" value="-1" />
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="118" />
|
||||
<define name="MAG_Y_NEUTRAL" value="-65" />
|
||||
<define name="MAG_Z_NEUTRAL" value="110" />
|
||||
<define name="MAG_X_SENS" value="14.6416865144" integer="16" />
|
||||
<define name="MAG_Y_SENS" value="14.5167340835" integer="16" />
|
||||
<define name="MAG_Z_SENS" value="15.1670335124" integer="16" />
|
||||
|
||||
<define name="MAG_X_SIGN" value="-1" />
|
||||
<define name="MAG_Y_SIGN" value="1" />
|
||||
<define name="MAG_Z_SIGN" value="-1" />
|
||||
|
||||
<!-- roll -->
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
|
||||
<!-- pitch -->
|
||||
<define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
|
||||
<!-- yaw -->
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg" />
|
||||
</section>
|
||||
|
||||
</airframe>
|
||||
@@ -0,0 +1,42 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# ardrone2_raw.makefile
|
||||
#
|
||||
# http://paparazzi.enac.fr/wiki/AR.Drone_2_-_Specifications
|
||||
#
|
||||
|
||||
BOARD=ardrone
|
||||
BOARD_VERSION=2
|
||||
BOARD_TYPE=raw
|
||||
BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\"
|
||||
|
||||
ARCH=omap
|
||||
$(TARGET).ARCHDIR = $(ARCH)
|
||||
|
||||
# -----------------------------------------------------------------------
|
||||
USER=foobar
|
||||
HOST=192.168.1.1
|
||||
SUB_DIR=raw
|
||||
FTP_DIR=/data/video
|
||||
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
|
||||
# -----------------------------------------------------------------------
|
||||
|
||||
# The GPS sensor is connected trough USB so we have to define the device
|
||||
GPS_PORT ?= UART1
|
||||
GPS_BAUD ?= B57600
|
||||
|
||||
# Here we define what the UART1_DEV device mapping
|
||||
$(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
|
||||
#$(TARGET).CFLAGS += -DUART0_DEV=\"/dev/ttyO3\"
|
||||
|
||||
# for distinction between RAW and SDK version
|
||||
$(TARGET).CFLAGS +=-DARDRONE2_RAW
|
||||
|
||||
# -----------------------------------------------------------------------
|
||||
|
||||
# default LED configuration
|
||||
RADIO_CONTROL_LED ?= none
|
||||
BARO_LED ?= none
|
||||
AHRS_ALIGNER_LED ?= none
|
||||
GPS_LED ?= none
|
||||
SYS_TIME_LED ?= none
|
||||
@@ -0,0 +1,34 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# ardrone2_sdk.makefile
|
||||
#
|
||||
# http://paparazzi.enac.fr/wiki/AR.Drone_2_-_Specifications
|
||||
#
|
||||
|
||||
BOARD=ardrone
|
||||
BOARD_VERSION=2
|
||||
BOARD_TYPE=sdk
|
||||
BOARD_CFG=\"boards/$(BOARD)$(BOARD_VERSION)_$(BOARD_TYPE).h\"
|
||||
|
||||
ARCH=omap
|
||||
$(TARGET).ARCHDIR = $(ARCH)
|
||||
|
||||
# -----------------------------------------------------------------------
|
||||
USER=foobar
|
||||
HOST=192.168.1.1
|
||||
SUB_DIR=sdk
|
||||
FTP_DIR=/data/video
|
||||
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
|
||||
# -----------------------------------------------------------------------
|
||||
|
||||
# The GPS sensor is connected trough USB so we have to define the device
|
||||
GPS_PORT ?= UART1
|
||||
GPS_BAUD ?= B57600
|
||||
|
||||
# Here we define what the UART1_DEV device mapping
|
||||
$(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
|
||||
|
||||
# for distinction between SDK and RAW version
|
||||
ap.CFLAGS +=-DARDRONE2_SDK
|
||||
|
||||
# -----------------------------------------------------------------------
|
||||
@@ -125,6 +125,26 @@
|
||||
settings=" settings/test_fixedwing_actuators.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="ardrone2_sdk"
|
||||
ac_id="200"
|
||||
airframe="airframes/ardrone2_sdk.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_ardrone.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="ardrone2_raw"
|
||||
ac_id="201"
|
||||
airframe="airframes/ardrone2_raw.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_ardrone.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
<aircraft
|
||||
name="Twinstar_energyadaptive"
|
||||
ac_id="13"
|
||||
|
||||
@@ -83,6 +83,30 @@
|
||||
|
||||
<section name="sessions">
|
||||
|
||||
<session name="ARDrone2 RAW Flight">
|
||||
<program name="Server"/>
|
||||
<program name="GCS"/>
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-ac" constant="ardrone2_raw"/>
|
||||
<arg flag="extreme_3d_pro.xml"/>
|
||||
</program>
|
||||
</session>
|
||||
|
||||
<session name="ARDrone2 SDK Flight">
|
||||
<program name="Server"/>
|
||||
<program name="GCS"/>
|
||||
<program name="Data Link">
|
||||
<arg flag="-udp"/>
|
||||
</program>
|
||||
<program name="Joystick">
|
||||
<arg flag="-ac" constant="ardrone2_sdk"/>
|
||||
<arg flag="extreme_3d_pro.xml"/>
|
||||
</program>
|
||||
</session>
|
||||
|
||||
<session name="Flight USB-serial@9600">
|
||||
<program name="Data Link">
|
||||
<arg flag="-d" constant="/dev/ttyUSB0"/>
|
||||
|
||||
@@ -72,6 +72,10 @@ ifeq ($(ARCH), stm32)
|
||||
ap.srcs += $(SRC_ARCH)/led_hw.c
|
||||
endif
|
||||
|
||||
ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
|
||||
ap.srcs += $(SRC_BOARD)/gpio.c
|
||||
endif
|
||||
|
||||
# frequency of main periodic
|
||||
PERIODIC_FREQUENCY ?= 512
|
||||
ap.CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
|
||||
@@ -145,6 +149,12 @@ else ifeq ($(BOARD), lisa_l)
|
||||
ap.CFLAGS += -DUSE_I2C2
|
||||
ap.srcs += $(SRC_BOARD)/baro_board.c
|
||||
|
||||
# Ardrone baro
|
||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
|
||||
ap.srcs += $(SRC_BOARD)/baro_board.c
|
||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
|
||||
ap.srcs += $(SRC_BOARD)/baro_board_dummy.c
|
||||
|
||||
# Lisa/M baro
|
||||
else ifeq ($(BOARD), lisa_m)
|
||||
# defaults to i2c baro bmp085 on the board
|
||||
@@ -206,6 +216,10 @@ ap.CFLAGS += -DUSE_ADC
|
||||
ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
|
||||
ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
|
||||
ap.srcs += subsystems/electrical.c
|
||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk)
|
||||
ap.srcs += $(SRC_BOARD)/electrical_dummy.c
|
||||
else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw)
|
||||
ap.srcs += $(SRC_ARCH)/subsystems/electrical/electrical_arch.c
|
||||
endif
|
||||
|
||||
|
||||
@@ -266,3 +280,8 @@ ap.srcs += subsystems/navigation/common_flight_plan.c
|
||||
# or
|
||||
# nothing
|
||||
#
|
||||
ifeq ($(ARCH), omap)
|
||||
SRC_FMS=fms
|
||||
ap.CFLAGS += -I. -I$(SRC_FMS)
|
||||
ap.srcs += $(SRC_FMS)/fms_serial_port.c
|
||||
endif
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
# Hey Emacs, this is a -*- makefile -*-
|
||||
#
|
||||
# Fixed point complementary filter using euler angles for attitude estimation
|
||||
#
|
||||
|
||||
AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_ARDRONE2
|
||||
|
||||
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_ardrone2.h\"
|
||||
AHRS_SRCS += subsystems/ahrs.c
|
||||
AHRS_SRCS += subsystems/ahrs/ahrs_ardrone2.c
|
||||
|
||||
ap.CFLAGS += $(AHRS_CFLAGS)
|
||||
ap.srcs += $(AHRS_SRCS)
|
||||
|
||||
nps.CFLAGS += $(AHRS_CFLAGS)
|
||||
nps.srcs += $(AHRS_SRCS)
|
||||
@@ -0,0 +1,7 @@
|
||||
#
|
||||
# simple INS with float vertical filter
|
||||
#
|
||||
|
||||
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_ardrone2.h\" -DAUTOPILOT_DISABLE_AHRS_KILL
|
||||
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
|
||||
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_ardrone2.c
|
||||
@@ -0,0 +1,11 @@
|
||||
|
||||
# Actuator drivers for both ardrone versions are conditionally included here
|
||||
# The AT-command and RAW drivers are not interchangeble
|
||||
|
||||
ifeq ($(BOARD_TYPE), sdk)
|
||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_at.c
|
||||
$(TARGET).srcs += $(SRC_BOARD)/at_com.c
|
||||
else ifeq ($(BOARD_TYPE), raw)
|
||||
$(TARGET).CFLAGS += -DACTUATORS
|
||||
$(TARGET).srcs += $(SRC_BOARD)/actuators_ardrone2_raw.c
|
||||
endif
|
||||
@@ -0,0 +1,22 @@
|
||||
# imu AR.Drone2
|
||||
|
||||
ifeq ($(BOARD_TYPE), sdk)
|
||||
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_sdk.h\" -DUSE_IMU
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_sdk.c
|
||||
else ifeq ($(BOARD_TYPE), raw)
|
||||
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_ardrone2_raw.h\" -DUSE_IMU
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_ardrone2_raw.c
|
||||
imu_srcs += $(SRC_BOARD)/navdata.c
|
||||
endif
|
||||
|
||||
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
|
||||
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
|
||||
ap.CFLAGS += $(imu_CFLAGS)
|
||||
ap.srcs += $(imu_srcs)
|
||||
|
||||
#
|
||||
# Simulator
|
||||
#
|
||||
include $(CFG_SHARED)/imu_nps.makefile
|
||||
+46
-2
@@ -19,7 +19,36 @@
|
||||
<field name="cpu_time" type="uint16" unit="s"></field>
|
||||
</message>
|
||||
|
||||
<!-- 5 is free -->
|
||||
<message name="ARDRONE_NAVDATA" id="5">
|
||||
<field name="taille" type="uint16" />
|
||||
<field name="nu_trame" type="uint16" />
|
||||
<field name="ax" type="uint16" />
|
||||
<field name="ay" type="uint16" />
|
||||
<field name="az" type="uint16" />
|
||||
<field name="vx" type="int16" />
|
||||
<field name="vy" type="int16" />
|
||||
<field name="vz" type="int16" />
|
||||
<field name="temperature_acc" type="uint16" />
|
||||
<field name="temperature_gyro" type="uint16" />
|
||||
<field name="ultrasound" type="uint16" />
|
||||
<field name="us_debut_echo" type="uint16" />
|
||||
<field name="us_fin_echo" type="uint16" />
|
||||
<field name="us_association_echo" type="uint16" />
|
||||
<field name="us_distance_echo" type="uint16" />
|
||||
<field name="us_curve_time" type="uint16" />
|
||||
<field name="us_curve_value" type="uint16" />
|
||||
<field name="us_curve_ref" type="uint16" />
|
||||
<field name="nb_echo" type="uint16" />
|
||||
<field name="sum_echo" type="uint32" />
|
||||
<field name="gradient" type="int16" />
|
||||
<field name="flag_echo_ini" type="uint16" />
|
||||
<field name="pressure" type="int32" />
|
||||
<field name="temperature_pressure" type="int16" />
|
||||
<field name="mx" type="int16" />
|
||||
<field name="my" type="int16" />
|
||||
<field name="mz" type="int16" />
|
||||
<field name="chksum" type="uint16" />
|
||||
</message>
|
||||
|
||||
<message name="ATTITUDE" id="6">
|
||||
<field name="phi" type="float" unit="rad" alt_unit="deg"/>
|
||||
@@ -1834,7 +1863,22 @@
|
||||
|
||||
<!--228 is free -->
|
||||
<!--229 is free -->
|
||||
<!--230 is free -->
|
||||
|
||||
<message name="AHRS_ARDRONE2" id="230">
|
||||
<field name="state" type="uint32" />
|
||||
<field name="control_state" type="uint32" values="DEFAULT|INIT|LANDING|FLYING|HOVERING|TEST|TRANS_TAKEOFF|TRANS_GOTOFIX|TRANS_LANDING|TRANS_LOOPING"/>
|
||||
<field name="phi" type="float" unit="mdeg"/>
|
||||
<field name="theta" type="float" unit="mdeg"/>
|
||||
<field name="psi" type="float" unit="mdeg"/>
|
||||
<field name="speed_x" type="float" unit="m/s"/>
|
||||
<field name="speed_y" type="float" unit="m/s"/>
|
||||
<field name="speed_z" type="float" unit="m/s"/>
|
||||
<field name="accel_x" type="float" unit="m/s^2"/>
|
||||
<field name="accel_y" type="float" unit="m/s^2"/>
|
||||
<field name="accel_z" type="float" unit="m/s^2"/>
|
||||
<field name="altitude" type="int32" unit="cm"/>
|
||||
<field name="battery" type="uint32" unit="%"/>
|
||||
</message>
|
||||
|
||||
<message name="ROTORCRAFT_STATUS" id="231">
|
||||
<field name="link_imu_nb_err" type="uint32"/>
|
||||
|
||||
@@ -0,0 +1,147 @@
|
||||
<?xml version="1.0"?>
|
||||
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
|
||||
<telemetry>
|
||||
|
||||
|
||||
<process name="Main">
|
||||
|
||||
<mode name="default" key_press="d">
|
||||
<message name="DL_VALUE" period="1.1"/>
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="ROTORCRAFT_FP" period="0.25"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||
<message name="WP_MOVED" period="1.3"/>
|
||||
<message name="ROTORCRAFT_CAM" period="1."/>
|
||||
<message name="GPS_INT" period=".25"/>
|
||||
<message name="INS" period=".25"/>
|
||||
<message name="I2C_ERRORS" period="4.1"/>
|
||||
<message name="UART_ERRORS" period="3.1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="ppm">
|
||||
<message name="ROTORCRAFT_CMD" period=".05"/>
|
||||
<message name="PPM" period="0.5"/>
|
||||
<message name="RC" period="0.5"/>
|
||||
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
|
||||
<message name="ROTORCRAFT_STATUS" period="1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="raw_sensors">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="IMU_ACCEL_RAW" period=".05"/>
|
||||
<message name="IMU_GYRO_RAW" period=".05"/>
|
||||
<message name="IMU_MAG_RAW" period=".05"/>
|
||||
<message name="BARO_RAW" period=".1"/>
|
||||
<message name="ARDRONE_NAVDATA" period=".05"/>
|
||||
</mode>
|
||||
|
||||
<mode name="scaled_sensors">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="IMU_GYRO_SCALED" period=".075"/>
|
||||
<message name="IMU_ACCEL_SCALED" period=".075"/>
|
||||
<message name="IMU_MAG_SCALED" period=".1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="ahrs">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="FILTER_ALIGNER" period="2.2"/>
|
||||
<message name="FILTER" period=".5"/>
|
||||
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
|
||||
<message name="AHRS_QUAT_INT" period=".25"/>
|
||||
<!-- <message name="AHRS_EULER_INT" period=".1"/> -->
|
||||
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
|
||||
<message name="AHRS_ARDRONE2" period=".1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="rate_loop">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="RATE_LOOP" period=".02"/>
|
||||
</mode>
|
||||
|
||||
<mode name="attitude_setpoint_viz" key_press="v">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
|
||||
<message name="AHRS_REF_QUAT" period="0.05"/>
|
||||
</mode>
|
||||
|
||||
<mode name="attitude_loop" key_press="a">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="STAB_ATTITUDE" period=".03"/>
|
||||
<message name="STAB_ATTITUDE_REF" period=".03"/>
|
||||
</mode>
|
||||
|
||||
<mode name="vert_loop" key_press="v">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="VFF" period=".05"/>
|
||||
<message name="VERT_LOOP" period=".05"/>
|
||||
<message name="INS" period=".05"/>
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="h_loop" key_press="h">
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="HOVER_LOOP" period="0.062"/>
|
||||
<message name="GUIDANCE_H_REF" period="0.062"/>
|
||||
<message name="STAB_ATTITUDE" period="0.4"/>
|
||||
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
|
||||
<message name="ROTORCRAFT_FP" period="0.8"/>
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
<!-- HFF messages are only sent if USE_HFF -->
|
||||
<message name="HFF" period=".05"/>
|
||||
<message name="HFF_GPS" period=".03"/>
|
||||
<message name="HFF_DBG" period=".2"/>
|
||||
</mode>
|
||||
|
||||
<mode name="aligner">
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="FILTER_ALIGNER" period="0.02"/>
|
||||
</mode>
|
||||
|
||||
<mode name="hs_att_roll">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="ALIVE" period="0.9"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<!-- <message name="STAB_ATTITUDE_HS_ROLL" period="0.02"/> -->
|
||||
</mode>
|
||||
|
||||
<mode name="tune_hover">
|
||||
<message name="DL_VALUE" period="1.1"/>
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="GUIDANCE_H_INT" period="0.05"/>
|
||||
<!--<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>-->
|
||||
<!-- <message name="GPS_INT" period=".20"/> -->
|
||||
<!--<message name="INS2" period=".05"/>
|
||||
<message name="INS3" period=".20"/>-->
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="mag_current_calibration">
|
||||
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="IMU_MAG_CURRENT_CALIBRATION" period="0.05"/>
|
||||
</mode>
|
||||
|
||||
</process>
|
||||
|
||||
</telemetry>
|
||||
|
||||
@@ -226,6 +226,26 @@
|
||||
settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml settings/modules/cam.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="ardrone2_sdk"
|
||||
ac_id="200"
|
||||
airframe="airframes/ardrone2_sdk.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_ardrone.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="ardrone2_raw"
|
||||
ac_id="201"
|
||||
airframe="airframes/ardrone2_raw.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_ardrone.xml"
|
||||
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||
settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
|
||||
gui_color="red"
|
||||
/>
|
||||
<!-- Hardware test Lisa/L -->
|
||||
<aircraft
|
||||
name="LisaLv11_Booz2v12_RC"
|
||||
|
||||
@@ -0,0 +1,169 @@
|
||||
/*
|
||||
* Original Code from:
|
||||
* Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
|
||||
*
|
||||
* Adapated for Paparazzi by:
|
||||
* Copyright (C) 2012 Dino Hensen <dino.hensen@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/actuators_ardrone2_raw.c
|
||||
* Actuator driver for ardrone2-raw version
|
||||
*/
|
||||
|
||||
#include "subsystems/actuators.h"
|
||||
#include "actuators_ardrone2_raw.h"
|
||||
#include "gpio.h"
|
||||
|
||||
#include <stdio.h> /* Standard input/output definitions */
|
||||
#include <string.h> /* String function definitions */
|
||||
#include <unistd.h> /* UNIX standard function definitions */
|
||||
#include <fcntl.h> /* File control definitions */
|
||||
#include <errno.h> /* Error number definitions */
|
||||
#include <termios.h> /* POSIX terminal control definitions */
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* Power consumption @ 11V all 4 motors running
|
||||
* PWM A
|
||||
* 0 0.2
|
||||
* 80 1.3
|
||||
* 100 1.5
|
||||
* 150 2.0
|
||||
* 190 2.5
|
||||
* 130 3.0
|
||||
*/
|
||||
int mot_fd; /**< File descriptor for the port */
|
||||
|
||||
void actuators_ardrone_init(void)
|
||||
{
|
||||
//open mot port
|
||||
mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (mot_fd == -1)
|
||||
{
|
||||
perror("open_port: Unable to open /dev/ttyO0 - ");
|
||||
return;
|
||||
}
|
||||
fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking
|
||||
fcntl(mot_fd, F_GETFL, 0);
|
||||
|
||||
//set port options
|
||||
struct termios options;
|
||||
//Get the current options for the port
|
||||
tcgetattr(mot_fd, &options);
|
||||
//Set the baud rates to 115200
|
||||
cfsetispeed(&options, B115200);
|
||||
cfsetospeed(&options, B115200);
|
||||
|
||||
options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
|
||||
options.c_iflag = 0; //clear input options
|
||||
options.c_lflag=0; //clear local options
|
||||
options.c_oflag &= ~OPOST; //clear output options (raw output)
|
||||
|
||||
//Set the new options for the port
|
||||
tcsetattr(mot_fd, TCSANOW, &options);
|
||||
|
||||
//reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
|
||||
gpio_set(106,-1);
|
||||
gpio_set(107,0);
|
||||
gpio_set(107,1);
|
||||
|
||||
//all select lines inactive
|
||||
gpio_set(68,1);
|
||||
gpio_set(69,1);
|
||||
gpio_set(70,1);
|
||||
gpio_set(71,1);
|
||||
|
||||
//configure motors
|
||||
uint8_t reply[256];
|
||||
for(int m=0;m<4;m++) {
|
||||
gpio_set(68+m,-1);
|
||||
actuators_ardrone_cmd(0xe0,reply,2);
|
||||
if(reply[0]!=0xe0 || reply[1]!=0x00)
|
||||
{
|
||||
printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]);
|
||||
}
|
||||
actuators_ardrone_cmd(m+1,reply,1);
|
||||
gpio_set(68+m,1);
|
||||
}
|
||||
|
||||
//all select lines active
|
||||
gpio_set(68,-1);
|
||||
gpio_set(69,-1);
|
||||
gpio_set(70,-1);
|
||||
gpio_set(71,-1);
|
||||
|
||||
//start multicast
|
||||
actuators_ardrone_cmd(0xa0,reply,1);
|
||||
actuators_ardrone_cmd(0xa0,reply,1);
|
||||
actuators_ardrone_cmd(0xa0,reply,1);
|
||||
actuators_ardrone_cmd(0xa0,reply,1);
|
||||
actuators_ardrone_cmd(0xa0,reply,1);
|
||||
|
||||
//reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
|
||||
gpio_set(106,-1);
|
||||
gpio_set(107,0);
|
||||
gpio_set(107,1);
|
||||
|
||||
//all leds green
|
||||
// actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
|
||||
}
|
||||
|
||||
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
|
||||
write(mot_fd, &cmd, 1);
|
||||
return read(mot_fd, reply, replylen);
|
||||
}
|
||||
|
||||
void actuators_ardrone_commit(void)
|
||||
{
|
||||
actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]);
|
||||
}
|
||||
|
||||
/**
|
||||
* Write motor speed command
|
||||
* cmd = 001aaaaa aaaabbbb bbbbbccc ccccccdd ddddddd0
|
||||
*/
|
||||
void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3)
|
||||
{
|
||||
uint8_t cmd[5];
|
||||
cmd[0] = 0x20 | ((pwm0&0x1ff)>>4);
|
||||
cmd[1] = ((pwm0&0x1ff)<<4) | ((pwm1&0x1ff)>>5);
|
||||
cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
|
||||
cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
|
||||
cmd[4] = ((pwm3&0x1ff)<<1);
|
||||
write(mot_fd, cmd, 5);
|
||||
}
|
||||
|
||||
/**
|
||||
* Write LED command
|
||||
* cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format)
|
||||
*/
|
||||
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3)
|
||||
{
|
||||
uint8_t cmd[2];
|
||||
cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1);
|
||||
cmd[1]=((led2&3)<<7) | ((led3&3)<<5);
|
||||
write(mot_fd, cmd, 2);
|
||||
}
|
||||
|
||||
void actuators_ardrone_close(void)
|
||||
{
|
||||
close(mot_fd);
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* Original Code from:
|
||||
* Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
|
||||
*
|
||||
* Adapated for Paparazzi by:
|
||||
* Copyright (C) 2012 Dino Hensen <dino.hensen@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/actuators_ardrone2_raw.h
|
||||
* Actuator driver for ardrone2-raw version
|
||||
*/
|
||||
|
||||
#ifndef ACTUATORS_ARDRONE2_RAW_H_
|
||||
#define ACTUATORS_ARDRONE2_RAW_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifndef ACTUATORS_ARDRONE_NB
|
||||
#define ACTUATORS_ARDRONE_NB 4
|
||||
#endif
|
||||
|
||||
#define SERVOS_TICS_OF_USEC(_v) (_v)
|
||||
|
||||
#define ActuatorArdroneSet(_i, _v) { actuators_pwm_values[_i] = _v; }
|
||||
#define ActuatorsArdroneCommit() actuators_ardrone_commit();
|
||||
#define ActuatorsArdroneInit() actuators_ardrone_init();
|
||||
|
||||
#define MOT_LEDOFF 0
|
||||
#define MOT_LEDRED 1
|
||||
#define MOT_LEDGREEN 2
|
||||
#define MOT_LEDORANGE 3
|
||||
|
||||
uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];
|
||||
|
||||
extern void actuators_ardrone_commit(void);
|
||||
extern void actuators_ardrone_init(void);
|
||||
|
||||
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen);
|
||||
void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3);
|
||||
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3);
|
||||
void actuators_ardrone_close(void);
|
||||
|
||||
#endif /* ACTUATORS_ARDRONE2_RAW_H_ */
|
||||
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* 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 boards/ardrone/actuators_at.c
|
||||
* ardrone2-sdk actuators are driven by external software controller by AT-commands
|
||||
*/
|
||||
|
||||
#include "subsystems/ahrs/ahrs_ardrone2.h"
|
||||
#include "actuators_at.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "boards/ardrone/at_com.h"
|
||||
|
||||
void actuators_init(void) {
|
||||
init_at_com();
|
||||
}
|
||||
|
||||
void actuators_set(pprz_t commands[]) {
|
||||
//Calculate the thrus, roll, pitch and yaw from the PPRZ commands
|
||||
float thrust = ((float)(commands[COMMAND_THRUST]-MAX_PPRZ/2) / (float)MAX_PPRZ)*2.0f;
|
||||
float roll = ((float)commands[COMMAND_ROLL] / (float)MAX_PPRZ);
|
||||
float pitch = ((float)commands[COMMAND_PITCH] / (float)MAX_PPRZ);
|
||||
float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ);
|
||||
|
||||
//Starting engine
|
||||
if(thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
|
||||
at_com_send_ref(REF_TAKEOFF);
|
||||
|
||||
//Check emergency or stop engine
|
||||
if((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0)
|
||||
at_com_send_ref(REF_EMERGENCY);
|
||||
else if(thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED))
|
||||
at_com_send_ref(0);
|
||||
|
||||
//Calibration
|
||||
if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
|
||||
at_com_send_calib(0);
|
||||
|
||||
//Moving
|
||||
if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING))
|
||||
at_com_send_pcmd(1, thrust, roll, pitch, yaw);
|
||||
|
||||
//Keep alive (FIXME)
|
||||
at_com_send_config("general:navdata_demo", "FALSE");
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* 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 boards/ardrone/actuators_at.h
|
||||
* ardrone2-sdk actuators are driven by external software controller by AT-commands
|
||||
*/
|
||||
|
||||
#ifndef BOARDS_ARDRONE_ACTUATORS_AT_H
|
||||
#define BOARDS_ARDRONE_ACTUATORS_AT_H
|
||||
|
||||
#include "paparazzi.h"
|
||||
|
||||
extern void actuators_init(void);
|
||||
extern void actuators_set(pprz_t commands[]);
|
||||
#define SetActuatorsFromCommands(commands, AP_MODE) actuators_set(commands)
|
||||
|
||||
#endif /* BOARDS_ARDRONE_ACTUATORS_AT_H */
|
||||
@@ -0,0 +1,194 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* 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 boards/ardrone/at_com.c
|
||||
* Sending and receiving of AT-commands specified by the ardrone API
|
||||
*/
|
||||
|
||||
#include "at_com.h"
|
||||
#include "boards/ardrone2_sdk.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <errno.h>
|
||||
|
||||
int packet_seq = 1; //Packet sequence number
|
||||
|
||||
int at_socket = -1, //AT socket connection
|
||||
navdata_socket = -1; //Navdata socket connection
|
||||
|
||||
struct sockaddr_in pc_addr, //Own pc address
|
||||
drone_at, //Drone AT address
|
||||
drone_nav, //Drone nav address
|
||||
from; //From address
|
||||
|
||||
bool_t at_com_ready = FALSE; //Status of the at communication
|
||||
char sessionId[9]; //THe config session ID
|
||||
|
||||
void at_com_send(char* command);
|
||||
void init_at_config(void);
|
||||
|
||||
//Init the at_com
|
||||
void init_at_com(void) {
|
||||
//Check if already initialized
|
||||
if (at_com_ready)
|
||||
return;
|
||||
|
||||
//Create the at and navdata socket
|
||||
if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
||||
printf("at_com: at_socket error (%s)\n", strerror(errno));
|
||||
}
|
||||
if ((navdata_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
|
||||
printf("at_com: navdata_socket error (%s)\n", strerror(errno));
|
||||
}
|
||||
|
||||
//For recvfrom
|
||||
pc_addr.sin_family = AF_INET;
|
||||
pc_addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
pc_addr.sin_port = htons(9800);
|
||||
|
||||
//For sendto AT
|
||||
drone_at.sin_family = AF_INET;
|
||||
drone_at.sin_addr.s_addr = inet_addr(ARDRONE_IP);
|
||||
drone_at.sin_port = htons(ARDRONE_AT_PORT);
|
||||
|
||||
//For sendto navadata init
|
||||
drone_nav.sin_family = AF_INET;
|
||||
drone_nav.sin_addr.s_addr = inet_addr(ARDRONE_IP);
|
||||
drone_nav.sin_port = htons(ARDRONE_NAVDATA_PORT);
|
||||
|
||||
//Bind the navdata socket
|
||||
if (bind(navdata_socket, (struct sockaddr *) &pc_addr, sizeof(pc_addr)) < 0) {
|
||||
printf("at_com: bind error (%s)\n", strerror(errno));
|
||||
}
|
||||
|
||||
//Set unicast mode on
|
||||
int one = 1;
|
||||
sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav,
|
||||
sizeof(drone_nav));
|
||||
|
||||
//Init at config
|
||||
init_at_config();
|
||||
|
||||
//Set at_com to ready
|
||||
at_com_ready = TRUE;
|
||||
}
|
||||
|
||||
//Init the at config
|
||||
void init_at_config(void) {
|
||||
//Generate a session id
|
||||
uint32_t binaryId = (uint32_t) rand();
|
||||
binaryId = (0 != binaryId) ? binaryId : 1u;
|
||||
snprintf(sessionId, 9, "%08x", binaryId);
|
||||
sessionId[8] = '\0';
|
||||
|
||||
//Send session, application and user id:
|
||||
at_com_send_config("custom:session_id", sessionId);
|
||||
at_com_send_config("custom:application_id", "9D7BFD45");
|
||||
at_com_send_config("custom:profile_id", "2BF07F58");
|
||||
|
||||
//Send config values
|
||||
at_com_send_config("control:euler_angle_max", "0.52");
|
||||
at_com_send_config("control:altitude_max", "20000");
|
||||
at_com_send_config("control:control_vz_max", "2000");
|
||||
at_com_send_config("control:control_yaw", "6.11");
|
||||
|
||||
//Send config values with the airframe.h
|
||||
#ifndef ARDRONE_FLIGHT_INDOOR
|
||||
at_com_send_config("control:outdoor", "TRUE");
|
||||
#else
|
||||
at_com_send_config("control:outdoor","FALSE");
|
||||
#endif
|
||||
#ifndef ARDRONE_WITHOUT_SHELL
|
||||
at_com_send_config("control:flight_without_shell", "FALSE");
|
||||
#else
|
||||
at_com_send_config("control:flight_without_shell","TRUE");
|
||||
#endif
|
||||
#ifdef ARDRONE_OWNER_MAC
|
||||
at_com_send_config("network:owner_mac",ARDRONE_OWNER_MAC);
|
||||
#endif
|
||||
}
|
||||
|
||||
//Recieve a navdata packet
|
||||
void at_com_recieve_navdata(unsigned char* buffer) {
|
||||
int l;
|
||||
recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
|
||||
(struct sockaddr *) &from, (socklen_t *) &l);
|
||||
}
|
||||
|
||||
//Send an AT command
|
||||
void at_com_send(char* command) {
|
||||
sendto(at_socket, command, strlen(command), 0, (struct sockaddr*) &drone_at,
|
||||
sizeof(drone_at));
|
||||
}
|
||||
|
||||
//Send a Config
|
||||
void at_com_send_config(char* key, char* value) {
|
||||
char command[256];
|
||||
sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r",
|
||||
packet_seq++, sessionId);
|
||||
at_com_send(command);
|
||||
sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value);
|
||||
at_com_send(command);
|
||||
}
|
||||
|
||||
//Send a Flat trim
|
||||
void at_com_send_ftrim(void) {
|
||||
char command[256];
|
||||
sprintf(command, "AT*FTRIM=%d\r", packet_seq++);
|
||||
at_com_send(command);
|
||||
}
|
||||
|
||||
//Send a Ref
|
||||
void at_com_send_ref(int bits) {
|
||||
char command[256];
|
||||
sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT);
|
||||
at_com_send(command);
|
||||
}
|
||||
|
||||
//Send a Pcmd
|
||||
void at_com_send_pcmd(int mode, float thrust, float roll, float pitch,
|
||||
float yaw) {
|
||||
int f_thrust, f_roll, f_pitch, f_yaw;
|
||||
char command[256];
|
||||
|
||||
//Change the floats to ints(dereferencing)
|
||||
memcpy(&f_thrust, &thrust, sizeof thrust);
|
||||
memcpy(&f_roll, &roll, sizeof roll);
|
||||
memcpy(&f_pitch, &pitch, sizeof pitch);
|
||||
memcpy(&f_yaw, &yaw, sizeof yaw);
|
||||
|
||||
sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll,
|
||||
f_pitch, f_thrust, f_yaw);
|
||||
at_com_send(command);
|
||||
}
|
||||
|
||||
//Send a Calib
|
||||
void at_com_send_calib(int device) {
|
||||
char command[256];
|
||||
sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device);
|
||||
at_com_send(command);
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* 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 boards/ardrone/at_com.h
|
||||
* Sending and receiving of AT-commands specified by the ardrone API
|
||||
*/
|
||||
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
#ifndef BOARDS_ARDRONE_AT_COM_H
|
||||
#define BOARDS_ARDRONE_AT_COM_H
|
||||
|
||||
//Define the AT_REF bits
|
||||
typedef enum {
|
||||
REF_TAKEOFF = 1U << 9,
|
||||
REF_EMERGENCY = 1U << 8,
|
||||
REF_DEFAULT = 0x11540000
|
||||
} AT_REFS;
|
||||
|
||||
//Define control states
|
||||
typedef enum {
|
||||
CTRL_DEFAULT,
|
||||
CTRL_INIT,
|
||||
CTRL_LANDED,
|
||||
CTRL_FLYING,
|
||||
CTRL_HOVERING,
|
||||
CTRL_TEST,
|
||||
CTRL_TRANS_TAKEOFF,
|
||||
CTRL_TRANS_GOTOFIX,
|
||||
CTRL_TRANS_LANDING,
|
||||
CTRL_TRANS_LOOPING,
|
||||
CTRL_NUM_STATES
|
||||
} CTRL_STATES;
|
||||
|
||||
//Define the AR.Drone states
|
||||
typedef enum {
|
||||
ARDRONE_FLY_MASK = 1U << 0, /*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */
|
||||
ARDRONE_VIDEO_MASK = 1U << 1, /*!< VIDEO MASK : (0) video disable, (1) video enable */
|
||||
ARDRONE_VISION_MASK = 1U << 2, /*!< VISION MASK : (0) vision disable, (1) vision enable */
|
||||
ARDRONE_CONTROL_MASK = 1U << 3, /*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */
|
||||
ARDRONE_ALTITUDE_MASK = 1U << 4, /*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
|
||||
ARDRONE_USER_FEEDBACK_START = 1U << 5, /*!< USER feedback : Start button state */
|
||||
ARDRONE_COMMAND_MASK = 1U << 6, /*!< Control command ACK : (0) None, (1) one received */
|
||||
ARDRONE_CAMERA_MASK = 1U << 7, /*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */
|
||||
ARDRONE_TRAVELLING_MASK = 1U << 8, /*!< Travelling mask : (0) disable, (1) enable */
|
||||
ARDRONE_USB_MASK = 1U << 9, /*!< USB key : (0) usb key not ready, (1) usb key ready */
|
||||
ARDRONE_NAVDATA_DEMO_MASK = 1U << 10, /*!< Navdata demo : (0) All navdata, (1) only navdata demo */
|
||||
ARDRONE_NAVDATA_BOOTSTRAP = 1U << 11, /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
|
||||
ARDRONE_MOTORS_MASK = 1U << 12, /*!< Motors status : (0) Ok, (1) Motors problem */
|
||||
ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
|
||||
ARDRONE_SOFTWARE_FAULT = 1U << 14, /*!< Software fault detected - user should land as quick as possible (1) */
|
||||
ARDRONE_VBAT_LOW = 1U << 15, /*!< VBat low : (1) too low, (0) Ok */
|
||||
ARDRONE_USER_EL = 1U << 16, /*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
|
||||
ARDRONE_TIMER_ELAPSED = 1U << 17, /*!< Timer elapsed : (1) elapsed, (0) not elapsed */
|
||||
ARDRONE_MAGNETO_NEEDS_CALIB = 1U << 18, /*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */
|
||||
ARDRONE_ANGLES_OUT_OF_RANGE = 1U << 19, /*!< Angles : (0) Ok, (1) out of range */
|
||||
ARDRONE_WIND_MASK = 1U << 20, /*!< WIND MASK: (0) ok, (1) Too much wind */
|
||||
ARDRONE_ULTRASOUND_MASK = 1U << 21, /*!< Ultrasonic sensor : (0) Ok, (1) deaf */
|
||||
ARDRONE_CUTOUT_MASK = 1U << 22, /*!< Cutout system detection : (0) Not detected, (1) detected */
|
||||
ARDRONE_PIC_VERSION_MASK = 1U << 23, /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
|
||||
ARDRONE_ATCODEC_THREAD_ON = 1U << 24, /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
|
||||
ARDRONE_NAVDATA_THREAD_ON = 1U << 25, /*!< Navdata thread ON : (0) thread OFF (1) thread ON */
|
||||
ARDRONE_VIDEO_THREAD_ON = 1U << 26, /*!< Video thread ON : (0) thread OFF (1) thread ON */
|
||||
ARDRONE_ACQ_THREAD_ON = 1U << 27, /*!< Acquisition thread ON : (0) thread OFF (1) thread ON */
|
||||
ARDRONE_CTRL_WATCHDOG_MASK = 1U << 28, /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
|
||||
ARDRONE_ADC_WATCHDOG_MASK = 1U << 29, /*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
|
||||
ARDRONE_COM_WATCHDOG_MASK = 1U << 30, /*!< Communication Watchdog : (1) com problem, (0) Com is ok */
|
||||
ARDRONE_EMERGENCY_MASK = 1U << 31 /*!< Emergency landing : (0) no emergency, (1) emergency */
|
||||
} ARDRONE_STATES;
|
||||
|
||||
//Navdata option packet without data
|
||||
typedef struct _navdata_option_t {
|
||||
uint16_t tag;
|
||||
uint16_t size;
|
||||
uint8_t data[1];
|
||||
} __attribute__ ((packed)) navdata_option_t;
|
||||
|
||||
//Main navdata packet
|
||||
typedef struct _navdata_t {
|
||||
uint32_t header; /*!< Always set to NAVDATA_HEADER */
|
||||
uint32_t ardrone_state; /*!< Bit mask built from def_ardrone_state_mask_t */
|
||||
uint32_t sequence; /*!< Sequence number, incremented for each sent packet */
|
||||
uint32_t vision_defined;
|
||||
|
||||
navdata_option_t options[1];
|
||||
} __attribute__ ((packed)) navdata_t;
|
||||
|
||||
//Navdata checksum packet
|
||||
typedef struct _navdata_cks_t {
|
||||
uint16_t tag;
|
||||
uint16_t size;
|
||||
uint32_t cks;
|
||||
} __attribute__ ((packed)) navdata_cks_t;
|
||||
|
||||
//Navdata demo option
|
||||
typedef struct _navdata_demo_t {
|
||||
uint16_t tag; /*!< Navdata block ('option') identifier */
|
||||
uint16_t size; /*!< set this to the size of this structure */
|
||||
uint32_t ctrl_state; /*!< Flying state (landed, flying, hovering, etc.) defined in CTRL_STATES enum. */
|
||||
uint32_t vbat_flying_percentage; /*!< battery voltage filtered (mV) */
|
||||
float theta; /*!< UAV's pitch in milli-degrees */
|
||||
float phi; /*!< UAV's roll in milli-degrees */
|
||||
float psi; /*!< UAV's yaw in milli-degrees */
|
||||
int32_t altitude; /*!< UAV's altitude in centimeters */
|
||||
float vx; /*!< UAV's estimated linear velocity */
|
||||
float vy; /*!< UAV's estimated linear velocity */
|
||||
float vz; /*!< UAV's estimated linear velocity */
|
||||
uint32_t num_frames; /*!< streamed frame index */ // Not used -> To integrate in video stage.
|
||||
// Camera parameters compute by detection
|
||||
struct FloatMat33 detection_camera_rot; /*!< Deprecated ! Don't use ! */
|
||||
struct FloatVect3 detection_camera_trans; /*!< Deprecated ! Don't use ! */
|
||||
uint32_t detection_tag_index; /*!< Deprecated ! Don't use ! */
|
||||
uint32_t detection_camera_type; /*!< Type of tag searched in detection */
|
||||
// Camera parameters compute by drone
|
||||
struct FloatMat33 drone_camera_rot; /*!< Deprecated ! Don't use ! */
|
||||
struct FloatVect3 drone_camera_trans; /*!< Deprecated ! Don't use ! */
|
||||
} __attribute__ ((packed)) navdata_demo_t;
|
||||
|
||||
//Navdata physical measures option
|
||||
typedef struct _navdata_phys_measures_t {
|
||||
uint16_t tag;
|
||||
uint16_t size;
|
||||
|
||||
float accs_temp;
|
||||
uint16_t gyro_temp;
|
||||
struct FloatVect3 phys_accs;
|
||||
struct FloatVect3 phys_gyros;
|
||||
uint32_t alim3V3; // 3.3volt alim [LSB]
|
||||
uint32_t vrefEpson; // ref volt Epson gyro [LSB]
|
||||
uint32_t vrefIDG; // ref volt IDG gyro [LSB]
|
||||
} __attribute__ ((packed)) navdata_phys_measures_t;
|
||||
|
||||
//External functions
|
||||
extern void init_at_com(void);
|
||||
extern void at_com_recieve_navdata(unsigned char* buffer);
|
||||
extern void at_com_send_config(char* key, char* value);
|
||||
extern void at_com_send_ftrim(void);
|
||||
extern void at_com_send_ref(int bits);
|
||||
extern void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw);
|
||||
extern void at_com_send_calib(int device);
|
||||
|
||||
#endif /* BOARDS_ARDRONE_AT_COM_H */
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* Copyright (C) 2012 TU Delft Quatrotor Team 1
|
||||
*
|
||||
* 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 boards/ardrone/baro_board.c
|
||||
* Paparazzi AR Drone 2 Baro Sensor implementation:.
|
||||
*
|
||||
* These functions are mostly empty because of the calibration and calculations
|
||||
* done by the Parrot Navigation board.
|
||||
*/
|
||||
|
||||
#include "subsystems/sensors/baro.h"
|
||||
#include "baro_board.h"
|
||||
|
||||
struct Baro baro;
|
||||
|
||||
void baro_init(void) {
|
||||
baro.status = BS_UNINITIALIZED;
|
||||
baro.absolute = 0;
|
||||
baro.differential = 0;
|
||||
baro_data_available = 0;
|
||||
}
|
||||
|
||||
void baro_periodic(void) {
|
||||
baro.status = BS_RUNNING;
|
||||
if(navdata_baro_available == 1) {
|
||||
navdata_baro_available = 0;
|
||||
// baro.absolute = navdata->pressure; // When this is un-commented the ardrone gets a pressure
|
||||
// TODO do the right calculations for the right absolute pressure
|
||||
baro.absolute = 0;
|
||||
baro_data_available = TRUE;
|
||||
}
|
||||
else {
|
||||
baro_data_available = FALSE;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Copyright (C) 2012 TU Delft Quatrotor Team 1
|
||||
*
|
||||
* 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 boards/ardrone/baro_board.h
|
||||
* Paparazzi AR Drone 2 Baro Sensor implementation:.
|
||||
*
|
||||
* These functions are mostly empty because of the calibration and calculations
|
||||
* done by the Parrot Navigation board.
|
||||
*/
|
||||
|
||||
#ifndef BOARDS_ARDRONE2_BARO_H
|
||||
#define BOARDS_ARDRONE2_BARO_H
|
||||
|
||||
#if BOARD_HAS_BARO
|
||||
#include "navdata.h"
|
||||
|
||||
int baro_data_available;
|
||||
|
||||
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
|
||||
if (baro_data_available) {
|
||||
b_abs_handler();
|
||||
}
|
||||
}
|
||||
|
||||
#define BaroEvent(_b_abs_handler, _b_diff_handler) {\
|
||||
baro_event(_b_abs_handler,_b_diff_handler);\
|
||||
}
|
||||
#else
|
||||
#define BaroEvent(_b_abs_handler, _b_diff_handler) {}
|
||||
#endif
|
||||
|
||||
#endif /* BOARDS_ARDRONE2_BARO_H */
|
||||
@@ -0,0 +1,40 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Dino Hensen
|
||||
*
|
||||
* 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 boards/ardrone/baro_board_dummy.c
|
||||
* Dummy Baro Board.
|
||||
*
|
||||
* These functions are mostly empty because this is a dummy.
|
||||
*/
|
||||
|
||||
#include "subsystems/sensors/baro.h"
|
||||
|
||||
struct Baro baro;
|
||||
|
||||
void baro_init(void) {
|
||||
baro.status = BS_UNINITIALIZED;
|
||||
baro.absolute = 0;
|
||||
baro.differential = 0;
|
||||
}
|
||||
|
||||
void baro_periodic(void) {
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
*
|
||||
* Copyright (C) 2009-2013 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 boards/ardrone/electrical_dummy.c
|
||||
* dummy electrical status readings for ardrone-sdk version.
|
||||
*
|
||||
* Because ardrone2-sdk version does its battery updating in ahrs_adrone2.c.
|
||||
*/
|
||||
|
||||
#include "subsystems/electrical.h"
|
||||
|
||||
struct Electrical electrical;
|
||||
|
||||
void electrical_init(void) { }
|
||||
|
||||
void electrical_periodic(void) { }
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/gpio.c
|
||||
* ardrone GPIO driver
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include "gpio.h"
|
||||
|
||||
//val=0 -> set gpio output lo
|
||||
//val=1 -> set gpio output hi
|
||||
//val=-1 -> set gpio as input (output hi-Z)
|
||||
int gpio_set(int nr,int val)
|
||||
{
|
||||
char cmdline[200];
|
||||
if(val<0) sprintf(cmdline,"/usr/sbin/gpio %d -d i",nr);
|
||||
else if(val>0) sprintf(cmdline,"/usr/sbin/gpio %d -d ho 1",nr);
|
||||
else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr);
|
||||
return system(cmdline);
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/gpio.h
|
||||
* ardrone GPIO driver
|
||||
*/
|
||||
|
||||
#ifndef GPIO_H
|
||||
#define GPIO_H
|
||||
|
||||
//val=0 -> set gpio output lo
|
||||
//val=1 -> set gpio output hi
|
||||
//val=-1 -> set gpio as input (output hi-Z)
|
||||
int gpio_set(int nr,int val);
|
||||
|
||||
#endif /* GPIO_H */
|
||||
@@ -0,0 +1,335 @@
|
||||
/*
|
||||
* i2c-dev.h - i2c-bus driver, char device interface
|
||||
*
|
||||
* Copyright (C) 1995-97 Simon G. Vogl
|
||||
* Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl>
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/* $Id: i2c-dev.h 5361 2008-10-19 09:47:02Z khali $ */
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/i2c-dev.h
|
||||
* I2C-bus driver
|
||||
*/
|
||||
|
||||
#ifndef LIB_I2CDEV_H
|
||||
#define LIB_I2CDEV_H
|
||||
|
||||
#include <linux/types.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
/*
|
||||
* I2C Message - used for pure i2c transaction, also from /dev interface
|
||||
*/
|
||||
struct i2c_msg {
|
||||
__u16 addr; /* slave address */
|
||||
unsigned short flags;
|
||||
#define I2C_M_TEN 0x10 /* we have a ten bit chip address */
|
||||
#define I2C_M_RD 0x01
|
||||
#define I2C_M_NOSTART 0x4000
|
||||
#define I2C_M_REV_DIR_ADDR 0x2000
|
||||
#define I2C_M_IGNORE_NAK 0x1000
|
||||
#define I2C_M_NO_RD_ACK 0x0800
|
||||
short len; /* msg length */
|
||||
char *buf; /* pointer to msg data */
|
||||
};
|
||||
|
||||
/* To determine what functionality is present */
|
||||
|
||||
#define I2C_FUNC_I2C 0x00000001
|
||||
#define I2C_FUNC_10BIT_ADDR 0x00000002
|
||||
#define I2C_FUNC_PROTOCOL_MANGLING 0x00000004 /* I2C_M_{REV_DIR_ADDR,NOSTART,..} */
|
||||
#define I2C_FUNC_SMBUS_PEC 0x00000008
|
||||
#define I2C_FUNC_SMBUS_BLOCK_PROC_CALL 0x00008000 /* SMBus 2.0 */
|
||||
#define I2C_FUNC_SMBUS_QUICK 0x00010000
|
||||
#define I2C_FUNC_SMBUS_READ_BYTE 0x00020000
|
||||
#define I2C_FUNC_SMBUS_WRITE_BYTE 0x00040000
|
||||
#define I2C_FUNC_SMBUS_READ_BYTE_DATA 0x00080000
|
||||
#define I2C_FUNC_SMBUS_WRITE_BYTE_DATA 0x00100000
|
||||
#define I2C_FUNC_SMBUS_READ_WORD_DATA 0x00200000
|
||||
#define I2C_FUNC_SMBUS_WRITE_WORD_DATA 0x00400000
|
||||
#define I2C_FUNC_SMBUS_PROC_CALL 0x00800000
|
||||
#define I2C_FUNC_SMBUS_READ_BLOCK_DATA 0x01000000
|
||||
#define I2C_FUNC_SMBUS_WRITE_BLOCK_DATA 0x02000000
|
||||
#define I2C_FUNC_SMBUS_READ_I2C_BLOCK 0x04000000 /* I2C-like block xfer */
|
||||
#define I2C_FUNC_SMBUS_WRITE_I2C_BLOCK 0x08000000 /* w/ 1-byte reg. addr. */
|
||||
|
||||
#define I2C_FUNC_SMBUS_BYTE (I2C_FUNC_SMBUS_READ_BYTE | \
|
||||
I2C_FUNC_SMBUS_WRITE_BYTE)
|
||||
#define I2C_FUNC_SMBUS_BYTE_DATA (I2C_FUNC_SMBUS_READ_BYTE_DATA | \
|
||||
I2C_FUNC_SMBUS_WRITE_BYTE_DATA)
|
||||
#define I2C_FUNC_SMBUS_WORD_DATA (I2C_FUNC_SMBUS_READ_WORD_DATA | \
|
||||
I2C_FUNC_SMBUS_WRITE_WORD_DATA)
|
||||
#define I2C_FUNC_SMBUS_BLOCK_DATA (I2C_FUNC_SMBUS_READ_BLOCK_DATA | \
|
||||
I2C_FUNC_SMBUS_WRITE_BLOCK_DATA)
|
||||
#define I2C_FUNC_SMBUS_I2C_BLOCK (I2C_FUNC_SMBUS_READ_I2C_BLOCK | \
|
||||
I2C_FUNC_SMBUS_WRITE_I2C_BLOCK)
|
||||
|
||||
/* Old name, for compatibility */
|
||||
#define I2C_FUNC_SMBUS_HWPEC_CALC I2C_FUNC_SMBUS_PEC
|
||||
|
||||
/*
|
||||
* Data for SMBus Messages
|
||||
*/
|
||||
#define I2C_SMBUS_BLOCK_MAX 32 /* As specified in SMBus standard */
|
||||
#define I2C_SMBUS_I2C_BLOCK_MAX 32 /* Not specified but we use same structure */
|
||||
union i2c_smbus_data {
|
||||
__u8 byte;
|
||||
__u16 word;
|
||||
__u8 block[I2C_SMBUS_BLOCK_MAX + 2]; /* block[0] is used for length */
|
||||
/* and one more for PEC */
|
||||
};
|
||||
|
||||
/* smbus_access read or write markers */
|
||||
#define I2C_SMBUS_READ 1
|
||||
#define I2C_SMBUS_WRITE 0
|
||||
|
||||
/* SMBus transaction types (size parameter in the above functions)
|
||||
Note: these no longer correspond to the (arbitrary) PIIX4 internal codes! */
|
||||
#define I2C_SMBUS_QUICK 0
|
||||
#define I2C_SMBUS_BYTE 1
|
||||
#define I2C_SMBUS_BYTE_DATA 2
|
||||
#define I2C_SMBUS_WORD_DATA 3
|
||||
#define I2C_SMBUS_PROC_CALL 4
|
||||
#define I2C_SMBUS_BLOCK_DATA 5
|
||||
#define I2C_SMBUS_I2C_BLOCK_BROKEN 6
|
||||
#define I2C_SMBUS_BLOCK_PROC_CALL 7 /* SMBus 2.0 */
|
||||
#define I2C_SMBUS_I2C_BLOCK_DATA 8
|
||||
|
||||
|
||||
/* ----- commands for the ioctl like i2c_command call:
|
||||
* note that additional calls are defined in the algorithm and hw
|
||||
* dependent layers - these can be listed here, or see the
|
||||
* corresponding header files.
|
||||
*/
|
||||
/* -> bit-adapter specific ioctls */
|
||||
#define I2C_RETRIES 0x0701 /* number of times a device address */
|
||||
/* should be polled when not */
|
||||
/* acknowledging */
|
||||
#define I2C_TIMEOUT 0x0702 /* set timeout - call with int */
|
||||
|
||||
|
||||
/* this is for i2c-dev.c */
|
||||
#define I2C_SLAVE 0x0703 /* Change slave address */
|
||||
/* Attn.: Slave address is 7 or 10 bits */
|
||||
#define I2C_SLAVE_FORCE 0x0706 /* Change slave address */
|
||||
/* Attn.: Slave address is 7 or 10 bits */
|
||||
/* This changes the address, even if it */
|
||||
/* is already taken! */
|
||||
#define I2C_TENBIT 0x0704 /* 0 for 7 bit addrs, != 0 for 10 bit */
|
||||
|
||||
#define I2C_FUNCS 0x0705 /* Get the adapter functionality */
|
||||
#define I2C_RDWR 0x0707 /* Combined R/W transfer (one stop only)*/
|
||||
#define I2C_PEC 0x0708 /* != 0 for SMBus PEC */
|
||||
|
||||
#define I2C_SMBUS 0x0720 /* SMBus-level access */
|
||||
|
||||
/* -- i2c.h -- */
|
||||
|
||||
|
||||
/* Note: 10-bit addresses are NOT supported! */
|
||||
|
||||
/* This is the structure as used in the I2C_SMBUS ioctl call */
|
||||
struct i2c_smbus_ioctl_data {
|
||||
char read_write;
|
||||
__u8 command;
|
||||
int size;
|
||||
union i2c_smbus_data *data;
|
||||
};
|
||||
|
||||
/* This is the structure as used in the I2C_RDWR ioctl call */
|
||||
struct i2c_rdwr_ioctl_data {
|
||||
struct i2c_msg *msgs; /* pointers to i2c_msgs */
|
||||
int nmsgs; /* number of i2c_msgs */
|
||||
};
|
||||
|
||||
|
||||
static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command,
|
||||
int size, union i2c_smbus_data *data)
|
||||
{
|
||||
struct i2c_smbus_ioctl_data args;
|
||||
|
||||
args.read_write = read_write;
|
||||
args.command = command;
|
||||
args.size = size;
|
||||
args.data = data;
|
||||
return ioctl(file,I2C_SMBUS,&args);
|
||||
}
|
||||
|
||||
|
||||
static inline __s32 i2c_smbus_write_quick(int file, __u8 value)
|
||||
{
|
||||
return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,NULL);
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_read_byte(int file)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_READ,0,I2C_SMBUS_BYTE,&data))
|
||||
return -1;
|
||||
else
|
||||
return 0x0FF & data.byte;
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_write_byte(int file, __u8 value)
|
||||
{
|
||||
return i2c_smbus_access(file,I2C_SMBUS_WRITE,value,
|
||||
I2C_SMBUS_BYTE,NULL);
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
|
||||
I2C_SMBUS_BYTE_DATA,&data))
|
||||
return -1;
|
||||
else
|
||||
return 0x0FF & data.byte;
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command,
|
||||
__u8 value)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
data.byte = value;
|
||||
return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_BYTE_DATA, &data);
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_read_word_data(int file, __u8 command)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
|
||||
I2C_SMBUS_WORD_DATA,&data))
|
||||
return -1;
|
||||
else
|
||||
return 0x0FFFF & data.word;
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_write_word_data(int file, __u8 command,
|
||||
__u16 value)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
data.word = value;
|
||||
return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_WORD_DATA, &data);
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
data.word = value;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_PROC_CALL,&data))
|
||||
return -1;
|
||||
else
|
||||
return 0x0FFFF & data.word;
|
||||
}
|
||||
|
||||
|
||||
/* Returns the number of read bytes */
|
||||
static inline __s32 i2c_smbus_read_block_data(int file, __u8 command,
|
||||
__u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
int i;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
|
||||
I2C_SMBUS_BLOCK_DATA,&data))
|
||||
return -1;
|
||||
else {
|
||||
for (i = 1; i <= data.block[0]; i++)
|
||||
values[i-1] = data.block[i];
|
||||
return data.block[0];
|
||||
}
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_write_block_data(int file, __u8 command,
|
||||
__u8 length, __u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
int i;
|
||||
if (length > 32)
|
||||
length = 32;
|
||||
for (i = 1; i <= length; i++)
|
||||
data.block[i] = values[i-1];
|
||||
data.block[0] = length;
|
||||
return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_BLOCK_DATA, &data);
|
||||
}
|
||||
|
||||
/* Returns the number of read bytes */
|
||||
/* Until kernel 2.6.22, the length is hardcoded to 32 bytes. If you
|
||||
ask for less than 32 bytes, your code will only work with kernels
|
||||
2.6.23 and later. */
|
||||
static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command,
|
||||
__u8 length, __u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
int i;
|
||||
|
||||
if (length > 32)
|
||||
length = 32;
|
||||
data.block[0] = length;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_READ,command,
|
||||
length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN :
|
||||
I2C_SMBUS_I2C_BLOCK_DATA,&data))
|
||||
return -1;
|
||||
else {
|
||||
for (i = 1; i <= data.block[0]; i++)
|
||||
values[i-1] = data.block[i];
|
||||
return data.block[0];
|
||||
}
|
||||
}
|
||||
|
||||
static inline __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command,
|
||||
__u8 length, __u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
int i;
|
||||
if (length > 32)
|
||||
length = 32;
|
||||
for (i = 1; i <= length; i++)
|
||||
data.block[i] = values[i-1];
|
||||
data.block[0] = length;
|
||||
return i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_I2C_BLOCK_BROKEN, &data);
|
||||
}
|
||||
|
||||
/* Returns the number of read bytes */
|
||||
static inline __s32 i2c_smbus_block_process_call(int file, __u8 command,
|
||||
__u8 length, __u8 *values)
|
||||
{
|
||||
union i2c_smbus_data data;
|
||||
int i;
|
||||
if (length > 32)
|
||||
length = 32;
|
||||
for (i = 1; i <= length; i++)
|
||||
data.block[i] = values[i-1];
|
||||
data.block[0] = length;
|
||||
if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command,
|
||||
I2C_SMBUS_BLOCK_PROC_CALL,&data))
|
||||
return -1;
|
||||
else {
|
||||
for (i = 1; i <= data.block[0]; i++)
|
||||
values[i-1] = data.block[i];
|
||||
return data.block[0];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif /* LIB_I2CDEV_H */
|
||||
@@ -0,0 +1,218 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Dino Hensen, Vincent van Hoek
|
||||
*
|
||||
* 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 boards/ardrone/navdata.c
|
||||
* ardrone2 navdata aquisition driver.
|
||||
*
|
||||
* The ardrone2 provides a navdata stream of packets
|
||||
* containing info about all sensors at a rate of 200Hz.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h> // for O_RDWR, O_NOCTTY, O_NONBLOCK
|
||||
#include <termios.h> // for baud rates and options
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include "navdata.h"
|
||||
|
||||
int nav_fd;
|
||||
|
||||
int navdata_init()
|
||||
{
|
||||
port = malloc(sizeof(navdata_port));
|
||||
|
||||
nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
if (nav_fd == -1)
|
||||
{
|
||||
perror("navdata_init: Unable to open /dev/ttyO1 - ");
|
||||
return 1;
|
||||
} else {
|
||||
port->isOpen = 1;
|
||||
}
|
||||
|
||||
fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
|
||||
//set port options
|
||||
struct termios options;
|
||||
//Get the current options for the port
|
||||
tcgetattr(nav_fd, &options);
|
||||
//Set the baud rates to 460800
|
||||
cfsetispeed(&options, B460800);
|
||||
cfsetospeed(&options, B460800);
|
||||
|
||||
options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
|
||||
options.c_iflag = 0; //clear input options
|
||||
options.c_lflag = 0; //clear local options
|
||||
options.c_oflag &= ~OPOST; //clear output options (raw output)
|
||||
|
||||
//Set the new options for the port
|
||||
tcsetattr(nav_fd, TCSANOW, &options);
|
||||
|
||||
// stop acquisition
|
||||
uint8_t cmd=0x02;
|
||||
write(nav_fd, &cmd, 1);
|
||||
|
||||
// start acquisition
|
||||
cmd=0x01;
|
||||
write(nav_fd, &cmd, 1);
|
||||
|
||||
navdata = malloc(sizeof(measures_t));
|
||||
navdata_imu_available = 0;
|
||||
navdata_baro_available = 0;
|
||||
|
||||
port->bytesRead = 0;
|
||||
port->totalBytesRead = 0;
|
||||
port->packetsRead = 0;
|
||||
port->isInitialized = 1;
|
||||
|
||||
previousUltrasoundHeight = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void navdata_close()
|
||||
{
|
||||
port->isOpen = 0;
|
||||
close(nav_fd);
|
||||
}
|
||||
|
||||
void navdata_read()
|
||||
{
|
||||
int newbytes = 0;
|
||||
|
||||
if (port->isInitialized != 1)
|
||||
navdata_init();
|
||||
|
||||
if (port->isOpen != 1)
|
||||
return;
|
||||
|
||||
newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead);
|
||||
|
||||
// because non-blocking read returns -1 when no bytes available
|
||||
if (newbytes > 0)
|
||||
{
|
||||
port->bytesRead += newbytes;
|
||||
port->totalBytesRead += newbytes;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void navdata_update()
|
||||
{
|
||||
navdata_read();
|
||||
|
||||
// while there is something interesting to do...
|
||||
while (port->bytesRead >= 60)
|
||||
{
|
||||
if (port->buffer[0] == NAVDATA_START_BYTE)
|
||||
{
|
||||
// if checksum is OK
|
||||
if ( 1 ) // we dont know how to calculate the checksum
|
||||
// if ( navdata_checksum() == 0 )
|
||||
{
|
||||
memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE);
|
||||
navdata_imu_available = 1;
|
||||
navdata_baro_available = 1;
|
||||
port->packetsRead++;
|
||||
// printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
|
||||
navdata_getHeight();
|
||||
}
|
||||
navdata_CropBuffer(60);
|
||||
}
|
||||
else
|
||||
{
|
||||
// find start byte, copy all data from startbyte to buffer origin, update bytesread
|
||||
uint8_t * pint;
|
||||
pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
|
||||
|
||||
if (pint != NULL) {
|
||||
port->bytesRead -= pint - port->buffer;
|
||||
navdata_CropBuffer(pint - port->buffer);
|
||||
} else {
|
||||
// if the start byte was not found, it means there is junk in the buffer
|
||||
port->bytesRead = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void navdata_CropBuffer(int cropsize)
|
||||
{
|
||||
if (port->bytesRead - cropsize < 0) {
|
||||
// TODO think about why the amount of bytes read minus the cropsize gets below zero
|
||||
printf("BytesRead - Cropsize may not be below zero...");
|
||||
return;
|
||||
}
|
||||
|
||||
memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
|
||||
port->bytesRead -= cropsize;
|
||||
}
|
||||
|
||||
int16_t navdata_getHeight() {
|
||||
|
||||
if (navdata->ultrasound > 10000) {
|
||||
return previousUltrasoundHeight;
|
||||
}
|
||||
|
||||
int16_t ultrasoundHeight = 0;
|
||||
|
||||
ultrasoundHeight = (navdata->ultrasound - 880) / 26.553;
|
||||
|
||||
previousUltrasoundHeight = ultrasoundHeight;
|
||||
|
||||
return ultrasoundHeight;
|
||||
}
|
||||
|
||||
// The checksum should be calculated here: we don't know the algorithm
|
||||
uint16_t navdata_checksum() {
|
||||
navdata_cks = 0;
|
||||
navdata_cks += navdata->nu_trame;
|
||||
navdata_cks += navdata->ax;
|
||||
navdata_cks += navdata->ay;
|
||||
navdata_cks += navdata->az;
|
||||
navdata_cks += navdata->vx;
|
||||
navdata_cks += navdata->vy;
|
||||
navdata_cks += navdata->vz;
|
||||
navdata_cks += navdata->temperature_acc;
|
||||
navdata_cks += navdata->temperature_gyro;
|
||||
navdata_cks += navdata->ultrasound;
|
||||
navdata_cks += navdata->us_debut_echo;
|
||||
navdata_cks += navdata->us_fin_echo;
|
||||
navdata_cks += navdata->us_association_echo;
|
||||
navdata_cks += navdata->us_distance_echo;
|
||||
navdata_cks += navdata->us_curve_time;
|
||||
navdata_cks += navdata->us_curve_value;
|
||||
navdata_cks += navdata->us_curve_ref;
|
||||
navdata_cks += navdata->nb_echo;
|
||||
navdata_cks += navdata->sum_echo;
|
||||
navdata_cks += navdata->gradient;
|
||||
navdata_cks += navdata->flag_echo_ini;
|
||||
navdata_cks += navdata->pressure;
|
||||
navdata_cks += navdata->temperature_pressure;
|
||||
navdata_cks += navdata->mx;
|
||||
navdata_cks += navdata->my;
|
||||
navdata_cks += navdata->mz;
|
||||
// navdata_cks += navdata->chksum;
|
||||
|
||||
return 0; // we dont know how to calculate the checksum
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Dino Hensen, Vincent van Hoek
|
||||
*
|
||||
* 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 boards/ardrone/navdata.h
|
||||
* ardrone2 navdata aquisition driver.
|
||||
*
|
||||
* The ardrone2 provides a navdata stream of packets
|
||||
* containing info about all sensors at a rate of 200Hz.
|
||||
*/
|
||||
|
||||
#ifndef NAVDATA_H_
|
||||
#define NAVDATA_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define NAVDATA_PACKET_SIZE 60
|
||||
#define NAVDATA_BUFFER_SIZE 80
|
||||
#define NAVDATA_START_BYTE 0x3a
|
||||
|
||||
typedef struct {
|
||||
uint8_t isInitialized;
|
||||
uint8_t isOpen;
|
||||
uint16_t bytesRead;
|
||||
uint32_t totalBytesRead;
|
||||
uint32_t packetsRead;
|
||||
uint8_t buffer[NAVDATA_BUFFER_SIZE];
|
||||
} navdata_port;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t taille;
|
||||
uint16_t nu_trame;
|
||||
|
||||
uint16_t ax;
|
||||
uint16_t ay;
|
||||
uint16_t az;
|
||||
|
||||
int16_t vx;
|
||||
int16_t vy;
|
||||
int16_t vz;
|
||||
uint16_t temperature_acc;
|
||||
uint16_t temperature_gyro;
|
||||
|
||||
uint16_t ultrasound;
|
||||
|
||||
uint16_t us_debut_echo;
|
||||
uint16_t us_fin_echo;
|
||||
uint16_t us_association_echo;
|
||||
uint16_t us_distance_echo;
|
||||
|
||||
uint16_t us_curve_time;
|
||||
uint16_t us_curve_value;
|
||||
uint16_t us_curve_ref;
|
||||
|
||||
uint16_t nb_echo;
|
||||
|
||||
uint32_t sum_echo; //unsigned long
|
||||
int16_t gradient;
|
||||
|
||||
uint16_t flag_echo_ini;
|
||||
|
||||
int32_t pressure; //long
|
||||
int16_t temperature_pressure;
|
||||
|
||||
int16_t mx;
|
||||
int16_t my;
|
||||
int16_t mz;
|
||||
|
||||
uint16_t chksum;
|
||||
|
||||
} __attribute__ ((packed)) measures_t;
|
||||
|
||||
measures_t* navdata;
|
||||
navdata_port* port;
|
||||
uint16_t navdata_cks;
|
||||
uint8_t navdata_imu_available;
|
||||
uint8_t navdata_baro_available;
|
||||
int16_t previousUltrasoundHeight;
|
||||
|
||||
int navdata_init(void);
|
||||
void navdata_close(void);
|
||||
|
||||
void navdata_read(void);
|
||||
void navdata_update(void);
|
||||
void navdata_CropBuffer(int cropsize);
|
||||
|
||||
uint16_t navdata_checksum(void);
|
||||
int16_t navdata_getHeight(void);
|
||||
|
||||
#endif /* NAVDATA_H_ */
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef CONFIG_ARDRONE2_RAW
|
||||
#define CONFIG_ARDRONE2_RAW
|
||||
|
||||
#define BOARD_ARDRONE2_RAW
|
||||
|
||||
/* Default actuators driver */
|
||||
#define DEFAULT_ACTUATORS "boards/ardrone/actuators_ardrone2_raw.h"
|
||||
#define ActuatorDefaultSet(_x,_y) ActuatorArdroneSet(_x,_y)
|
||||
#define ActuatorsDefaultInit() ActuatorsArdroneInit()
|
||||
#define ActuatorsDefaultCommit() ActuatorsArdroneCommit()
|
||||
|
||||
#define BOARD_HAS_BARO 1
|
||||
|
||||
#endif /* CONFIG_ARDRONE2_RAW */
|
||||
@@ -0,0 +1,20 @@
|
||||
#ifndef CONFIG_ARDRONE2_SDK
|
||||
#define CONFIG_ARDRONE2_SDK
|
||||
|
||||
#define BOARD_ARDRONE2_SDK
|
||||
|
||||
/* Internal communication */
|
||||
#define ARDRONE_NAVDATA_PORT 5554
|
||||
#define ARDRONE_AT_PORT 5556
|
||||
#define ARDRONE_NAVDATA_BUFFER_SIZE 2048
|
||||
#define ARDRONE_IP "192.168.1.1"
|
||||
|
||||
/* Default actuators driver */
|
||||
#define DEFAULT_ACTUATORS "boards/ardrone/actuators_at.h"
|
||||
#define ActuatorDefaultSet(_x,_y) {}
|
||||
#define ActuatorsDefaultInit() {}
|
||||
#define ActuatorsDefaultCommit() {}
|
||||
|
||||
#define BOARD_HAS_BARO 0
|
||||
|
||||
#endif /* CONFIG_ARDRONE2_SDK */
|
||||
@@ -38,6 +38,9 @@
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "subsystems/settings.h"
|
||||
#include "subsystems/datalink/xbee.h"
|
||||
#if DATALINK == WIFI
|
||||
#include "subsystems/datalink/wifi.h"
|
||||
#endif
|
||||
|
||||
#include "subsystems/commands.h"
|
||||
#include "subsystems/actuators.h"
|
||||
@@ -155,6 +158,10 @@ STATIC_INLINE void main_init( void ) {
|
||||
xbee_init();
|
||||
#endif
|
||||
|
||||
#if DATALINK == WIFI
|
||||
wifi_init();
|
||||
#endif
|
||||
|
||||
// register the timers for the periodic functions
|
||||
main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL);
|
||||
modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL);
|
||||
|
||||
@@ -358,6 +358,28 @@
|
||||
#define PERIODIC_SEND_FILTER(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#if USE_AHRS_ARDRONE2
|
||||
#include "subsystems/ahrs/ahrs_ardrone2.h"
|
||||
#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) { \
|
||||
DOWNLINK_SEND_AHRS_ARDRONE2(_trans, _dev, \
|
||||
&ahrs_impl.state, \
|
||||
&ahrs_impl.control_state, \
|
||||
&ahrs_impl.eulers.phi, \
|
||||
&ahrs_impl.eulers.theta, \
|
||||
&ahrs_impl.eulers.psi, \
|
||||
&ahrs_impl.speed.x, \
|
||||
&ahrs_impl.speed.y, \
|
||||
&ahrs_impl.speed.z, \
|
||||
&ahrs_impl.accel.x, \
|
||||
&ahrs_impl.accel.y, \
|
||||
&ahrs_impl.accel.z, \
|
||||
&ahrs_impl.altitude, \
|
||||
&ahrs_impl.battery); \
|
||||
}
|
||||
#else
|
||||
#define PERIODIC_SEND_AHRS_ARDRONE2(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#if USE_AHRS_CMPL_EULER || USE_AHRS_CMPL_QUAT
|
||||
#define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) { \
|
||||
DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev, \
|
||||
@@ -932,6 +954,42 @@
|
||||
#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#ifdef ARDRONE2_RAW
|
||||
#include "navdata.h"
|
||||
#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \
|
||||
&navdata->taille, \
|
||||
&navdata->nu_trame, \
|
||||
&navdata->ax, \
|
||||
&navdata->ay, \
|
||||
&navdata->az, \
|
||||
&navdata->vx, \
|
||||
&navdata->vy, \
|
||||
&navdata->vz, \
|
||||
&navdata->temperature_acc, \
|
||||
&navdata->temperature_gyro, \
|
||||
&navdata->ultrasound, \
|
||||
&navdata->us_debut_echo, \
|
||||
&navdata->us_fin_echo, \
|
||||
&navdata->us_association_echo, \
|
||||
&navdata->us_distance_echo, \
|
||||
&navdata->us_curve_time, \
|
||||
&navdata->us_curve_value, \
|
||||
&navdata->us_curve_ref, \
|
||||
&navdata->nb_echo, \
|
||||
&navdata->sum_echo, \
|
||||
&navdata->gradient, \
|
||||
&navdata->flag_echo_ini, \
|
||||
&navdata->pressure, \
|
||||
&navdata->temperature_pressure, \
|
||||
&navdata->mx, \
|
||||
&navdata->my, \
|
||||
&navdata->mz, \
|
||||
&navdata->chksum \
|
||||
)
|
||||
#else
|
||||
#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#include "generated/settings.h"
|
||||
#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
|
||||
|
||||
|
||||
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/ahrs/ahrs_ardrone2.c
|
||||
* AHRS implementation for ardrone2-sdk based on AT-commands.
|
||||
*
|
||||
* Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
|
||||
* and also sets battery level.
|
||||
*/
|
||||
|
||||
#include "ahrs_ardrone2.h"
|
||||
#include "state.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "boards/ardrone/at_com.h"
|
||||
#include "subsystems/electrical.h"
|
||||
|
||||
struct AhrsARDrone ahrs_impl;
|
||||
struct AhrsAligner ahrs_aligner;
|
||||
unsigned char buffer[2048]; //Packet buffer
|
||||
|
||||
void ahrs_init(void) {
|
||||
init_at_com();
|
||||
|
||||
//Set navdata_demo to FALSE and flat trim the ar drone
|
||||
at_com_send_config("general:navdata_demo", "FALSE");
|
||||
at_com_send_ftrim();
|
||||
|
||||
ahrs.status = AHRS_RUNNING;
|
||||
}
|
||||
|
||||
void ahrs_align(void) {
|
||||
|
||||
}
|
||||
|
||||
void ahrs_propagate(void) {
|
||||
//Recieve the main packet
|
||||
at_com_recieve_navdata(buffer);
|
||||
navdata_t* main_packet = (navdata_t*) &buffer;
|
||||
ahrs_impl.state = main_packet->ardrone_state;
|
||||
|
||||
//Init the option
|
||||
navdata_option_t* navdata_option = (navdata_option_t*)&(main_packet->options[0]);
|
||||
bool_t full_read = FALSE;
|
||||
|
||||
//The possible packets
|
||||
navdata_demo_t* navdata_demo;
|
||||
navdata_phys_measures_t* navdata_phys_measures;
|
||||
|
||||
//Read the navdata until packet is fully readed
|
||||
while(!full_read) {
|
||||
//Check the tag for the right option
|
||||
switch(navdata_option->tag) {
|
||||
case 0: //NAVDATA_DEMO
|
||||
navdata_demo = (navdata_demo_t*) navdata_option;
|
||||
|
||||
//Set the AHRS state
|
||||
ahrs_impl.control_state = navdata_demo->ctrl_state >> 16;
|
||||
ahrs_impl.eulers.phi = navdata_demo->phi;
|
||||
ahrs_impl.eulers.theta = navdata_demo->theta;
|
||||
ahrs_impl.eulers.psi = navdata_demo->psi;
|
||||
ahrs_impl.speed.x = navdata_demo->vx / 1000;
|
||||
ahrs_impl.speed.y = navdata_demo->vy / 1000;
|
||||
ahrs_impl.speed.z = navdata_demo->vz / 1000;
|
||||
ahrs_impl.altitude = navdata_demo->altitude / 10;
|
||||
ahrs_impl.battery = navdata_demo->vbat_flying_percentage;
|
||||
|
||||
//Set the ned to body eulers
|
||||
struct FloatEulers angles;
|
||||
angles.theta = navdata_demo->theta/180000.*M_PI;
|
||||
angles.psi = navdata_demo->psi/180000.*M_PI;
|
||||
angles.phi = navdata_demo->phi/180000.*M_PI;
|
||||
stateSetNedToBodyEulers_f(&angles);
|
||||
|
||||
//Update the electrical supply
|
||||
electrical.vsupply = navdata_demo->vbat_flying_percentage;
|
||||
break;
|
||||
case 3: //NAVDATA_PHYS_MEASURES
|
||||
navdata_phys_measures = (navdata_phys_measures_t*) navdata_option;
|
||||
|
||||
//Set the AHRS accel state
|
||||
INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
|
||||
break;
|
||||
case 0xFFFF: //CHECKSUM
|
||||
//TODO: Check the checksum
|
||||
full_read = TRUE;
|
||||
break;
|
||||
default:
|
||||
//printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
|
||||
break;
|
||||
}
|
||||
navdata_option = (navdata_option_t*) ((int)navdata_option + navdata_option->size);
|
||||
}
|
||||
}
|
||||
|
||||
void ahrs_update_accel(void) {
|
||||
|
||||
}
|
||||
|
||||
void ahrs_update_mag(void) {
|
||||
|
||||
}
|
||||
|
||||
void ahrs_update_gps(void) {
|
||||
|
||||
}
|
||||
|
||||
void ahrs_aligner_init(void) {
|
||||
|
||||
}
|
||||
|
||||
void ahrs_aligner_run(void) {
|
||||
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/ahrs/ahrs_ardrone2.h
|
||||
* AHRS implementation for ardrone2-sdk based on AT-commands.
|
||||
*
|
||||
* Uses AT-Commands to communicate with ardrone api to retrieve AHRS data
|
||||
* and also sets battery level.
|
||||
*/
|
||||
|
||||
#ifndef AHRS_ARDRONE2_H
|
||||
#define AHRS_ARDRONE2_H
|
||||
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "subsystems/ahrs/ahrs_aligner.h"
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
|
||||
struct AhrsARDrone {
|
||||
uint32_t state; // ARDRONE_STATES
|
||||
uint32_t control_state; // CTRL_STATES
|
||||
struct FloatEulers eulers; // in radians
|
||||
struct NedCoor_f speed; // in m/s
|
||||
struct NedCoor_f accel; // in m/s^2
|
||||
int32_t altitude; // in cm above ground
|
||||
uint32_t battery; // in percentage
|
||||
struct Int32Quat ltp_to_imu_quat;
|
||||
};
|
||||
extern struct AhrsARDrone ahrs_impl;
|
||||
|
||||
#endif /* AHRS_ARDRONE2_H */
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Dino Hensen, Vincent van Hoek
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/imu/imu_ardrone2_raw.c
|
||||
* IMU implementation for ardrone2-raw.
|
||||
*/
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "navdata.h"
|
||||
#include "imu_ardrone2_raw.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
void imu_impl_init(void) {
|
||||
imu_data_available = FALSE;
|
||||
navdata_init();
|
||||
}
|
||||
|
||||
void imu_periodic(void) {
|
||||
navdata_update();
|
||||
//checks if the navboard has a new dataset ready
|
||||
if (navdata_imu_available == TRUE) {
|
||||
navdata_imu_available = FALSE;
|
||||
RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
|
||||
VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
|
||||
VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
|
||||
imu_data_available = TRUE;
|
||||
}
|
||||
else {
|
||||
imu_data_available = FALSE;
|
||||
}
|
||||
|
||||
#ifdef USE_UART1
|
||||
uart1_handler();
|
||||
#endif
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Dino Hensen, Vincent van Hoek
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/imu/imu_ardrone2_raw.h
|
||||
* IMU implementation for ardrone2-raw.
|
||||
*/
|
||||
|
||||
#ifndef IMU_ARDRONE2_RAW_H_
|
||||
#define IMU_ARDRONE2_RAW_H_
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "navdata.h"
|
||||
|
||||
int imu_data_available;
|
||||
|
||||
static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
|
||||
{
|
||||
if (imu_data_available) {
|
||||
imu_data_available = FALSE;
|
||||
_gyro_handler();
|
||||
_accel_handler();
|
||||
_mag_handler();
|
||||
}
|
||||
}
|
||||
|
||||
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
|
||||
imu_ardrone2_event(_gyro_handler, _accel_handler, _mag_handler); \
|
||||
}
|
||||
|
||||
#endif /* IMU_ARDRONE2_RAW_H_ */
|
||||
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Dino Hensen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/imu/imu_ardrone2_sdk.c
|
||||
* IMU implementation for ardrone2-sdk.
|
||||
*/
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "imu_ardrone2_sdk.h"
|
||||
|
||||
|
||||
void imu_impl_init(void) {
|
||||
|
||||
}
|
||||
|
||||
void imu_periodic(void) {
|
||||
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Dino Hensen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/imu/imu_ardrone2_sdk.h
|
||||
* IMU implementation for ardrone2-sdk.
|
||||
*/
|
||||
|
||||
#ifndef IMU_ARDRONE2_SDK_H_
|
||||
#define IMU_ARDRONE2_SDK_H_
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
|
||||
_gyro_handler();
|
||||
_accel_handler();
|
||||
_mag_handler();
|
||||
}
|
||||
|
||||
#endif /* IMU_ARDRONE2_SDK_H_ */
|
||||
@@ -0,0 +1,147 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/ins/ins_ardrone2.c
|
||||
* INS implementation for ardrone2-sdk.
|
||||
*/
|
||||
|
||||
#include "subsystems/ins/ins_ardrone2.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "subsystems/gps.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "math/pprz_geodetic_int.h"
|
||||
|
||||
#ifdef SITL
|
||||
#include "nps_fdm.h"
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
/* TODO: implement in state */
|
||||
int32_t ins_qfe;
|
||||
int32_t ins_baro_alt;
|
||||
|
||||
//Keep track of gps pos and the init pos
|
||||
struct NedCoor_i ins_ltp_pos;
|
||||
struct LtpDef_i ins_ltp_def;
|
||||
|
||||
// Keep track of INS LTP accel and speed
|
||||
struct NedCoor_f ins_ltp_accel;
|
||||
struct NedCoor_f ins_ltp_speed;
|
||||
|
||||
bool_t ins_ltp_initialised;
|
||||
|
||||
void ins_init() {
|
||||
#if USE_INS_NAV_INIT
|
||||
struct LlaCoor_i llh_nav;
|
||||
|
||||
/** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
|
||||
llh_nav.lat = INT32_RAD_OF_DEG(NAV_LAT0);
|
||||
llh_nav.lon = INT32_RAD_OF_DEG(NAV_LON0);
|
||||
llh_nav.alt = NAV_ALT0 + NAV_MSL0;
|
||||
|
||||
//Convert ltp
|
||||
ltp_def_from_lla_i(&ins_ltp_def, &llh_nav);
|
||||
ins_ltp_def.hmsl = NAV_ALT0;
|
||||
|
||||
//Set the ltp
|
||||
stateSetLocalOrigin_i(&ins_ltp_def);
|
||||
|
||||
ins_ltp_initialised = TRUE;
|
||||
#else
|
||||
ins_ltp_initialised = FALSE;
|
||||
#endif
|
||||
|
||||
ins.vf_realign = FALSE;
|
||||
ins.hf_realign = FALSE;
|
||||
|
||||
INT32_VECT3_ZERO(ins_ltp_pos);
|
||||
|
||||
// TODO correct init
|
||||
ins.status = INS_RUNNING;
|
||||
}
|
||||
|
||||
void ins_periodic( void ) {
|
||||
|
||||
}
|
||||
|
||||
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
|
||||
|
||||
}
|
||||
|
||||
void ins_realign_v(float z __attribute__ ((unused))) {
|
||||
|
||||
}
|
||||
|
||||
void ins_propagate() {
|
||||
/* untilt accels and speeds */
|
||||
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_accel, (*stateGetNedToBodyRMat_f()), ahrs_impl.accel);
|
||||
FLOAT_RMAT_VECT3_TRANSP_MUL(ins_ltp_speed, (*stateGetNedToBodyRMat_f()), ahrs_impl.speed);
|
||||
|
||||
//Add g to the accelerations
|
||||
ins_ltp_accel.z += 9.81;
|
||||
|
||||
//Save the accelerations and speeds
|
||||
stateSetAccelNed_f(&ins_ltp_accel);
|
||||
stateSetSpeedNed_f(&ins_ltp_speed);
|
||||
|
||||
#if !USE_GPS_HEIGHT
|
||||
//Set the height and save the position
|
||||
ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
|
||||
stateSetPositionNed_i(&ins_ltp_pos);
|
||||
#endif
|
||||
}
|
||||
|
||||
void ins_update_baro() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
void ins_update_gps(void) {
|
||||
#if USE_GPS
|
||||
//Check for GPS fix
|
||||
if (gps.fix == GPS_FIX_3D) {
|
||||
//Set the initial coordinates
|
||||
if(!ins_ltp_initialised) {
|
||||
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
|
||||
ins_ltp_def.lla.alt = gps.lla_pos.alt;
|
||||
ins_ltp_def.hmsl = gps.hmsl;
|
||||
ins_ltp_initialised = TRUE;
|
||||
stateSetLocalOrigin_i(&ins_ltp_def);
|
||||
}
|
||||
|
||||
//Set the x and y and maybe z position in ltp and save
|
||||
struct NedCoor_i ins_gps_pos_cm_ned;
|
||||
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
|
||||
#if USE_GPS_HEIGHT
|
||||
INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
#else
|
||||
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
#endif
|
||||
stateSetPositionNed_i(&ins_ltp_pos);
|
||||
}
|
||||
#endif /* USE_GPS */
|
||||
}
|
||||
|
||||
void ins_update_sonar() {
|
||||
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
/*
|
||||
* Copyright (C) 2012-2013 Freek van Tienen
|
||||
*
|
||||
* This file is part of Paparazzi.
|
||||
*
|
||||
* Paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file subsystems/ins/ins_ardrone2.h
|
||||
* INS implementation for ardrone2-sdk.
|
||||
*/
|
||||
|
||||
#ifndef INS_INT_H
|
||||
#define INS_INT_H
|
||||
|
||||
#include "subsystems/ins.h"
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_int.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
//TODO: implement in state
|
||||
extern int32_t ins_qfe;
|
||||
extern int32_t ins_baro_alt;
|
||||
|
||||
extern struct NedCoor_i ins_ltp_pos;
|
||||
extern struct LtpDef_i ins_ltp_def;
|
||||
extern struct NedCoor_f ins_ltp_speed;
|
||||
extern struct NedCoor_f ins_ltp_accel;
|
||||
extern bool_t ins_ltp_initialised;
|
||||
|
||||
#endif /* INS_INT_H */
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user