mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 21:36:28 +08:00
Merge pull request #1416 from EwoudSmeur/mavshot
New asctec ESC i2c protocol
This commit is contained in:
Felix Ruess
@@ -0,0 +1,305 @@
|
||||
<!-- this is a quadshot vehicle equiped with Lisa/m2.0 and an aspirin 2.1
|
||||
More information on the Quadshot can be found at transition-robotics.com -->
|
||||
|
||||
<airframe name="quadshot aspirin 2.1 spektrum">
|
||||
|
||||
<servos driver="Pwm">
|
||||
<!-- <servo name="A1" no="0" min="1000" neutral="1100" max="2000"/>
|
||||
<servo name="A2" no="1" min="1000" neutral="1100" max="2000"/>
|
||||
<servo name="B1" no="2" min="1000" neutral="1100" max="2000"/>
|
||||
<servo name="B2" no="3" min="1000" neutral="1100" max="2000"/>-->
|
||||
<servo name="ELEVON_LEFT" no="4" min="1000" neutral="1500" max="2000"/>
|
||||
<servo name="ELEVON_RIGHT" no="5" min="1000" neutral="1500" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<servos driver="Asctec_v2_new">
|
||||
<servo name="FRONT" no="0" min="0" neutral="3" max="200"/>
|
||||
<servo name="BACK" no="1" min="0" neutral="3" max="200"/>
|
||||
<servo name="LEFT" no="2" min="0" neutral="3" max="200"/>
|
||||
<servo name="RIGHT" no="3" min="0" neutral="3" max="200"/>
|
||||
</servos>
|
||||
|
||||
<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>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
||||
<set servo="FRONT" value="motor_mixing.commands[0]"/>
|
||||
<set servo="BACK" value="motor_mixing.commands[1]"/>
|
||||
<set servo="LEFT" value="motor_mixing.commands[2]"/>
|
||||
<set servo="RIGHT" value="motor_mixing.commands[3]"/>
|
||||
|
||||
<!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
|
||||
<!-- First the correct feedback is stored in variables -->
|
||||
<let var="aileron_feedback_left" value="-@YAW"/>
|
||||
<let var="aileron_feedback_right" value="-@YAW"/>
|
||||
|
||||
<let var="elevator_feedback_left" value="+@PITCH"/>
|
||||
<let var="elevator_feedback_right" value="-@PITCH"/>
|
||||
|
||||
<!-- Here the gains are defined for the two feedback cases, hover and forward-->
|
||||
<let var="hover_left" value="3*$aileron_feedback_left"/>
|
||||
<let var="hover_right" value="3*$aileron_feedback_right"/>
|
||||
|
||||
<let var="forward_left" value="6*$aileron_feedback_left+4*$elevator_feedback_left"/>
|
||||
<let var="forward_right" value="6*$aileron_feedback_right+4*$elevator_feedback_right"/>
|
||||
|
||||
<!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
|
||||
<set servo="ELEVON_LEFT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left : $forward_left" />
|
||||
<set servo="ELEVON_RIGHT" value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right : $forward_right" />
|
||||
</command_laws>
|
||||
|
||||
<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="256"/>
|
||||
<define name="PITCH_COEF" value="{ -256, -256, 256, 256 }"/>
|
||||
<define name="ROLL_COEF" value="{ 256, -256, 256, -256 }"/>
|
||||
<define name="YAW_COEF" value="{ 256, -256, -128, 128 }"/>
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||
</section>
|
||||
|
||||
<!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
|
||||
|
||||
<!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
|
||||
The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<!-- Use default driver values for gyro -->
|
||||
|
||||
<!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
|
||||
<define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_X_SENS" value="4.86487566223" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="4.89957269597" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="4.82616398266" integer="16"/>
|
||||
|
||||
<define name="MAG_X_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="0"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="0"/>
|
||||
<define name="MAG_X_SENS" value="4.19385009207" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="4.32306399648" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.63243801309" integer="16"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="AUTOPILOT">
|
||||
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_RATE_DIRECT"/>
|
||||
|
||||
<!-- <define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/> -->
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MIN_BAT_LEVEL" value="10.4" units="V"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
</section>
|
||||
|
||||
<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="GAIN_P" value="350"/>
|
||||
<define name="GAIN_Q" value="250"/>
|
||||
<define name="GAIN_R" value="350"/>
|
||||
|
||||
<define name="IGAIN_P" value="200"/>
|
||||
<define name="IGAIN_Q" value="200"/>
|
||||
<define name="IGAIN_R" value="200"/>
|
||||
</section>
|
||||
|
||||
<!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
|
||||
<section name ="GAIN_SETS">
|
||||
<define name="NUMBER_OF_GAINSETS" value="2"/>
|
||||
<define name="SCHEDULING_VARIABLE" value="(radio.values[COMMAND_THRUST]+ transition_status"/>
|
||||
<define name="SCHEDULING_POINTS" value="{1000, 6000}"/>
|
||||
<define name="SCHEDULING_VARIABLE_FRAC" value="0"/>
|
||||
|
||||
<define name="PHI_P" value="{230, 230}"/>
|
||||
<define name="PHI_D" value="{170, 170}"/>
|
||||
<define name="PHI_I" value="{30, 30}"/>
|
||||
<define name="PHI_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="THETA_P" value="{200, 300}"/>
|
||||
<define name="THETA_D" value="{100, 50}"/>
|
||||
<define name="THETA_I" value="{40, 40}"/>
|
||||
<define name="THETA_DD" value="{0, 0}"/>
|
||||
|
||||
<define name="PSI_P" value="{300, 300}"/>
|
||||
<define name="PSI_D" value="{150, 150}"/>
|
||||
<define name="PSI_I" value="{0, 0}"/>
|
||||
<define name="PSI_DD" value="{0, 0}"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_PHI" value="60." unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="60." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
<define name="DEADBAND_A" value="250"/>
|
||||
<define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="1500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="1500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="1500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="170"/>
|
||||
<define name="PHI_DGAIN" value="120"/>
|
||||
<define name="PHI_IGAIN" value="30"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="150"/>
|
||||
<define name="THETA_DGAIN" value="70"/>
|
||||
<define name="THETA_IGAIN" value="40"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="300"/>
|
||||
<define name="PSI_DGAIN" value="150"/>
|
||||
<define name="PSI_IGAIN" value="0"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value=" 0"/>
|
||||
<define name="THETA_DDGAIN" value=" 0"/>
|
||||
<define name="PSI_DDGAIN" value=" 0"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.012"/>
|
||||
<define name="G1_Q" value="0.04"/>
|
||||
<define name="G1_R" value="0.004"/>
|
||||
<define name="G2_R" value="0.0"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="70.0"/>
|
||||
<define name="REF_ERR_Q" value="45.0"/>
|
||||
<define name="REF_ERR_R" value="70.0"/>
|
||||
<define name="REF_RATE_P" value="9.0"/>
|
||||
<define name="REF_RATE_Q" value="16.0"/>
|
||||
<define name="REF_RATE_R" value="12.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="50.0"/>
|
||||
<define name="FILT_ZETA" value="0.7"/>
|
||||
<define name="FILT_OMEGA_R" value="20.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.55"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.15"/>
|
||||
<define name="ACT_DYN_Q" value="0.15"/>
|
||||
<define name="ACT_DYN_R" value="0.15"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</section>
|
||||
|
||||
|
||||
<!-- Gains for vertical navigation -->
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="200"/>
|
||||
<define name="HOVER_KD" value="175"/>
|
||||
<define name="HOVER_KI" value="72"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<define name="H_X" value=" 0.47577"/>
|
||||
<define name="H_Y" value=" 0.11811"/>
|
||||
<define name="H_Z" value=" 0.87161"/>
|
||||
</section>
|
||||
|
||||
<!-- Gains for horizontal navigation-->
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="100"/>
|
||||
<define name="DGAIN" value="100"/>
|
||||
<define name="IGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<!-- <define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/> -->
|
||||
<!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
|
||||
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
|
||||
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
|
||||
<define name="TRANSITION_MAX_OFFSET" value="-82.0" unit="deg"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
<!-- <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value=" {1,1,-1,-1,1,-1,1,1,1,1,1,1}"/> -->
|
||||
</section>
|
||||
|
||||
<modules main_freq="512">
|
||||
<load name="gps_ubx_ucenter.xml"/>
|
||||
|
||||
<!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
|
||||
<load name="led_safety_status.xml">
|
||||
<define name="USE_LED_BODY" value="1"/>
|
||||
<define name="SAFETY_WARNING_LED" value="BODY"/>
|
||||
</load>
|
||||
|
||||
<!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
|
||||
<!--load name="gain_scheduling.xml"/-->
|
||||
</modules>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="lisa_m_2.0">
|
||||
<subsystem name="radio_control" type="spektrum">
|
||||
<!-- Put the mode on channel AUX1-->
|
||||
<define name="RADIO_KILL_SWITCH" value="6"/>
|
||||
<!-- <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
|
||||
</subsystem>
|
||||
|
||||
<configure name="FLASH_MODE" value="SWD"/>
|
||||
|
||||
<!-- on aspirin 2.1 the ms5611 baro is not connected via SPI, use BMP085 on Lisa/M board instead -->
|
||||
<!-- <configure name="LISA_M_BARO" value="BARO_BOARD_BMP085"/> -->
|
||||
<!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
|
||||
<configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/>
|
||||
</target>
|
||||
|
||||
<subsystem name="motor_mixing"/>
|
||||
<subsystem name="actuators" type="pwm">
|
||||
<define name="SERVO_HZ" value="400"/>
|
||||
</subsystem>
|
||||
<subsystem name="actuators" type="asctec_v2_new"/>
|
||||
|
||||
<subsystem name="telemetry" type="xbee_api"/>
|
||||
<!-- <subsystem name="telemetry" type="transparent"/> -->
|
||||
<subsystem name="imu" type="aspirin_v2.2"/>
|
||||
<subsystem name="gps" type="ublox"/>
|
||||
<subsystem name="stabilization" type="indi"/>
|
||||
|
||||
<subsystem name="ahrs" type="int_cmpl_quat">
|
||||
<configure name="USE_MAGNETOMETER" value="TRUE"/>
|
||||
<!--define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/-->
|
||||
</subsystem>
|
||||
|
||||
<subsystem name="ins"/>
|
||||
</firmware>
|
||||
</airframe>
|
||||
@@ -197,6 +197,17 @@
|
||||
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="mavshot"
|
||||
ac_id="5"
|
||||
airframe="airframes/TUDelft/airframes/mavshot.xml"
|
||||
radio="radios/cockpitSX.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/dummy.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_indi.xml settings/modules/config_asctec_v2_new.xml"
|
||||
settings_modules="modules/gps_ubx_ucenter.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="quadshot_pylons"
|
||||
ac_id="178"
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
# asctec controllers v2 with new I2C protocol
|
||||
#
|
||||
# required xml configuration:
|
||||
#
|
||||
# servo section with driver="Asctec_v2_new"
|
||||
# command_laws section to map motor_mixing commands to servos
|
||||
#
|
||||
|
||||
$(TARGET).CFLAGS += -DACTUATORS
|
||||
ACTUATORS_ASCTEC_V2_SRCS = subsystems/actuators/actuators_asctec_v2_new.c
|
||||
|
||||
|
||||
# set default i2c device if not already configured
|
||||
ifeq ($(ARCH), lpc21)
|
||||
ACTUATORS_ASCTEC_V2_I2C_DEV ?= i2c0
|
||||
else ifeq ($(ARCH), stm32)
|
||||
ACTUATORS_ASCTEC_V2_I2C_DEV ?= i2c1
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET), ap)
|
||||
ifndef ACTUATORS_ASCTEC_V2_I2C_DEV
|
||||
$(error Error: ACTUATORS_ASCTEC_V2_I2C_DEV not configured!)
|
||||
endif
|
||||
endif
|
||||
|
||||
# convert i2cx to upper/lower case
|
||||
ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_ASCTEC_V2_I2C_DEV) | tr a-z A-Z)
|
||||
ACTUATORS_ASCTEC_V2_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_ASCTEC_V2_I2C_DEV) | tr A-Z a-z)
|
||||
|
||||
ACTUATORS_ASCTEC_V2_CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=$(ACTUATORS_ASCTEC_V2_I2C_DEV_LOWER)
|
||||
ACTUATORS_ASCTEC_V2_CFLAGS += -DUSE_$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
# set default i2c timing if not already configured
|
||||
ACTUATORS_ASCTEC_V2_I2C_SCL_TIME ?= 150
|
||||
ACTUATORS_ASCTEC_V2_CFLAGS += -D$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_ASCTEC_V2_I2C_SCL_TIME)
|
||||
ACTUATORS_ASCTEC_V2_CFLAGS += -D$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_ASCTEC_V2_I2C_SCL_TIME)
|
||||
endif
|
||||
|
||||
ap.CFLAGS += $(ACTUATORS_ASCTEC_V2_CFLAGS)
|
||||
ap.srcs += $(ACTUATORS_ASCTEC_V2_SRCS)
|
||||
|
||||
|
||||
|
||||
# Simulator
|
||||
nps.srcs += subsystems/actuators/actuators_asctec_v2_new.c
|
||||
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c0
|
||||
@@ -6,13 +6,13 @@
|
||||
<dl_settings NAME="indi">
|
||||
<dl_setting var="reference_acceleration.err_p" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_p" param="STABILIZATION_INDI_REF_ERR_P" persistent="true"/>
|
||||
<dl_setting var="reference_acceleration.rate_p" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_p" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
|
||||
<dl_setting var="g1.p" min="0" step="1" max="500" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
|
||||
<dl_setting var="g1.p" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_p" param="STABILIZATION_INDI_G1_P" persistent="true"/>
|
||||
<dl_setting var="reference_acceleration.err_q" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_q" param="STABILIZATION_INDI_REF_ERR_Q" persistent="true"/>
|
||||
<dl_setting var="reference_acceleration.rate_q" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_q" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
|
||||
<dl_setting var="g1.q" min="0" step="1" max="500" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
|
||||
<dl_setting var="g1.q" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_q" param="STABILIZATION_INDI_G1_Q" persistent="true"/>
|
||||
<dl_setting var="reference_acceleration.err_r" min="0" step="1" max="2500" module="stabilization/stabilization_attitude_quat_indi" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R" persistent="true"/>
|
||||
<dl_setting var="reference_acceleration.rate_r" min="0" step="0.1" max="100" module="stabilization/stabilization_attitude_quat_indi" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P" persistent="true"/>
|
||||
<dl_setting var="g1.r" min="0" step="0.01" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
|
||||
<dl_setting var="g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
|
||||
<dl_setting var="g2" min="0" step="0.01" max="10" module="stabilization/stabilization_attitude_quat_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
|
||||
<dl_setting var="use_adaptive_indi" min="0" step="1" max="1" module="stabilization/stabilization_attitude_quat_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
|
||||
</dl_settings>
|
||||
|
||||
@@ -0,0 +1,11 @@
|
||||
<settings>
|
||||
<dl_settings>
|
||||
|
||||
<dl_settings NAME="asctec_v2_new">
|
||||
<dl_setting var="actuators_asctec_v2.cmd" min="0" step="1" max="3" module="subsystems/actuators/actuators_asctec_v2_new" shortname="cmd" values="NONE|TEST|REVERSE|SET_ADDR" handler="SetCommand"/>
|
||||
<dl_setting var="actuators_asctec_v2.cur_addr" min="0" step="1" max="3" module="subsystems/actuators/actuators_asctec_v2_new" shortname="cur_addr" values="FRONT|BACK|LEFT|RIGHT" handler="SetCurAddr"/>
|
||||
<dl_setting var="actuators_asctec_v2.new_addr" min="0" step="1" max="3" module="subsystems/actuators/actuators_asctec_v2_new" shortname="new_addr" values="FRONT|BACK|LEFT|RIGHT" handler="SetNewAddr"/>
|
||||
</dl_settings>
|
||||
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -0,0 +1,187 @@
|
||||
/*
|
||||
* Copyright (C) 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 actuators_asctec_v2_new.c
|
||||
* Actuators driver for Asctec v2 motor controllers with the new I2C protocol.
|
||||
*/
|
||||
|
||||
#include "subsystems/actuators.h"
|
||||
#include "subsystems/actuators/actuators_asctec_v2_new.h"
|
||||
|
||||
#include "mcu_periph/i2c.h"
|
||||
#include "mcu_periph/sys_time.h"
|
||||
|
||||
#define ACTUATORS_ASCTEC_V2_SLAVE_ADDR 0x00
|
||||
|
||||
PRINT_CONFIG_VAR(ACTUATORS_ASCTEC_V2_I2C_DEV)
|
||||
|
||||
struct ActuatorsAsctecV2 actuators_asctec_v2;
|
||||
|
||||
static uint16_t crc_update(uint16_t crc, uint8_t data);
|
||||
|
||||
void actuators_asctec_v2_init(void)
|
||||
{
|
||||
actuators_asctec_v2.cmd = NONE;
|
||||
actuators_asctec_v2.cur_addr = FRONT;
|
||||
actuators_asctec_v2.new_addr = FRONT;
|
||||
actuators_asctec_v2.i2c_trans.status = I2CTransSuccess;
|
||||
actuators_asctec_v2.i2c_trans.type = I2CTransTx;
|
||||
actuators_asctec_v2.i2c_trans.slave_addr = ACTUATORS_ASCTEC_V2_SLAVE_ADDR;
|
||||
actuators_asctec_v2.i2c_trans.len_w = 5;
|
||||
actuators_asctec_v2.nb_err = 0;
|
||||
}
|
||||
|
||||
|
||||
void actuators_asctec_v2_set(void)
|
||||
{
|
||||
#if defined ACTUATORS_START_DELAY && ! defined SITL
|
||||
if (!actuators_delay_done) {
|
||||
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
|
||||
#ifdef USE_I2C_ACTUATORS_REBOOT_HACK
|
||||
//Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
|
||||
//multiple re-init solves the problem.
|
||||
i2c1_init();
|
||||
#endif
|
||||
return;
|
||||
} else { actuators_delay_done = TRUE; }
|
||||
}
|
||||
#endif
|
||||
|
||||
switch (actuators_asctec_v2.i2c_trans.status) {
|
||||
case I2CTransFailed:
|
||||
actuators_asctec_v2.nb_err++;
|
||||
actuators_asctec_v2.i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
case I2CTransSuccess:
|
||||
case I2CTransDone:
|
||||
actuators_asctec_v2.i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
actuators_asctec_v2.nb_err++;
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t crc=0xff;
|
||||
|
||||
#ifdef KILL_MOTORS
|
||||
actuators_asctec_v2.i2c_trans.buf[0] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[1] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[2] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[3] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[4] = 0xC0;
|
||||
|
||||
for (uint8_t i=0;i<5;i++)
|
||||
crc=crc_update(crc,actuators_asctec_v2.i2c_trans.buf[i]);
|
||||
|
||||
actuators_asctec_v2.i2c_trans.buf[5] = crc;
|
||||
i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 6);
|
||||
#else
|
||||
switch (actuators_asctec_v2.cmd) {
|
||||
case TEST:
|
||||
actuators_asctec_v2.i2c_trans.buf[0] = 251;
|
||||
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
|
||||
actuators_asctec_v2.i2c_trans.buf[2] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[3] = 231 + actuators_asctec_v2.cur_addr;
|
||||
|
||||
i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
|
||||
actuators_asctec_v2.cmd = NONE;
|
||||
break;
|
||||
case REVERSE:
|
||||
actuators_asctec_v2.i2c_trans.buf[0] = 254;
|
||||
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
|
||||
actuators_asctec_v2.i2c_trans.buf[2] = 0;
|
||||
actuators_asctec_v2.i2c_trans.buf[3] = 234 + actuators_asctec_v2.cur_addr;
|
||||
|
||||
i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
|
||||
actuators_asctec_v2.cmd = NONE;
|
||||
break;
|
||||
case SET_ADDR: {
|
||||
static uint8_t set_addr = 0;
|
||||
static uint8_t serial_num[2];
|
||||
switch(set_addr) {
|
||||
|
||||
// Request address
|
||||
case 0:
|
||||
case 2:
|
||||
i2c_receive(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, 0x06 + actuators_asctec_v2.cur_addr*2, 3);
|
||||
set_addr++;
|
||||
break;
|
||||
case 1:
|
||||
if(actuators_asctec_v2.i2c_trans.buf[0] == 2) {
|
||||
serial_num[0] = actuators_asctec_v2.i2c_trans.buf[1];
|
||||
set_addr++;
|
||||
break;
|
||||
}
|
||||
set_addr--;
|
||||
break;
|
||||
case 3:
|
||||
if(actuators_asctec_v2.i2c_trans.buf[0] == 3) {
|
||||
serial_num[1] = actuators_asctec_v2.i2c_trans.buf[1];
|
||||
set_addr++;
|
||||
break;
|
||||
}
|
||||
|
||||
set_addr--;
|
||||
break;
|
||||
case 4:
|
||||
//update ID of the motor based on the address
|
||||
actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.new_addr;
|
||||
actuators_asctec_v2.i2c_trans.buf[1] = serial_num[0];
|
||||
actuators_asctec_v2.i2c_trans.buf[2] = serial_num[1];
|
||||
actuators_asctec_v2.i2c_trans.buf[3] = 252;
|
||||
|
||||
i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
|
||||
set_addr++;
|
||||
default:
|
||||
set_addr = 0;
|
||||
actuators_asctec_v2.cmd = NONE;
|
||||
actuators_asctec_v2.cur_addr = actuators_asctec_v2.new_addr;
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case NONE:
|
||||
actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
|
||||
actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
|
||||
actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
|
||||
actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
|
||||
actuators_asctec_v2.i2c_trans.buf[4] = 0xC0;
|
||||
|
||||
for (uint8_t i=0;i<5;i++)
|
||||
crc=crc_update(crc,actuators_asctec_v2.i2c_trans.buf[i]);
|
||||
|
||||
actuators_asctec_v2.i2c_trans.buf[5] = crc;
|
||||
i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 6);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
static uint16_t crc_update(uint16_t crc, uint8_t data)
|
||||
{
|
||||
data ^= (crc & 0xff);
|
||||
data ^= data << 4;
|
||||
|
||||
return ((((uint16_t)data << 8) | ((crc>>8)&0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
|
||||
}
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* 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 actuators_asctec_v2_new.h
|
||||
* Actuators driver for Asctec v2 motor controllers with the new I2C protocol.
|
||||
*/
|
||||
|
||||
#ifndef ACTUATORS_ASCTEC_V2_NEW_H
|
||||
#define ACTUATORS_ASCTEC_V2_NEW_H
|
||||
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
enum actuators_asctec_v2_cmd { NONE,
|
||||
TEST,
|
||||
REVERSE,
|
||||
SET_ADDR
|
||||
};
|
||||
|
||||
enum actuators_asctec_v2_addr { FRONT,
|
||||
BACK,
|
||||
LEFT,
|
||||
RIGHT
|
||||
};
|
||||
|
||||
struct ActuatorsAsctecV2 {
|
||||
enum actuators_asctec_v2_cmd cmd;
|
||||
enum actuators_asctec_v2_addr cur_addr;
|
||||
enum actuators_asctec_v2_addr new_addr;
|
||||
int32_t cmds[4];
|
||||
struct i2c_transaction i2c_trans;
|
||||
volatile uint32_t nb_err;
|
||||
};
|
||||
|
||||
|
||||
extern struct ActuatorsAsctecV2 actuators_asctec_v2;
|
||||
|
||||
#define actuators_asctec_v2_new_SetCommand(_v) { \
|
||||
actuators_asctec_v2.cmd = _v; \
|
||||
}
|
||||
|
||||
#define actuators_asctec_v2_new_SetNewAddr(_v) { \
|
||||
actuators_asctec_v2.new_addr = _v; \
|
||||
}
|
||||
|
||||
#define actuators_asctec_v2_new_SetCurAddr(_v) { \
|
||||
actuators_asctec_v2.cur_addr = _v; \
|
||||
}
|
||||
|
||||
extern void actuators_asctec_v2_init(void);
|
||||
extern void actuators_asctec_v2_set(void);
|
||||
|
||||
#define ActuatorAsctec_v2_newSet(_i, _v) { actuators_asctec_v2.cmds[_i] = _v; }
|
||||
#define ActuatorsAsctec_v2_newInit() actuators_asctec_v2_init()
|
||||
#define ActuatorsAsctec_v2_newCommit() actuators_asctec_v2_set()
|
||||
|
||||
|
||||
#endif /* ACTUATORS_ASCTEC_V2_NEW_H */
|
||||
Reference in New Issue
Block a user