Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 03:27:33 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge branch 'master' into stm32f4

Browse Source 
* master: (31 commits)
  [tcas] tcas always runs on AP. Proposed solution for compile error.
  [umarim] remove warning if no mag are used with umarim
  [Makefile] use -fPIC for all architectures
  [rotorcraft] TRANSITION_MAX_OFFSET in radians, float fixes
  [rotorcraft] Add forward mode for transitioning vehicles
  [Makefile] improve detection of lablgtk2-gnome.*
  [makefile] fix sim build on OS X after broken from no_custom, can't seem to build the autopilot.so properly so back to -custom just for simsitl
  [rotorcraft] alternate buses when sending I2C_ERRORS message
  [rotorcraft] silence warning in guidance
  [stm32] Fixes for USART interrupt handler.
  [airframes] update GUIDANCE_H gains for quadrotor example
  [rotorcraft][messages] GUIDANCE_H_INT message
  [nps] don't bypass ahrs by default
  [rotorcraft] use horizontal guidance ref by default
  [supervision] allow launching of external tools with absolute path
  [dfu] print exception
  [spi] remove warning
  [doc][modules] minor update to sonar doc
  [sonar] split sonar modules with and without ins binding
  [sonar] and event hook to use the sonar with the ins
  ...
This commit is contained in:
Felix Ruess
2013-04-24 10:47:59 +02:00
parent 478f055825 eecac3e67d
commit 639b0ee447
69 changed files with 843 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.sim
+13 -9
View File
@@ -45,26 +45,24 @@ Q=@
# End of configuration part.
#
LBITS := $(shell getconf LONG_BIT)
ifeq ($(LBITS),64)
FPIC = -fPIC
else
FPIC =
endif
INCLUDES += -I `ocamlc -where`
INCLUDES += -I $(shell $(OCAMLC) -where)
CFLAGS = -W -Wall
CFLAGS += $(INCLUDES)
CFLAGS += $($(TARGET).CFLAGS)
CFLAGS += $(LOCAL_CFLAGS)
CFLAGS += $(FPIC)
CFLAGS += -fPIC
CFLAGS += -O2
CFLAGS += -g
CFLAGS += -std=gnu99
LDFLAGS = -lm
UNAME = $(shell uname -s)
ifeq ("$(UNAME)","Darwin")
CAMLINCLUDES += $(shell ocamlfind query -r -i-format lablgtk2) $(shell ocamlfind query -r -i-format xml-light)
CAMLCMAS = unix.cma str.cma xml-light.cma glibivy-ocaml.cma lib-pprz.cma lablgtk.cma
endif
#
# General rules
@@ -81,9 +79,15 @@ autopilot.so : $($(TARGET).objs)
@echo BUILD $@
$(Q)$(CC) -shared -o $(OBJDIR)/$@ $^
ifeq ("$(UNAME)","Darwin")
$(OBJDIR)/simsitl : $($(TARGET).objs) $(SITLCMA) $(SIMSITLML)
@echo LD $@
$(Q)$(OCAMLC) -g -custom $(CAMLINCLUDES) -o $@ $(CAMLCMAS) $(MYGTKINITCMO) $^
else
$(OBJDIR)/simsitl : autopilot.so $(SITLCMA) $(SIMSITLML)
@echo LD $@
$(Q)$(OCAMLC) -g $(CAMLINCLUDES) -o $@ $(LINKPKG) $(MYGTKINITCMO) $^ -dllpath $(OBJDIR) -dllpath $(SIMDIR)
endif
# The id of the A/C is hardcoded in the code (to be improved with dynlink ?)
conf/airframes/ENAC/fixed-wing/mythe.xml
+2 -2
View File
@@ -157,8 +157,8 @@
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="rad"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0.144" unit="rad"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="4." unit="deg"/>
</section>
<section name="BAT">
conf/airframes/ENAC/quadrotor/blender.xml
+1 -1
View File
@@ -5,7 +5,7 @@
<modules main_freq="512">
<load name="servo_switch.xml"/>
<load name="rotorcraft_cam.xml"/>
<!--load name="sonar_maxbotix_booz.xml"/-->
<!--load name="sonar_adc_ins.xml"/-->
<!--load name="adc_generic.xml">
<configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
</load-->
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+5 -2
View File
@@ -3,7 +3,10 @@
<modules main_freq="512">
<load name="servo_switch.xml"/>
<load name="rotorcraft_cam.xml"/>
<load name="sonar_adc.xml"/>
<load name="sonar_adc_ins.xml">
<configure name="ADC_SONAR" value="ADC_0"/>
<!--define name="SENSOR_SYNC_SEND_SONAR"/-->
</load>
<!--load name="baro_mpl3115.xml">
<define name="MPL3115_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
@@ -176,7 +179,7 @@
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="15." integer="16"/>
<!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply -->
<define name="SONAR_SENS" value="0.009856" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
<define name="SONAR_SENS" value="0.016775" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
<define name="SONAR_MAX_RANGE" value="5.0"/>
<define name="SONAR_MIN_RANGE" value="0.25"/>
<!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
conf/airframes/booz2_flixr.xml
+1 -1
View File
@@ -40,7 +40,7 @@
<load name="booz_drop.xml"/>
<load name="sys_mon.xml"/>
<!--load name="booz_cam.xml"/-->
<load name="sonar_maxbotix_booz.xml">
<load name="sonar_adc_ins.xml">
<configure name="ADC_SONAR" value="ADC_0"/>
</load>
</modules>
conf/airframes/esden/gain_scheduling_example.xml
+2 -2
View File
@@ -51,7 +51,7 @@
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<include href="conf/airframes/esden/calib/asp21-018.xml"/>
<include href="conf/airframes/esden/calib/asp22-019.xml"/>
<section name="IMU" prefix="IMU_">
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
@@ -65,7 +65,6 @@
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="AUTOPILOT_KILL_WITHOUT_AHRS" value="TRUE"/>
<define name="LOBATT_WING_WAGGLE" value="TRUE"/>
<define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
</section>
@@ -210,6 +209,7 @@
<subsystem name="radio_control" type="ppm">
<configure name="RADIO_CONTROL_PPM_PIN" value="UART1_RX"/>
</subsystem>
<configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/>
</target>
<target name="nps" board="pc">
conf/airframes/esden/qs_asp22.xml
+251
View File
@@ -0,0 +1,251 @@
<!-- this is a quadshot vehicle equiped with Lia -->
<!-- Copyright (C) 2012, Transition Robotics, Inc. -->
<airframe name="qs_asp22">
<servos driver="Pwm">
<servo name="A1" no="0" min="1000" neutral="1050" max="2000"/>
<servo name="A2" no="1" min="1000" neutral="1050" max="2000"/>
<servo name="B1" no="2" min="1000" neutral="1050" max="2000"/>
<servo name="B2" no="3" min="1000" neutral="1050" 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>
<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="A1" value="motor_mixing.commands[SERVO_A1]"/>
<set servo="A2" value="motor_mixing.commands[SERVO_A2]"/>
<set servo="B1" value="motor_mixing.commands[SERVO_B1]"/>
<set servo="B2" value="motor_mixing.commands[SERVO_B2]"/>
<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"/>
<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"/>
<!-- <set servo="ELEVON_LEFT" value="RC_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left : $forward_left" />
<set servo="ELEVON_RIGHT" value="RC_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>
<include href="conf/airframes/esden/calib/asp22-019.xml"/>
<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_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<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>
<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="3000" 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="3000" 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="3000" 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="230"/>
<define name="PHI_DGAIN" value="170"/>
<define name="PHI_IGAIN" value="30"/>
<define name="THETA_PGAIN" value="300"/>
<define name="THETA_DGAIN" value="50"/>
<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=" 140"/-->
<define name="THETA_DDGAIN" value=" 0"/>
<define name="PSI_DDGAIN" value=" 0"/>
<!-- <define name="TURN_COORDINATION" value="0"/>
<define name="TURN_ACCEL_COORDINATION" value="0"/>-->
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="22.4" integer="16"/>
</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="200"/>
<define name="HOVER_KD" value="175"/>
<define name="HOVER_KI" value="72"/>
<define name="RC_CLIMB_COEF" value ="163"/>
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value ="0.5"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="H_X" value=" 0.47577"/>
<define name="H_Y" value=" 0.11811"/>
<define name="H_Z" value=" 0.87161"/>
</section>
<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"/>
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- <define name="USE_REFERENCE_SYSTEM" value="FALSE"/> -->
<!-- <define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/> -->
<define name="TRANSITION_MAX_OFFSET" value="-82.0" unit="deg"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
</section>
<modules main_freq="512">
<!--load name="vehicle_interface_overo_link.xml"/-->
<load name="gps_ubx_ucenter.xml"/>
<load name="led_safety_status.xml">
<define name="USE_LED_BODY" value="1"/>
<define name="SAFETY_WARNING_LED" value="BODY"/>
</load>
<!--load name="gain_scheduling.xml"/-->
</modules>
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="radio_control" type="ppm">
<configure name="RADIO_CONTROL_PPM_PIN" value="UART1_RX"/>
</subsystem>
<configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/>
</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="pwm">
<define name="SERVO_HZ" value="400"/>
<define name="USE_SERVOS_7AND8"/>
</subsystem>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="aspirin_v2.2"/>
<!-- <subsystem name="imu" type="aspirin_v1.5"/> -->
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="0"/>
<define name="AHRS_USE_GPS_HEADING" value="0"/>
</subsystem>
<subsystem name="ins"/>
<!--define name="USE_INS_NAV_INIT"/-->
<!--subsystem name="stabilization" type="euler"/>
<subsystem name="ahrs" type="int_cmpl_euler"/-->
</firmware>
</airframe>
conf/airframes/examples/Twinstar_energyadaptive.xml
+3 -4
View File
@@ -33,7 +33,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<subsystem name="radio_control" type="ppm"/> <!-- Radio -->
<subsystem name="gps" type="ublox"/> <!-- GPS -->
<subsystem name="telemetry" type="transparent"/> <!-- Communication -->
<subsystem name="imu" type="aspirin_i2c_v1.5"/> <!-- IMU -->
<subsystem name="imu" type="aspirin_i2c_v1.5"> <!-- IMU -->
<configure name="ASPIRIN_I2C_DEV" value="i2c0"/>
</subsystem>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="alt_float"/>
<subsystem name="control" type="energyadaptive"/>
@@ -134,9 +136,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<define name="MAG_X_SENS" value="1." integer="16"/>
<define name="MAG_Y_SENS" value="1." integer="16"/>
<define name="MAG_Z_SENS" value="1." integer="16"/>
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="-2" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="2" unit="deg"/>
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+3 -2
View File
@@ -193,9 +193,10 @@
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="20" unit="deg"/>
<define name="PGAIN" value="100"/>
<define name="PGAIN" value="50"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="0"/>
<define name="AGAIN" value="100"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
conf/airframes/obsolete/ENAC/g1_vision.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<modules main_freq="512">
<!--load name="vision_cmucam.xml"/-->
<load name="sonar_maxbotix_booz.xml"/>
<load name="sonar_adc_ins.xml"/>
<load name="booz_extra_dl.xml"/>
<load name="booz_gumstix_com.xml"/>
</modules>
conf/airframes/obsolete/ENAC/mkk1-vision.xml
+1 -1
View File
@@ -4,7 +4,7 @@
<modules main_freq="512">
<load name="vision_cmucam.xml"/>
<load name="sonar_maxbotix_booz.xml"/>
<load name="sonar_adc_ins.xml"/>
</modules>
<servos>
conf/firmwares/lisa_passthrough.makefile
+1 -1
View File
@@ -76,7 +76,7 @@ stm_passthrough.srcs += subsystems/commands.c
#stm_passthrough.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
#stm_passthrough.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
#
#stm_passthrough.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
#stm_passthrough.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c1
#stm_passthrough.CFLAGS += -DUSE_I2C1
# PWM actuator
conf/firmwares/lisa_test_progs.makefile
+2 -2
View File
@@ -582,7 +582,7 @@ test_actuators_mkk.srcs += $(COMMON_TELEMETRY_SRCS)
test_actuators_mkk.srcs += test/test_actuators.c
test_actuators_mkk.srcs += subsystems/commands.c
test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_I2C_DEV=i2c1
test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
test_actuators_mkk.CFLAGS += -DUSE_I2C1
test_actuators_mkk.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
@@ -599,7 +599,7 @@ test_actuators_asctecv1.srcs += $(COMMON_TELEMETRY_SRCS)
test_actuators_asctecv1.srcs += test/test_actuators.c
test_actuators_asctecv1.srcs += subsystems/commands.c
test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c1
test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
test_actuators_asctecv1.CFLAGS += -DUSE_I2C1
test_actuators_asctecv1.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
conf/firmwares/subsystems/shared/actuators_asctec.makefile
+3 -3
View File
@@ -10,17 +10,17 @@ $(TARGET).CFLAGS += -DACTUATORS
ap.srcs += subsystems/actuators/actuators_asctec.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0
ap.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
endif
ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
ap.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c1
ap.CFLAGS += -DUSE_I2C1
endif
# Simulator
nps.srcs += subsystems/actuators/actuators_asctec.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_I2C_DEV=i2c0
conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
+3 -3
View File
@@ -10,16 +10,16 @@ $(TARGET).CFLAGS += -DACTUATORS
ap.srcs += subsystems/actuators/actuators_asctec_v2.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_ASCTEC_V2_DEVICE=i2c0
ap.CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
endif
ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_ASCTEC_V2_DEVICE=i2c1
ap.CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c1
ap.CFLAGS += -DUSE_I2C1
endif
# Simulator
nps.srcs += subsystems/actuators/actuators_asctec_v2.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c0
conf/firmwares/subsystems/shared/actuators_mkk.makefile
+3 -3
View File
@@ -31,14 +31,14 @@ ifeq ($(MKK_I2C_SCL_TIME), )
MKK_I2C_SCL_TIME=150
endif
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
ap.CFLAGS += -DACTUATORS_MKK_I2C_DEV=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME)
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
ap.CFLAGS += -DACTUATORS_MKK_I2C_DEV=i2c1
ap.CFLAGS += -DUSE_I2C1
endif
# Simulator
nps.srcs += subsystems/actuators/actuators_mkk.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_DEVICE=i2c0
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_I2C_DEV=i2c0
conf/firmwares/subsystems/shared/actuators_mkk_v2.makefile
+3 -3
View File
@@ -31,14 +31,14 @@ ifeq ($(MKK_V2_I2C_SCL_TIME), )
MKK_V2_I2C2_SCL_TIME=150
endif
ap.CFLAGS += -DACTUATORS_MKK_V2_DEVICE=i2c0
ap.CFLAGS += -DACTUATORS_MKK_V2_I2C_DEV=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_V2_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_V2_I2C_SCL_TIME)
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_MKK_V2_DEVICE=i2c1
ap.CFLAGS += -DACTUATORS_MKK_V2_I2C_DEV=i2c1
ap.CFLAGS += -DUSE_I2C1
endif
# Simulator:
nps.srcs += subsystems/actuators/actuators_mkk_v2.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_V2_DEVICE=i2c0
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_V2_I2C_DEV=i2c0
conf/firmwares/subsystems/shared/actuators_skiron.makefile
+2 -2
View File
@@ -26,11 +26,11 @@ $(TARGET).CFLAGS += -DACTUATORS
ap.srcs += subsystems/actuators/actuators_skiron.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_SKIRON_DEVICE=i2c0
ap.CFLAGS += -DACTUATORS_SKIRON_I2C_DEV=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME)
endif
# Simulator
nps.srcs += subsystems/actuators/actuators_skiron.c
nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DACTUATORS_SKIRON_DEVICE=i2c0
nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DACTUATORS_SKIRON_I2C_DEV=i2c0
conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
+12 -3
View File
@@ -49,15 +49,24 @@ IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_SLAVE_IDX=SPI_SLAVE0
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_DEV=spi1
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
ifndef ASPIRIN_I2C_DEV
ASPIRIN_I2C_DEV=i2c0
endif
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (ADXL345 CS)
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
ifndef ASPIRIN_I2C_DEV
ASPIRIN_I2C_DEV=i2c2
endif
endif
# convert i2cx to upper case
ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
#
# NPS simulator
conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
+11 -4
View File
@@ -38,12 +38,19 @@ IMU_ASPIRIN_SRCS += peripherals/itg3200.c
# Magnetometer
IMU_ASPIRIN_SRCS += peripherals/hmc58xx.c
# set default i2c bus
ifndef ASPIRIN_I2C_DEV
ifeq ($(ARCH), lpc21)
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
IMU_ASPIRIN_CFLAGS += -DI2C1_VIC_SLOT=12
ASPIRIN_I2C_DEV=i2c0
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
ASPIRIN_I2C_DEV=i2c2
endif
endif
# convert i2cx to upper case
ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
include $(CFG_SHARED)/imu_nps.makefile
conf/messages.xml
+13 -6
View File
@@ -1135,11 +1135,13 @@
<field name="cmd_thrust" type="int32"/>
</message>
<message name="GUIDANCE" id="144">
<field name="pos_n" type="int32"/>
<field name="pos_e" type="int32"/>
<field name="sp_pos_n" type="int32"/>
<field name="sp_pos_e" type="int32"/>
<message name="GUIDANCE_H_INT" id="144">
<field name="sp_x" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
<field name="sp_y" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
<field name="ref_x" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
<field name="ref_y" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
<field name="est_x" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
<field name="est_y" type="int32" alt_unit="m" alt_unit_coef="0.0039063"/>
</message>
<message name="VERT_LOOP" id="145">
@@ -1753,7 +1755,12 @@
<field name="electrical_current" type="int32" unit="mA"/>
</message>
<!--208 is free -->
<message name="UART_ERRORS" id="208">
<field name="overrun_cnt" type="uint16"/>
<field name="noise_err_cnt" type="uint16"/>
<field name="framing_err_cnt" type="uint16"/>
<field name="bus_number" type="uint8"/>
</message>
<message name="IMU_GYRO_LP" id="209">
<field name="gp" type="float" unit="rad/s"/>
conf/modules/sonar_adc.xml
+2 -2
View File
@@ -3,8 +3,8 @@
<module name="sonar">
<doc>
<description>
Sonar ADC driver
output - Sonar_distance in [m]
Sonar ADC driver.
Reads an anlog sonar sensor and outputs sonar distance in [m]
</description>
<configure name="ADC_SONAR" value="ADC_X" description="select ADC to use with the sonar"/>
<define name="SONAR_OFFSET" value="0.0" description="sensor offset in [m] - default is 0.0"/>
conf/modules/sonar_maxbotix_booz.xml → conf/modules/sonar_adc_ins.xml
+3 -8
View File
@@ -3,8 +3,9 @@
<module name="sonar">
<doc>
<description>
Sonar ADC INS.
On Booz board, ADC_0 should be use by default (payload connector)
Sonar ADC (INS bindings).
Sonar ADC driver with INS binding, wich oes the same than sonar_adc module with an event function to feed INS subsystem.
Even if SONAR_OFFSET and _SCALE can be set, currently only the raw value and the INS_SONAR_SENS will be used in ins filters.
</description>
<configure name="ADC_SONAR" value="ADC_X" description="select ADC to use with the sonar"/>
</doc>
@@ -19,12 +20,6 @@
<file name="sonar_adc.c"/>
</makefile>
<makefile target="ap">
<raw>
# set ADC_SONAR to ADC_0 as default
ifeq ($(ADC_SONAR),)
ADC_SONAR = ADC_0
endif
</raw>
<define name="ADC_CHANNEL_SONAR" value="$(ADC_SONAR)"/>
<define name="USE_$(ADC_SONAR)"/>
<define name="USE_SONAR"/>
conf/settings/control/rotorcraft_guidance.xml
+9 -8
View File
@@ -10,9 +10,10 @@
<dl_setting var="guidance_v_nominal_throttle" min="0.1" step="0.01" max="0.9" module="guidance/guidance_v" shortname="nominal_throttle" param="GUIDANCE_V_NOMINAL_HOVER_THROTTLE"/>
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins.vf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
</dl_settings>
<dl_settings NAME="Horiz Loop">
<dl_settings NAME="Horiz Loop">
<dl_setting var="guidance_h_use_ref" min="0" step="1" max="1" module="guidance/guidance_h" shortname="use_ref" values="FALSE|TRUE" handler="SetUseRef" param="GUIDANCE_H_USE_REF"/>
<dl_setting var="guidance_h_pgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kp" param="GUIDANCE_H_PGAIN"/>
<dl_setting var="guidance_h_dgain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="kd" param="GUIDANCE_H_DGAIN"/>
<dl_setting var="guidance_h_igain" min="0" step="1" max="400" module="guidance/guidance_h" shortname="ki" handler="SetKi" param="GUIDANCE_H_IGAIN"/>
@@ -20,13 +21,13 @@
<dl_setting var="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins.hf_realign" min="0" step="1" max="1" module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
</dl_settings>
<dl_settings NAME="NAV">
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="nav_heading" MIN="0" STEP="1" MAX="360" module="navigation" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
<dl_setting var="nav_radius" MIN="-150" STEP="0.1" MAX="150" module="navigation" unit="m"/>
</dl_settings>
<dl_settings NAME="NAV">
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="nav_heading" MIN="0" STEP="1" MAX="360" module="navigation" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
<dl_setting var="nav_radius" MIN="-150" STEP="0.1" MAX="150" module="navigation" unit="m"/>
</dl_settings>
</dl_settings>
</settings>
conf/telemetry/default_rotorcraft.xml
+10 -5
View File
@@ -16,7 +16,8 @@
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="I2C_ERRORS" period="6."/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
</mode>
<mode name="ppm">
@@ -94,14 +95,17 @@
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="STAB_ATTITUDE" period=".4"/>
<message name="HFF_DBG" period=".2"/>
<!--<message name="STAB_ATTITUDE_REF" period=".4"/>-->
<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="HFF_GPS" period=".03"/>
<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">
@@ -120,6 +124,7 @@
<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"/>
sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+13 -15
View File
@@ -213,15 +213,15 @@ void i2c0_ISR(void) {
ISR_EXIT(); // recover registers and return
}
uint8_t i2c0_vic_slot;
uint8_t i2c0_vic_channel;
/* SDA0 on P0.3 */
/* SCL0 on P0.2 */
void i2c0_hw_init ( void ) {
i2c0.reg_addr = I2C0;
i2c0_vic_slot = VIC_I2C0;
i2c0.init_struct = (void*)(&i2c0_vic_slot);
i2c0_vic_channel = VIC_I2C0;
i2c0.init_struct = (void*)(&i2c0_vic_channel);
/* set P0.2 and P0.3 to I2C0 */
PINSEL0 |= 1 << 4 | 1 << 6;
@@ -296,15 +296,15 @@ void i2c1_ISR(void) {
ISR_EXIT(); // recover registers and return
}
uint8_t i2c1_vic_slot;
uint8_t i2c1_vic_channel;
/* SDA1 on P0.14 */
/* SCL1 on P0.11 */
void i2c1_hw_init ( void ) {
i2c1.reg_addr = I2C1;
i2c1_vic_slot = VIC_I2C1;
i2c1.init_struct = (void*)(&i2c1_vic_slot);
i2c1_vic_channel = VIC_I2C1;
i2c1.init_struct = (void*)(&i2c1_vic_channel);
/* set P0.11 and P0.14 to I2C1 */
PINSEL0 |= 3 << 22 | 3 << 28;
@@ -331,7 +331,6 @@ bool_t i2c_idle(struct i2c_periph* p) {
}
bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
unsigned cpsr;
uint8_t idx;
idx = p->trans_insert_idx + 1;
@@ -342,10 +341,10 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
}
t->status = I2CTransPending;
uint8_t* vic = (uint8_t*)(p->init_struct);
cpsr = disableIRQ(); // disable global interrupts
VICIntEnClear = VIC_BIT(*vic);
restoreIRQ(cpsr); // restore global interrupts
/* disable I2C interrupt */
//uint8_t* vic = (uint8_t*)(p->init_struct);
//VICIntEnClear = VIC_BIT(*vic);
disableIRQ();
p->trans[p->trans_insert_idx] = t;
p->trans_insert_idx = idx;
@@ -355,10 +354,9 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
/* else it will be started by the interrupt handler */
/* when the previous transactions completes */
//int_enable();
cpsr = disableIRQ(); // disable global interrupts
VICIntEnable = VIC_BIT(*vic);
restoreIRQ(cpsr); // restore global interrupts
/* enable I2C interrupt again */
//VICIntEnable = VIC_BIT(*vic);
enableIRQ();
return TRUE;
}
sw/airborne/arch/lpc21/mcu_periph/spi_arch.c
+10 -8
View File
@@ -467,7 +467,7 @@ void spi1_arch_init(void) {
bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) {
unsigned cpsr;
//unsigned cpsr;
uint8_t idx;
idx = p->trans_insert_idx + 1;
@@ -479,10 +479,11 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) {
t->status = SPITransPending;
// Disable interrupts
uint8_t* vic = (uint8_t*)(p->init_struct);
cpsr = disableIRQ(); // disable global interrupts
VICIntEnClear = VIC_BIT(*vic);
restoreIRQ(cpsr); // restore global interrupts
//uint8_t* vic = (uint8_t*)(p->init_struct);
//cpsr = disableIRQ(); // disable global interrupts
//VICIntEnClear = VIC_BIT(*vic);
//restoreIRQ(cpsr); // restore global interrupts
disableIRQ();
p->trans[p->trans_insert_idx] = t;
p->trans_insert_idx = idx;
@@ -491,9 +492,10 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) {
SpiStart(p,p->trans[p->trans_extract_idx]);
}
cpsr = disableIRQ(); // disable global interrupts
VICIntEnable = VIC_BIT(*vic);
restoreIRQ(cpsr); // restore global interrupts
//cpsr = disableIRQ(); // disable global interrupts
//VICIntEnable = VIC_BIT(*vic);
//restoreIRQ(cpsr); // restore global interrupts
enableIRQ();
return TRUE;
}
sw/airborne/arch/stm32/mcu_periph/uart_arch.c
+39 -14
View File
@@ -53,6 +53,12 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_fl
usart_set_flow_control((u32)p->reg_addr, USART_FLOWCONTROL_NONE);
}
/* Disable Idle Line interrupt */
USART_CR1((u32)p->reg_addr) &= ~USART_CR1_IDLEIE;
/* Disable LIN break detection interrupt */
USART_CR2((u32)p->reg_addr) &= ~USART_CR2_LBDIE;
/* Enable USART1 Receive interrupts */
USART_CR1((u32)p->reg_addr) |= USART_CR1_RXNEIE;
@@ -88,7 +94,7 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) {
static inline void usart_isr(struct uart_periph* p) {
if (((USART_CR1((u32)p->reg_addr) & USART_CR1_TXEIE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_TXE) != 0)) {
((USART_SR((u32)p->reg_addr) & USART_SR_TXE) != 0)) {
// check if more data to send
if (p->tx_insert_idx != p->tx_extract_idx) {
usart_send((u32)p->reg_addr,p->tx_buf[p->tx_extract_idx]);
@@ -102,14 +108,34 @@ static inline void usart_isr(struct uart_periph* p) {
}
if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_RXNE) != 0)) {
((USART_SR((u32)p->reg_addr) & USART_SR_RXNE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_ORE) == 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_NE) == 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_FE) == 0)) {
uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;;
p->rx_buf[p->rx_insert_idx] = usart_recv((u32)p->reg_addr);
// check for more room in queue
if (temp != p->rx_extract_idx)
p->rx_insert_idx = temp; // update insert index
}
else {
/* ORE, NE or FE error - read USART_DR reg and log the error */
if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_ORE) != 0)) {
usart_recv((u32)p->reg_addr);
p->ore++;
}
if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_NE) != 0)) {
usart_recv((u32)p->reg_addr);
p->ne_err++;
}
if (((USART_CR1((u32)p->reg_addr) & USART_CR1_RXNEIE) != 0) &&
((USART_SR((u32)p->reg_addr) & USART_SR_FE) != 0)) {
usart_recv((u32)p->reg_addr);
p->fe_err++;
}
}
}
static inline void usart_enable_irq(u8 IRQn) {
@@ -196,9 +222,9 @@ void uart1_init( void ) {
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
gpio_set_mode(GPIO_BANK_USART1_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_TX);
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_TX);
gpio_set_mode(GPIO_BANK_USART1_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX);
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_RX);
#endif
/* Init GPIOS */
@@ -216,9 +242,9 @@ void uart1_init( void ) {
#if UART1_HW_FLOW_CONTROL
#warning "USING UART1 FLOW CONTROL. Make sure to pull down CTS if you are not connecting any flow-control-capable hardware."
gpio_set_mode(GPIO_BANK_USART1_RTS, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_RTS);
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART1_RTS);
gpio_set_mode(GPIO_BANK_USART1_CTS, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_CTS);
GPIO_CNF_INPUT_FLOAT, GPIO_USART1_CTS);
/* Configure USART1, enable hardware flow control*/
uart_periph_set_baudrate(&uart1, UART1_BAUD, TRUE);
@@ -255,9 +281,9 @@ void uart2_init( void ) {
#warning "UART2 is not yet working on STM32F4"
#elif defined(STM32F1)
gpio_set_mode(GPIO_BANK_USART2_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
gpio_set_mode(GPIO_BANK_USART2_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART2_RX);
GPIO_CNF_INPUT_FLOAT, GPIO_USART2_RX);
#endif
/* Configure USART */
@@ -294,9 +320,9 @@ void uart3_init( void ) {
#elif defined(STM32F1)
AFIO_MAPR |= AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP;
gpio_set_mode(GPIO_BANK_USART3_PR_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART3_PR_TX);
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART3_PR_TX);
gpio_set_mode(GPIO_BANK_USART3_PR_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_USART3_PR_RX);
GPIO_CNF_INPUT_FLOAT, GPIO_USART3_PR_RX);
#endif
/* Configure USART */
@@ -364,9 +390,9 @@ void uart5_init( void ) {
gpio_set_af(GPIOD, GPIO_AF8, GPIO2);
#elif defined(STM32F1)
gpio_set_mode(GPIO_BANK_UART5_TX, GPIO_MODE_OUTPUT_50_MHZ,
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_UART5_TX);
GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_UART5_TX);
gpio_set_mode(GPIO_BANK_UART5_RX, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO_UART5_RX);
GPIO_CNF_INPUT_FLOAT, GPIO_UART5_RX);
#endif
/* Configure USART */
@@ -405,4 +431,3 @@ void uart6_init( void ) {
void usart6_isr(void) { usart_isr(&uart6); }
#endif /* USE_UART6 */
sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c
+2 -2
View File
@@ -49,7 +49,7 @@ static uint32_t timer_rollover_cnt;
#if USE_PPM_TIM2
INFO("Using TIM2 for PPM input on PA_10 (SERVO6) pin.")
PRINT_CONFIG_MSG("Using TIM2 for PPM input on PA_10 (SERVO6) pin.")
#define PPM_RCC &RCC_APB1ENR
#define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN
@@ -64,7 +64,7 @@ INFO("Using TIM2 for PPM input on PA_10 (SERVO6) pin.")
#elif USE_PPM_TIM1
INFO("Using TIM1 for PPM input on PA_01 (UART1_RX) pin.")
PRINT_CONFIG_MSG("Using TIM1 for PPM input on PA_01 (UART1_RX) pin.")
#define PPM_RCC &RCC_APB2ENR
#define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN
sw/airborne/boards/lisa_l/baro_board.c
+2 -2
View File
@@ -68,10 +68,10 @@ void baro_periodic(void) {
void baro_board_send_config_abs(void) {
#ifndef BARO_LOW_GAIN
#pragma message "Using High LisaL Baro Gain: Do not use below 1000hPa"
INFO("Using High LisaL Baro Gain: Do not use below 1000hPa")
baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83);
#else
#pragma message "Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more"
INFO("Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more")
//config register should be 0x84 in low countries, or 0x86 in normal countries
baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x84, 0x83);
#endif
sw/airborne/boards/umarim/imu_umarim.h
+1 -1
View File
@@ -125,7 +125,7 @@ extern void imu_umarim_event( void );
extern void imu_umarim_downlink_raw( void );
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) {
imu_umarim_event();
if (imu_umarim.gyr_valid) {
imu_umarim.gyr_valid = FALSE;
sw/airborne/firmwares/rotorcraft/autopilot.c
+4
View File
@@ -156,6 +156,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
case AP_MODE_ATTITUDE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_FORWARD:
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
break;
@@ -186,6 +189,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE_DIRECT:
case AP_MODE_FORWARD:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
sw/airborne/firmwares/rotorcraft/autopilot.h
+1
View File
@@ -51,6 +51,7 @@
#define AP_MODE_NAV 12
#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
#define AP_MODE_CARE_FREE_DIRECT 14
#define AP_MODE_FORWARD 15
extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+71 -38
View File
@@ -34,7 +34,30 @@
#include "generated/airframe.h"
/* error if some gains are negative */
#if (GUIDANCE_H_PGAIN < 0) || \
(GUIDANCE_H_DGAIN < 0) || \
(GUIDANCE_H_IGAIN < 0)
#error "ALL control gains have to be positive!!!"
#endif
#ifndef GUIDANCE_H_AGAIN
#define GUIDANCE_H_AGAIN 0
#else
#if (GUIDANCE_H_AGAIN < 0)
#error "ALL control gains have to be positive!!!"
#endif
#endif
#ifndef GUIDANCE_H_MAX_BANK
#define GUIDANCE_H_MAX_BANK RadOfDeg(20)
#endif
PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
uint8_t guidance_h_mode;
bool_t guidance_h_use_ref;
struct Int32Vect2 guidance_h_pos_sp;
struct Int32Vect2 guidance_h_pos_ref;
@@ -56,33 +79,17 @@ int32_t guidance_h_dgain;
int32_t guidance_h_igain;
int32_t guidance_h_again;
/* warn if some gains are still negative */
#if (GUIDANCE_H_PGAIN < 0) || \
(GUIDANCE_H_DGAIN < 0) || \
(GUIDANCE_H_IGAIN < 0)
#warning "ALL control gains are now positive!!!"
#endif
int32_t transition_percentage;
int32_t transition_theta_offset;
#ifndef GUIDANCE_H_AGAIN
#define GUIDANCE_H_AGAIN 0
#else
#if (GUIDANCE_H_AGAIN < 0)
#warning "ALL control gains are now positive!!!"
#endif
#endif
#ifndef GUIDANCE_H_MAX_BANK
#define GUIDANCE_H_MAX_BANK RadOfDeg(20)
#endif
static inline void guidance_h_update_reference(bool_t use_ref);
static inline void guidance_h_traj_run(bool_t in_flight);
static inline void guidance_h_hover_enter(void);
static inline void guidance_h_nav_enter(void);
static void guidance_h_update_reference(void);
static void guidance_h_traj_run(bool_t in_flight);
static void guidance_h_hover_enter(void);
static void guidance_h_nav_enter(void);
static inline void transition_run(void);
#define GuidanceHSetRef(_pos, _speed, _accel) { \
gh_set_ref(_pos, _speed, _accel); \
gh_set_ref(_pos, _speed, _accel); \
VECT2_COPY(guidance_h_pos_ref, _pos); \
VECT2_COPY(guidance_h_speed_ref, _speed); \
VECT2_COPY(guidance_h_accel_ref, _accel); \
@@ -92,6 +99,8 @@ static inline void guidance_h_nav_enter(void);
void guidance_h_init(void) {
guidance_h_mode = GUIDANCE_H_MODE_KILL;
guidance_h_use_ref = GUIDANCE_H_USE_REF;
INT_VECT2_ZERO(guidance_h_pos_sp);
INT_VECT2_ZERO(guidance_h_pos_err_sum);
INT_EULERS_ZERO(guidance_h_rc_sp);
@@ -100,7 +109,8 @@ void guidance_h_init(void) {
guidance_h_igain = GUIDANCE_H_IGAIN;
guidance_h_dgain = GUIDANCE_H_DGAIN;
guidance_h_again = GUIDANCE_H_AGAIN;
transition_percentage = 0;
transition_theta_offset = 0;
}
@@ -108,6 +118,11 @@ void guidance_h_mode_changed(uint8_t new_mode) {
if (new_mode == guidance_h_mode)
return;
if(new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) {
transition_percentage = 0;
transition_theta_offset = 0;
}
switch (new_mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_enter();
@@ -119,6 +134,7 @@ void guidance_h_mode_changed(uint8_t new_mode) {
case GUIDANCE_H_MODE_CARE_FREE:
stabilization_attitude_reset_care_free_heading();
case GUIDANCE_H_MODE_FORWARD:
case GUIDANCE_H_MODE_ATTITUDE:
stabilization_attitude_enter();
break;
@@ -151,6 +167,7 @@ void guidance_h_read_rc(bool_t in_flight) {
stabilization_rate_read_rc();
break;
case GUIDANCE_H_MODE_FORWARD:
case GUIDANCE_H_MODE_CARE_FREE:
case GUIDANCE_H_MODE_ATTITUDE:
stabilization_attitude_read_rc(in_flight);
@@ -186,13 +203,17 @@ void guidance_h_run(bool_t in_flight) {
stabilization_rate_run(in_flight);
break;
case GUIDANCE_H_MODE_FORWARD:
if(transition_percentage < (100<<INT32_PERCENTAGE_FRAC)) {
transition_run();
}
case GUIDANCE_H_MODE_CARE_FREE:
case GUIDANCE_H_MODE_ATTITUDE:
stabilization_attitude_run(in_flight);
break;
case GUIDANCE_H_MODE_HOVER:
guidance_h_update_reference(FALSE);
guidance_h_update_reference();
/* set psi command */
guidance_h_command_body.psi = guidance_h_rc_sp.psi;
@@ -218,11 +239,8 @@ void guidance_h_run(bool_t in_flight) {
else {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
#if GUIDANCE_H_USE_REF
guidance_h_update_reference(TRUE);
#else
guidance_h_update_reference(FALSE);
#endif
guidance_h_update_reference();
/* set psi command */
guidance_h_command_body.psi = nav_heading;
/* compute roll and pitch commands and set final attitude setpoint */
@@ -237,10 +255,15 @@ void guidance_h_run(bool_t in_flight) {
}
static inline void guidance_h_update_reference(bool_t use_ref) {
/* convert our reference to generic representation */
if (use_ref) {
gh_update_ref_from_pos_sp(guidance_h_pos_sp);
static void guidance_h_update_reference(void) {
/* compute reference even if usage temporarily disabled via guidance_h_use_ref */
#if GUIDANCE_H_USE_REF
gh_update_ref_from_pos_sp(guidance_h_pos_sp);
#endif
/* either use the reference or simply copy the pos setpoint */
if (guidance_h_use_ref) {
/* convert our reference to generic representation */
INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_speed_ref, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_accel_ref, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC));
@@ -263,7 +286,7 @@ static inline void guidance_h_update_reference(bool_t use_ref) {
/** maximum bank angle: default 20 deg */
#define TRAJ_MAX_BANK BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC)
static inline void guidance_h_traj_run(bool_t in_flight) {
static void guidance_h_traj_run(bool_t in_flight) {
/* compute position error */
VECT2_DIFF(guidance_h_pos_err, guidance_h_pos_ref, *stateGetPositionNed_i());
@@ -325,7 +348,7 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
}
static inline void guidance_h_hover_enter(void) {
static void guidance_h_hover_enter(void) {
VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i());
@@ -336,7 +359,7 @@ static inline void guidance_h_hover_enter(void) {
}
static inline void guidance_h_nav_enter(void) {
static void guidance_h_nav_enter(void) {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
struct Int32Vect2 pos,speed,zero;
@@ -352,3 +375,13 @@ static inline void guidance_h_nav_enter(void) {
INT_VECT2_ZERO(guidance_h_pos_err_sum);
}
static inline void transition_run(void) {
//Add 0.00625%
transition_percentage += 1<<(INT32_PERCENTAGE_FRAC-4);
#ifdef TRANSITION_MAX_OFFSET
const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC);
#endif
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+24 -4
View File
@@ -31,6 +31,15 @@
#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
#include "std.h"
/** Use horizontal guidance reference trajectory.
* Default is TRUE, define to FALSE to always disable it.
*/
#ifndef GUIDANCE_H_USE_REF
#define GUIDANCE_H_USE_REF TRUE
#endif
#define GUIDANCE_H_MODE_KILL 0
#define GUIDANCE_H_MODE_RATE 1
@@ -39,9 +48,11 @@
#define GUIDANCE_H_MODE_NAV 4
#define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
extern uint8_t guidance_h_mode;
extern bool_t guidance_h_use_ref;
/** horizontal position setpoint in NED.
* fixed point representation: Q23.8
@@ -67,16 +78,25 @@ extern int32_t guidance_h_dgain;
extern int32_t guidance_h_igain;
extern int32_t guidance_h_again;
extern int32_t transition_percentage;
extern int32_t transition_theta_offset;
extern void guidance_h_init(void);
extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool_t in_flight);
extern void guidance_h_run(bool_t in_flight);
extern void guidance_h_read_rc(bool_t in_flight);
extern void guidance_h_run(bool_t in_flight);
#define guidance_h_SetKi(_val) { \
guidance_h_igain = _val; \
#define guidance_h_SetKi(_val) { \
guidance_h_igain = _val; \
INT_VECT2_ZERO(guidance_h_pos_err_sum); \
}
/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
#define guidance_h_SetUseRef(_val) { \
guidance_h_use_ref = _val && GUIDANCE_H_USE_REF; \
}
#endif /* GUIDANCE_H_H */
sw/airborne/firmwares/rotorcraft/navigation.c
+3
View File
@@ -39,6 +39,9 @@
#include "generated/modules.h"
#include "generated/flight_plan.h"
/* for default GUIDANCE_H_USE_REF */
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "math/pprz_algebra_int.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+5
View File
@@ -125,6 +125,11 @@ void stabilization_attitude_gain_schedule(uint8_t idx)
void stabilization_attitude_enter(void) {
float heading = stabilization_attitude_get_heading_f();
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = heading;
stabilization_attitude_ref_enter();
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+7 -1
View File
@@ -74,8 +74,10 @@ void stabilization_attitude_init(void) {
void stabilization_attitude_enter(void) {
int32_t heading = stabilization_attitude_get_heading_i();
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
stab_att_sp_euler.psi = heading;
stabilization_attitude_ref_enter();
@@ -177,6 +179,10 @@ void stabilization_attitude_run(bool_t enable_integrator) {
void stabilization_attitude_read_rc(bool_t in_flight) {
struct FloatQuat q_sp;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight);
#else
stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight);
#endif
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
+118 -13
View File
@@ -29,6 +29,7 @@
#include "state.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h"
#include "firmwares/rotorcraft/autopilot.h"
#if defined STABILIZATION_ATTITUDE_TYPE_INT
#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
@@ -53,6 +54,43 @@ void stabilization_attitude_reset_care_free_heading(void) {
care_free_heading = stateGetNedToBodyEulers_f()->psi;
}
/* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
However, when rolling more then 90 degrees in combination with pitch it switches. For a
transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
int32_t stabilization_attitude_get_heading_i(void) {
struct Int32Eulers* att = stateGetNedToBodyEulers_i();
int32_t heading;
if(abs(att->phi) < INT32_ANGLE_PI_2) {
int32_t sin_theta;
PPRZ_ITRIG_SIN(sin_theta, att->theta);
heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
}
else if(ANGLE_FLOAT_OF_BFP(att->theta) > 0)
heading = att->psi - att->phi;
else
heading = att->psi + att->phi;
return heading;
}
float stabilization_attitude_get_heading_f(void) {
struct FloatEulers* att = stateGetNedToBodyEulers_f();
float heading;
if(abs(att->phi) < M_PI/2) {
heading = att->psi - sinf(att->theta)*att->phi;
}
else if(att->theta > 0)
heading = att->psi - att->phi;
else
heading = att->psi + att->phi;
return heading;
}
/** Read attitude setpoint from RC as euler angles.
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
@@ -67,17 +105,32 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool
sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(sp->psi);
}
if (autopilot_mode == AP_MODE_FORWARD) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
if(abs(sp->phi) < max_phi)
omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
else //max 60 degrees roll, then take constant omega
omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi;
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit;
sp->psi = heading + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit;
sp->psi = heading - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
@@ -117,16 +170,31 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo
sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (autopilot_mode == AP_MODE_FORWARD) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
float omega;
const float max_phi = RadOfDeg(85.0);
if(abs(sp->phi) < max_phi)
omega = 1.3*tanf(sp->phi);
else //max 60 degrees roll, then take constant omega
omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
float delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi;
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
@@ -141,8 +209,8 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sin(care_free_delta_psi_f);
cos_psi = cos(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;
@@ -182,8 +250,9 @@ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q)
float qx_roll = sinf(roll2);
float qi_roll = cosf(roll2);
//An offset is added if in forward mode
/* only non-zero entries for pitch quaternion */
float pitch2 = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2;
float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) / 2;
float qy_pitch = sinf(pitch2);
float qi_pitch = cosf(pitch2);
@@ -205,11 +274,7 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool
#endif
struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
#else
stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
#endif
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
@@ -249,3 +314,43 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool
QUAT_COPY(*q_sp, q_rp_sp);
}
}
//Function that reads the rc setpoint in an earth bound frame
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight) {
// FIXME: remove me, do in quaternion directly
// is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight);
#endif
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_rp_cmd;
stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd);
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
#if defined STABILIZATION_ATTITUDE_TYPE_INT
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
#else
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi);
#endif
FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd);
}
else {
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi);
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd);
FLOAT_QUAT_NORMALIZE(q_rp_sp);
QUAT_COPY(*q_sp, q_rp_sp);
}
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+3
View File
@@ -56,10 +56,13 @@
radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
extern void stabilization_attitude_reset_care_free_heading(void);
extern int32_t stabilization_attitude_get_heading_i(void);
extern float stabilization_attitude_get_heading_f(void);
extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight);
extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight);
extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q);
extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q);
extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight);
extern void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight);
#endif /* STABILIZATION_ATTITUDE_RC_SETPOINT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h
+2 -1
View File
@@ -40,7 +40,8 @@ struct FloatRefModel {
extern struct FloatRefModel stab_att_ref_model[];
static inline void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = stateGetNedToBodyEulers_f()->psi;
//sp has been set from body using stabilization_attitude_get_yaw_f, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi;
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+2 -1
View File
@@ -58,7 +58,8 @@ extern struct Int32RefModel stab_att_ref_model;
static inline void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = stateGetNedToBodyEulers_i()->psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
//sp has been set from body using stabilization_attitude_get_yaw_i, use that value
stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}
sw/airborne/firmwares/rotorcraft/telemetry.h
+102 -11
View File
@@ -582,12 +582,14 @@
#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
#endif
#define PERIODIC_SEND_GUIDANCE(_trans, _dev) { \
DOWNLINK_SEND_GUIDANCE(_trans, _dev, \
&guidance_h_cur_pos.x, \
&guidance_h_cur_pos.y, \
&guidance_h_held_pos.x, \
&guidance_h_held_pos.y); \
#define PERIODIC_SEND_GUIDANCE_H_INT(_trans, _dev) { \
DOWNLINK_SEND_GUIDANCE_H_INT(_trans, _dev, \
&guidance_h_pos_sp.x, \
&guidance_h_pos_sp.y, \
&guidance_h_pos_ref.x, \
&guidance_h_pos_ref.y, \
&ins_ltp_pos.x, \
&ins_ltp_pos.y); \
}
#define PERIODIC_SEND_INS_Z(_trans, _dev) { \
@@ -874,11 +876,22 @@
#endif
#ifndef SITL
#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \
PERIODIC_SEND_I2C0_ERRORS(_trans, _dev); \
PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); \
PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); \
}
#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \
static uint8_t _i2c_nb_cnt = 0; \
switch (_i2c_nb_cnt) { \
case 0: \
PERIODIC_SEND_I2C0_ERRORS(_trans, _dev); break; \
case 1: \
PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); break; \
case 2: \
PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); break; \
default: \
break; \
} \
_i2c_nb_cnt++; \
if (_i2c_nb_cnt == 3) \
_i2c_nb_cnt = 0; \
}
#else
#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) {}
#endif
@@ -902,4 +915,82 @@
#include "generated/settings.h"
#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
/*
* Sending of UART errors.
*/
#ifdef USE_UART1
#define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \
const uint8_t _bus1 = 1; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart1.ore, \
&uart1.ne_err, \
&uart1.fe_err, \
&_bus1); \
}
#else
#define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) {}
#endif
#ifdef USE_UART2
#define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \
const uint8_t _bus2 = 2; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart2.ore, \
&uart2.ne_err, \
&uart2.fe_err, \
&_bus2); \
}
#else
#define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) {}
#endif
#ifdef USE_UART3
#define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \
const uint8_t _bus3 = 3; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart3.ore, \
&uart3.ne_err, \
&uart3.fe_err, \
&_bus3); \
}
#else
#define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) {}
#endif
#ifdef USE_UART5
#define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \
const uint8_t _bus5 = 5; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart5.ore, \
&uart5.ne_err, \
&uart5.fe_err, \
&_bus5); \
}
#else
#define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) {}
#endif
#ifndef SITL
#define PERIODIC_SEND_UART_ERRORS(_trans, _dev) { \
static uint8_t uart_nb_cnt = 0; \
switch (uart_nb_cnt) { \
case 0: \
PERIODIC_SEND_UART1_ERRORS(_trans, _dev); break; \
case 1: \
PERIODIC_SEND_UART2_ERRORS(_trans, _dev); break; \
case 2: \
PERIODIC_SEND_UART3_ERRORS(_trans, _dev); break; \
case 3: \
PERIODIC_SEND_UART5_ERRORS(_trans, _dev); break; \
default: break; \
} \
uart_nb_cnt++; \
if (uart_nb_cnt == 4) \
uart_nb_cnt = 0; \
}
#else
#define PERIODIC_SEND_UART_ERRORS(_trans, _dev) {}
#endif
#endif /* TELEMETRY_H */
sw/airborne/math/pprz_algebra_int.h
+2
View File
@@ -61,6 +61,8 @@ struct Int16Vect3 {
#define INT32_ACCEL_FRAC 10
#define INT32_MAG_FRAC 11
#define INT32_PERCENTAGE_FRAC 10
struct Int32Vect2 {
int32_t x;
int32_t y;
sw/airborne/mcu.c
+2 -1
View File
@@ -26,6 +26,7 @@
*/
#include "mcu.h"
#include "std.h"
#ifdef PERIPHERALS_AUTO_INIT
#include "mcu_periph/sys_time.h"
@@ -145,7 +146,7 @@ void mcu_init(void) {
dac_init();
#endif
#else
#pragma message "Info: Not auto-initializing mcu peripherals including sys_time"
INFO("PERIPHERALS_AUTO_INIT not enabled! Peripherals (including sys_time) need explicit initialization.")
#endif /* PERIPHERALS_AUTO_INIT */
}
sw/airborne/mcu_periph/uart.c
+3
View File
@@ -56,6 +56,9 @@ void uart_periph_init(struct uart_periph* p) {
p->tx_insert_idx = 0;
p->tx_extract_idx = 0;
p->tx_running = FALSE;
p->ore = 0;
p->ne_err = 0;
p->fe_err = 0;
}
bool_t uart_check_free_space(struct uart_periph* p, uint8_t len) {
sw/airborne/mcu_periph/uart.h
+9 -4
View File
@@ -48,26 +48,31 @@
#define B57600 57600
#define B115200 115200
#define B230400 230400
#define B921600 921600
/**
* UART peripheral
*/
struct uart_periph {
/* Receive buffer */
/** Receive buffer */
uint8_t rx_buf[UART_RX_BUFFER_SIZE];
uint16_t rx_insert_idx;
uint16_t rx_extract_idx;
/* Transmit buffer */
/** Transmit buffer */
uint8_t tx_buf[UART_TX_BUFFER_SIZE];
uint16_t tx_insert_idx;
uint16_t tx_extract_idx;
uint8_t tx_running;
/* UART Register */
/** UART Register */
void* reg_addr;
/* UART Dev (linux) */
/** UART Dev (linux) */
char dev[UART_DEV_NAME_SIZE];
volatile uint16_t ore; ///< overrun error counter
volatile uint16_t ne_err; ///< noise error counter
volatile uint16_t fe_err; ///< framing error counter
};
extern void uart_periph_init(struct uart_periph* p);
extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud, bool_t hw_flow_control);
extern void uart_transmit(struct uart_periph* p, uint8_t data);
sw/airborne/modules/config/config_mkk_v2.c
+2 -2
View File
@@ -143,7 +143,7 @@ void config_mkk_v2_read_eeprom(void)
config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG<<3);
config_mkk_v2.trans.len_r = 8;
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &config_mkk_v2.trans);
i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans);
}
void config_mkk_v2_parse_eeprom(void)
@@ -190,6 +190,6 @@ void config_mkk_v2_send_eeprom(void)
config_mkk_v2.trans.buf[8] = config_mkk_v2_eeprom.BitConfig;
config_mkk_v2.trans.buf[9] = config_mkk_v2_crc(2);
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &config_mkk_v2.trans);
i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans);
}
sw/airborne/modules/multi/tcas.c
+3
View File
@@ -33,6 +33,9 @@
#include "generated/flight_plan.h"
#include "messages.h"
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#include "subsystems/datalink/downlink.h"
float tcas_alt_setpoint;
sw/airborne/subsystems/actuators/actuators_asctec.c
+3 -1
View File
@@ -37,6 +37,8 @@
#define ACTUATORS_ASCTEC_SLAVE_ADDR 0x02
PRINT_CONFIG_VAR(ACTUATORS_ASCTEC_I2C_DEV)
struct ActuatorsAsctec actuators_asctec;
void actuators_asctec_init(void) {
@@ -120,7 +122,7 @@ void actuators_asctec_set(bool_t motors_on) {
}
actuators_asctec.cmd = NONE;
i2c_transmit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans,
i2c_transmit(&ACTUATORS_ASCTEC_I2C_DEV, &actuators_asctec.i2c_trans,
ACTUATORS_ASCTEC_SLAVE_ADDR, 4);
}

Some files were not shown because too many files have changed in this diff Show More