mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 04:46:51 +08:00
[fixedwing][feature] AP and FBW on different boards connected via Uart
* with example airframe and updated outdated airframes * renamed spi version and make it a separate subsystem (e.g. 2 twogs)
This commit is contained in:
Christophe De Wagter
committed by
Felix Ruess
Felix Ruess
parent
b542655602
commit
3fbef2e490
@@ -0,0 +1,317 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Lisa + Aspirin v2 using SPI only
|
||||||
|
-->
|
||||||
|
|
||||||
|
<airframe name="LisaAspirin2">
|
||||||
|
|
||||||
|
<servos>
|
||||||
|
<servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
|
||||||
|
<servo name="AILEVON_LEFT" no="1" min="1200" neutral="1500" max="1800"/>
|
||||||
|
<servo name="ELEVATOR" no="2" min="2000" neutral="1500" max="1000"/>
|
||||||
|
<servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
|
||||||
|
<servo name="AILEVON_RIGHT" no="4" min="1800" neutral="1500" max="1200"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="THROTTLE" failsafe_value="0"/>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="FORCECRASH" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<rc_commands>
|
||||||
|
<set command="THROTTLE" value="@THROTTLE"/>
|
||||||
|
<set command="ROLL" value="@ROLL"/>
|
||||||
|
<set command="PITCH" value="@PITCH"/>
|
||||||
|
<set command="YAW" value="@YAW"/>
|
||||||
|
<!-- <set command="FORCECRASH" value="@FLAPS"/> -->
|
||||||
|
</rc_commands>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<set servo="AILEVON_LEFT" value="@ROLL"/>
|
||||||
|
<set servo="AILEVON_RIGHT" value="-@ROLL"/>
|
||||||
|
<set servo="THROTTLE" value="@THROTTLE"/>
|
||||||
|
<set servo="ELEVATOR" value="@PITCH"/>
|
||||||
|
<set servo="RUDDER" value="@YAW"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<!-- Local magnetic field -->
|
||||||
|
<section name="AHRS" prefix="AHRS_">
|
||||||
|
<define name="H_X" value="0.51562740288882"/>
|
||||||
|
<define name="H_Y" value="-0.05707735220832"/>
|
||||||
|
<define name="H_Z" value="0.85490967783446"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<!-- Calibration Neutral -->
|
||||||
|
<define name="GYRO_P_NEUTRAL" value="0"/>
|
||||||
|
<define name="GYRO_Q_NEUTRAL" value="0"/>
|
||||||
|
<define name="GYRO_R_NEUTRAL" value="0"/>
|
||||||
|
|
||||||
|
<!-- SENS = 16.4 LSB/(deg/sec) * 57.6 deg/rad = 939.650 LSB/rad/sec / 12bit FRAC: 4096 / 939.65 -->
|
||||||
|
<define name="GYRO_P_SENS" value="4.359" integer="16"/>
|
||||||
|
<define name="GYRO_Q_SENS" value="4.359" integer="16"/>
|
||||||
|
<define name="GYRO_R_SENS" value="4.359" integer="16"/>
|
||||||
|
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||||
|
|
||||||
|
<!-- SENS = 2048 LSB/g / 9.81 ms2/g = 208.766564729 LSB/ms2 / 10bit FRAC: 1024 / 208.7665 -->
|
||||||
|
<define name="ACCEL_X_SENS" value="9.81" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="9.81" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="9.81" 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="1" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="1" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="1" integer="16"/>
|
||||||
|
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="0"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="0"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<define name="ROLL_NEUTRAL_DEFAULT" value="-0.0" unit="rad"/>
|
||||||
|
<define name="PITCH_NEUTRAL_DEFAULT" value="0.103000000119" unit="rad"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="TRIM" prefix="COMMAND_">
|
||||||
|
<define name="ROLL_TRIM" value="0"/>
|
||||||
|
<define name="PITCH_TRIM" value="788."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
<section name="AUTO1" prefix="AUTO1_">
|
||||||
|
<define name="MAX_ROLL" value="0.8"/>
|
||||||
|
<define name="MAX_PITCH" value="0.8"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
|
||||||
|
|
||||||
|
<define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
|
||||||
|
<define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="MISC">
|
||||||
|
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
|
||||||
|
<define name="CARROT" value="5." unit="s"/>
|
||||||
|
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||||
|
|
||||||
|
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||||
|
|
||||||
|
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||||
|
|
||||||
|
<define name="NAV_GLIDE_PITCH_TRIM" value="0."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="CATAPULT" prefix="NAV_CATAPULT_" >
|
||||||
|
<define name="MOTOR_DELAY" value="45" />
|
||||||
|
<define name="HEADING_DELAY" value="(60*3)" />
|
||||||
|
<define name="ACCELERATION_THRESHOLD" value="1.75" />
|
||||||
|
<define name="INITIAL_PITCH" value="(15.0/57.0)" />
|
||||||
|
<define name="INITIAL_THROTTLE" value="1.0" />
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GLS_APPROACH" prefix="APP_" >
|
||||||
|
<define name="ANGLE" value="5" />
|
||||||
|
<define name="INTERCEPT_AF_TOD" value="10" />
|
||||||
|
<define name="TARGET_SPEED" value="13" />
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||||
|
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
|
||||||
|
<!-- outer loop proportional gain -->
|
||||||
|
<define name="ALTITUDE_PGAIN" value="0.104999996722"/>
|
||||||
|
<!-- outer loop saturation -->
|
||||||
|
<define name="ALTITUDE_MAX_CLIMB" value="3."/>
|
||||||
|
<define name="ALTITUDE_PRE_CLIMB_CORRECTION" value="0.0960000008345"/>
|
||||||
|
<!--
|
||||||
|
<define name="AUTO_PITCH_PGAIN" value="-0.00"/>
|
||||||
|
<define name="AUTO_PITCH_IGAIN" value="-0.00"/>
|
||||||
|
-->
|
||||||
|
|
||||||
|
<!-- auto throttle inner loop -->
|
||||||
|
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.5"/>
|
||||||
|
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.36700001359"/>
|
||||||
|
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.626999974251"/>
|
||||||
|
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||||
|
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
|
||||||
|
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.518000006676" unit="%/(m/s)"/>
|
||||||
|
<define name="AUTO_THROTTLE_PGAIN" value="0.00"/>
|
||||||
|
<define name="AUTO_THROTTLE_IGAIN" value="0."/>
|
||||||
|
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.307000011206"/>
|
||||||
|
|
||||||
|
<define name="THROTTLE_SLEW_LIMITER" value="1" unit="s"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||||
|
<define name="COURSE_PGAIN" value="1.16600000858"/>
|
||||||
|
<define name="COURSE_DGAIN" value="0.324999988079"/>
|
||||||
|
<define name="COURSE_PRE_BANK_CORRECTION" value="0.889999985695"/>
|
||||||
|
|
||||||
|
<define name="ROLL_MAX_SETPOINT" value="0.586000025272" unit="radians"/>
|
||||||
|
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
|
||||||
|
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
|
||||||
|
|
||||||
|
<define name="PITCH_PGAIN" value="12587.4130859"/>
|
||||||
|
<define name="PITCH_DGAIN" value="1.5"/>
|
||||||
|
|
||||||
|
<define name="ELEVATOR_OF_ROLL" value="1273.88500977"/>
|
||||||
|
|
||||||
|
<define name="ROLL_SLEW" value="1."/>
|
||||||
|
|
||||||
|
<define name="ROLL_ATTITUDE_GAIN" value="7972.02783203"/>
|
||||||
|
<define name="ROLL_RATE_GAIN" value="500."/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="AGGRESSIVE" prefix="AGR_">
|
||||||
|
<define name="BLEND_START" value="70"/>
|
||||||
|
<define name="BLEND_END" value="60"/>
|
||||||
|
<define name="CLIMB_THROTTLE" value="0.949999988079"/>
|
||||||
|
<define name="CLIMB_PITCH" value="0.352999985218"/>
|
||||||
|
<define name="DESCENT_THROTTLE" value="0."/>
|
||||||
|
<define name="DESCENT_PITCH" value="-0.252000004053"/>
|
||||||
|
<define name="CLIMB_NAV_RATIO" value="0.8"/>
|
||||||
|
<define name="DESCENT_NAV_RATIO" value="1.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||||
|
<define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
|
||||||
|
<define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
|
||||||
|
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
|
||||||
|
|
||||||
|
<define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
|
||||||
|
<define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
|
||||||
|
<define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="DIGITAL_CAMERA" prefix="DC_">
|
||||||
|
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
|
||||||
|
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<modules>
|
||||||
|
<load name="gps_ubx_ucenter.xml"/>
|
||||||
|
<!--
|
||||||
|
<load name="adc_generic.xml">
|
||||||
|
<configure name="ADC_CHANNEL_GENERIC1" value="0" />
|
||||||
|
<configure name="ADC_CHANNEL_GENERIC2" value="1" />
|
||||||
|
</load>
|
||||||
|
-->
|
||||||
|
<load name="light.xml">
|
||||||
|
<define name="LIGHT_LED_STROBE" value="2"/>
|
||||||
|
<define name="LIGHT_LED_NAV" value="3"/>
|
||||||
|
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
|
||||||
|
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
|
||||||
|
</load>
|
||||||
|
|
||||||
|
<!-- <load name="digital_cam_i2c.xml"/> -->
|
||||||
|
<load name="digital_cam.xml">
|
||||||
|
<define name="DC_SHUTTER_LED" value="3"/>
|
||||||
|
</load>
|
||||||
|
|
||||||
|
<load name="nav_catapult.xml"/>
|
||||||
|
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<firmware name="fixedwing">
|
||||||
|
|
||||||
|
<target name="fbw" board="lisa_m_2.0">
|
||||||
|
<configure name="SYS_TIME_LED" value="2" />
|
||||||
|
<define name="RADIO_CONTROL_LED" value="4"/>
|
||||||
|
|
||||||
|
<configure name="FLASH_MODE" value="JTAG" />
|
||||||
|
<configure name="NO_LUFTBOOT" value="1" />
|
||||||
|
<!-- <configure name="BMP_PORT" value="/dev/ttyACM0" /> -->
|
||||||
|
|
||||||
|
<define name="OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP" value="1" />
|
||||||
|
<define name="OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE" value="1" />
|
||||||
|
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<subsystem name="radio_control" type="ppm"/>
|
||||||
|
<subsystem name="actuators" type="direct"/>
|
||||||
|
<subsystem name="intermcu" type="uart">
|
||||||
|
<configure name="INTERMCU_PORT_NR" value="2" />
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<firmware name="fixedwing">
|
||||||
|
|
||||||
|
<target name="ap" board="lisa_m_2.0">
|
||||||
|
<define name="LISA_M_LONGITUDINAL_X"/>
|
||||||
|
|
||||||
|
<configure name="SEPARATE_FBW" value="1"/>
|
||||||
|
|
||||||
|
<configure name="PERIODIC_FREQUENCY" value="120"/>
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
|
||||||
|
<configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
|
||||||
|
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP"/>
|
||||||
|
|
||||||
|
<configure name="FLASH_MODE" value="JTAG" />
|
||||||
|
<configure name="NO_LUFTBOOT" value="1" />
|
||||||
|
<!-- <configure name="BMP_PORT" value="/dev/ttyACM0" /> -->
|
||||||
|
|
||||||
|
|
||||||
|
<configure name="AHRS_ALIGNER_LED" value="1"/>
|
||||||
|
<configure name="CPU_LED" value="1"/>
|
||||||
|
</target>
|
||||||
|
<target name="sim" board="pc"/>
|
||||||
|
|
||||||
|
<define name="AGR_CLIMB"/>
|
||||||
|
<define name="LOITER_TRIM"/>
|
||||||
|
<define name="ALT_KALMAN"/>
|
||||||
|
<define name="TUNE_AGRESSIVE_CLIMB"/>
|
||||||
|
<define name="STRONG_WIND"/>
|
||||||
|
<define name="WIND_INFO"/>
|
||||||
|
<define name="WIND_INFO_RET"/>
|
||||||
|
<define name="RADIO_CONTROL_AUTO1"/>
|
||||||
|
|
||||||
|
|
||||||
|
<subsystem name="imu" type="aspirin_v2.1"/>
|
||||||
|
<subsystem name="ahrs" type="float_dcm" />
|
||||||
|
|
||||||
|
<!-- Communication -->
|
||||||
|
<!-- <subsystem name="telemetry" type="xbee_api"> -->
|
||||||
|
<subsystem name="telemetry" type="transparent">
|
||||||
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
|
<configure name="MODEM_PORT" value="UART2"/>
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
<!-- Actuators -->
|
||||||
|
<subsystem name="control"/>
|
||||||
|
<!-- Sensors -->
|
||||||
|
<subsystem name="navigation" type="extra"/>
|
||||||
|
<subsystem name="gps" type="ublox">
|
||||||
|
<configure name="GPS_PORT" value="UART1"/>
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
<subsystem name="actuators" type="dummy"/>
|
||||||
|
|
||||||
|
<subsystem name="intermcu" type="uart">
|
||||||
|
<configure name="INTERMCU_PORT_NR" value="5" />
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<makefile>
|
||||||
|
|
||||||
|
|
||||||
|
</makefile>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -186,7 +186,7 @@ fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLI
|
|||||||
fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
|
fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
|
||||||
|
|
||||||
fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
||||||
fbw.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
|
fbw.srcs += inter_mcu.c link_mcu_spi.c spi.c $(SRC_ARCH)/spi_hw.c
|
||||||
|
|
||||||
fbw.CFLAGS += -DADC -DUSE_AD0
|
fbw.CFLAGS += -DADC -DUSE_AD0
|
||||||
fbw.srcs += $(SRC_ARCH)/adc_hw.c
|
fbw.srcs += $(SRC_ARCH)/adc_hw.c
|
||||||
@@ -202,7 +202,7 @@ ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLIN
|
|||||||
ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
|
ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
|
||||||
|
|
||||||
ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
|
ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
|
||||||
ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
|
ap.srcs += inter_mcu.c link_mcu_spi.c spi.c $(SRC_ARCH)/spi_hw.c
|
||||||
|
|
||||||
ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
|
ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
|
||||||
ap.srcs += gps_ubx.c gps.c latlong.c
|
ap.srcs += gps_ubx.c gps.c latlong.c
|
||||||
|
|||||||
@@ -195,7 +195,7 @@ fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLI
|
|||||||
fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
|
fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
|
||||||
|
|
||||||
fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
||||||
fbw.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
|
fbw.srcs += inter_mcu.c link_mcu_spi.c spi.c $(SRC_ARCH)/spi_hw.c
|
||||||
|
|
||||||
fbw.CFLAGS += -DADC -DUSE_AD0
|
fbw.CFLAGS += -DADC -DUSE_AD0
|
||||||
fbw.srcs += $(SRC_ARCH)/adc_hw.c
|
fbw.srcs += $(SRC_ARCH)/adc_hw.c
|
||||||
@@ -208,7 +208,7 @@ ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLIN
|
|||||||
ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c subsystems/datalink/pprz_transport.c
|
ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c subsystems/datalink/pprz_transport.c
|
||||||
|
|
||||||
ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER
|
ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER
|
||||||
ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
|
ap.srcs += inter_mcu.c link_mcu_spi.c spi.c $(SRC_ARCH)/spi_hw.c
|
||||||
|
|
||||||
ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
|
ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
|
||||||
ap.srcs += gps_ubx.c gps.c latlong.c
|
ap.srcs += gps_ubx.c gps.c latlong.c
|
||||||
|
|||||||
@@ -239,15 +239,16 @@ jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
|
|||||||
#
|
#
|
||||||
|
|
||||||
ifeq ($(BOARD),classix)
|
ifeq ($(BOARD),classix)
|
||||||
fbw.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
include $(CFG_FIXEDWING)/intermcu_spi.makefile
|
||||||
fbw.srcs += $(SRC_FIXEDWING)/link_mcu.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
|
|
||||||
ap_srcs += $(SRC_FIRMWARE)/fbw_downlink.c
|
|
||||||
ap.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
|
|
||||||
ap.srcs += $(SRC_FIXEDWING)/link_mcu.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
|
|
||||||
else
|
else
|
||||||
# Single MCU's run both
|
# Single MCU's run both
|
||||||
ap.CFLAGS += $(fbw_CFLAGS)
|
ifeq ($(SEPARATE_FBW),)
|
||||||
ap.srcs += $(fbw_srcs)
|
ap.CFLAGS += $(fbw_CFLAGS)
|
||||||
|
ap.srcs += $(fbw_srcs)
|
||||||
|
else
|
||||||
|
# avoid fbw_telemetry_mode error
|
||||||
|
ap_srcs += $(SRC_FIRMWARE)/fbw_downlink.c
|
||||||
|
endif
|
||||||
endif
|
endif
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|||||||
@@ -0,0 +1,11 @@
|
|||||||
|
# Hey Emacs, this is a -*- makefile -*-
|
||||||
|
|
||||||
|
# InterMCU type SPI
|
||||||
|
|
||||||
|
|
||||||
|
fbw.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
|
||||||
|
fbw.srcs += $(SRC_FIXEDWING)/link_mcu_spi.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
|
||||||
|
ap_srcs += $(SRC_FIRMWARE)/fbw_downlink.c
|
||||||
|
ap.CFLAGS += -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
|
||||||
|
ap.srcs += $(SRC_FIXEDWING)/link_mcu_spi.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
|
||||||
|
SEPARATE_FBW = 1
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
# Hey Emacs, this is a -*- makefile -*-
|
||||||
|
|
||||||
|
# InterMCU type UART
|
||||||
|
|
||||||
|
|
||||||
|
ifeq ($(TARGET),fbw)
|
||||||
|
ifeq ($(INTERMCU_PORT),none)
|
||||||
|
INTERMCU_PORT_NR = 2
|
||||||
|
endif
|
||||||
|
fbw.CFLAGS += -DINTERMCU_LINK=Uart$(INTERMCU_PORT_NR) -DUSE_UART$(INTERMCU_PORT_NR) -DUART$(INTERMCU_PORT_NR)_BAUD=B57600
|
||||||
|
else
|
||||||
|
ifeq ($(INTERMCU_PORT),none)
|
||||||
|
INTERMCU_PORT_NR = 5
|
||||||
|
endif
|
||||||
|
ap.CFLAGS += -DINTERMCU_LINK=Uart$(INTERMCU_PORT_NR) -DUSE_UART$(INTERMCU_PORT_NR) -DUART$(INTERMCU_PORT_NR)_BAUD=B57600
|
||||||
|
endif
|
||||||
|
|
||||||
|
$(TARGET).CFLAGS += -DINTER_MCU -DMCU_UART_LINK
|
||||||
|
$(TARGET).srcs += ./link_mcu_usart.c
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#############################
|
||||||
|
# CAN:
|
||||||
|
# fbw.srcs += ./link_mcu_can.c ./mcu_periph/can.c ./arch/stm32/mcu_periph/can_arch.c
|
||||||
|
# fbw.CFLAGS += -DINTER_MCU -DMCU_CAN_LINK
|
||||||
|
# $(TARGET).CFLAGS += -DCAN_SJW_TQ=CAN_SJW_1tq -DCAN_BS1_TQ=CAN_BS1_3tq -DCAN_BS2_TQ=CAN_BS2_4tq -DCAN_PRESCALER=12 -DUSE_CAN -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DCAN_ERR_RESUME=DISABLE
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
# for classix and AP only boards
|
||||||
@@ -188,7 +188,7 @@ void init_ap( void ) {
|
|||||||
stateInit();
|
stateInit();
|
||||||
|
|
||||||
/************* Links initialization ***************/
|
/************* Links initialization ***************/
|
||||||
#if defined MCU_SPI_LINK
|
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
|
||||||
link_mcu_init();
|
link_mcu_init();
|
||||||
#endif
|
#endif
|
||||||
#if USE_AUDIO_TELEMETRY
|
#if USE_AUDIO_TELEMETRY
|
||||||
@@ -533,7 +533,7 @@ void attitude_loop( void ) {
|
|||||||
|
|
||||||
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
|
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
|
||||||
|
|
||||||
#if defined MCU_SPI_LINK
|
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
|
||||||
link_mcu_send();
|
link_mcu_send();
|
||||||
#elif defined INTER_MCU && defined SINGLE_MCU
|
#elif defined INTER_MCU && defined SINGLE_MCU
|
||||||
/**Directly set the flag indicating to FBW that shared buffer is available*/
|
/**Directly set the flag indicating to FBW that shared buffer is available*/
|
||||||
@@ -639,7 +639,7 @@ void event_task_ap( void ) {
|
|||||||
DatalinkEvent();
|
DatalinkEvent();
|
||||||
|
|
||||||
|
|
||||||
#ifdef MCU_SPI_LINK
|
#if defined MCU_SPI_LINK || defined MCU_UART_LINK
|
||||||
link_mcu_event_task();
|
link_mcu_event_task();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|||||||
@@ -49,6 +49,10 @@
|
|||||||
#include "link_mcu.h"
|
#include "link_mcu.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef MCU_UART_LINK
|
||||||
|
#include "link_mcu_usart.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
uint8_t fbw_mode;
|
uint8_t fbw_mode;
|
||||||
|
|
||||||
#include "inter_mcu.h"
|
#include "inter_mcu.h"
|
||||||
@@ -125,7 +129,7 @@ void event_task_fbw( void) {
|
|||||||
i2c_event();
|
i2c_event();
|
||||||
|
|
||||||
#ifdef INTER_MCU
|
#ifdef INTER_MCU
|
||||||
#ifdef MCU_SPI_LINK
|
#if defined MCU_SPI_LINK | defined MCU_UART_LINK
|
||||||
link_mcu_event_task();
|
link_mcu_event_task();
|
||||||
#endif /* MCU_SPI_LINK */
|
#endif /* MCU_SPI_LINK */
|
||||||
|
|
||||||
@@ -204,6 +208,11 @@ void periodic_task_fbw( void ) {
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef MCU_UART_LINK
|
||||||
|
inter_mcu_fill_fbw_state();
|
||||||
|
link_mcu_periodic_task();
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef DOWNLINK
|
#ifdef DOWNLINK
|
||||||
fbw_downlink_periodic_task();
|
fbw_downlink_periodic_task();
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -0,0 +1,353 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010-2012 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.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "link_mcu_usart.h"
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
#include "led.h"
|
||||||
|
|
||||||
|
#include "commands.h"
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// LINK
|
||||||
|
|
||||||
|
#define __InterMcuLink(dev, _x) dev##_x
|
||||||
|
#define _InterMcuLink(dev, _x) __InterMcuLink(dev, _x)
|
||||||
|
#define InterMcuLink(_x) _InterMcuLink(INTERMCU_LINK, _x)
|
||||||
|
|
||||||
|
#define InterMcuBuffer() InterMcuLink(ChAvailable())
|
||||||
|
|
||||||
|
#define InterMcuUartSend1(c) InterMcuLink(Transmit(c))
|
||||||
|
#define InterMcuUartSetBaudrate(_a) InterMcuLink(SetBaudrate(_a))
|
||||||
|
#define InterMcuUartRunning InterMcuLink(TxRunning)
|
||||||
|
#define InterMcuUartSendMessage InterMcuLink(SendMessage)
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// PROTOCOL
|
||||||
|
|
||||||
|
#define INTERMCU_SYNC1 0xB5
|
||||||
|
#define INTERMCU_SYNC2 0x62
|
||||||
|
|
||||||
|
#define InterMcuInitCheksum() { intermcu_data.send_ck_a = intermcu_data.send_ck_b = 0; }
|
||||||
|
#define UpdateChecksum(c) { intermcu_data.send_ck_a += c; intermcu_data.send_ck_b += intermcu_data.send_ck_a; }
|
||||||
|
#define InterMcuTrailer() { InterMcuUartSend1(intermcu_data.send_ck_a); InterMcuUartSend1(intermcu_data.send_ck_b); InterMcuUartSendMessage(); }
|
||||||
|
|
||||||
|
#define InterMcuSend1(c) { uint8_t i8=c; InterMcuUartSend1(i8); UpdateChecksum(i8); }
|
||||||
|
#define InterMcuSend2(c) { uint16_t i16=c; InterMcuSend1(i16&0xff); InterMcuSend1(i16 >> 8); }
|
||||||
|
#define InterMcuSend1ByAddr(x) { InterMcuSend1(*x); }
|
||||||
|
#define InterMcuSend2ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); }
|
||||||
|
#define InterMcuSend4ByAddr(x) { InterMcuSend1(*x); InterMcuSend1(*(x+1)); InterMcuSend1(*(x+2)); InterMcuSend1(*(x+3)); }
|
||||||
|
|
||||||
|
#define InterMcuHeader(nav_id, msg_id, len) { \
|
||||||
|
InterMcuUartSend1(INTERMCU_SYNC1); \
|
||||||
|
InterMcuUartSend1(INTERMCU_SYNC2); \
|
||||||
|
InterMcuInitCheksum(); \
|
||||||
|
InterMcuSend1(nav_id); \
|
||||||
|
InterMcuSend1(msg_id); \
|
||||||
|
InterMcuSend2(len); \
|
||||||
|
}
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// MESSAGES
|
||||||
|
|
||||||
|
// class
|
||||||
|
#define MSG_INTERMCU_ID 100
|
||||||
|
|
||||||
|
|
||||||
|
#define MSG_INTERMCU_COMMAND_ID 0x05
|
||||||
|
#define MSG_INTERMCU_COMMAND_LENGTH (2*(COMMANDS_NB))
|
||||||
|
#define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
|
||||||
|
|
||||||
|
#define InterMcuSend_INTERMCU_COMMAND(cmd) { \
|
||||||
|
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\
|
||||||
|
for (int i=0;i<COMMANDS_NB;i++) { \
|
||||||
|
uint16_t _cmd = cmd[i]; \
|
||||||
|
InterMcuSend2ByAddr((uint8_t*)&_cmd);\
|
||||||
|
} \
|
||||||
|
InterMcuTrailer();\
|
||||||
|
}
|
||||||
|
|
||||||
|
#define MSG_INTERMCU_RADIO_ID 0x08
|
||||||
|
#define MSG_INTERMCU_RADIO_LENGTH (2*(RADIO_CONTROL_NB_CHANNEL))
|
||||||
|
#define MSG_INTERMCU_RADIO(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8)
|
||||||
|
|
||||||
|
#define InterMcuSend_INTERMCU_RADIO(cmd) { \
|
||||||
|
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_RADIO_ID, MSG_INTERMCU_RADIO_LENGTH);\
|
||||||
|
for (int i=0;i<RADIO_CONTROL_NB_CHANNEL;i++) { \
|
||||||
|
uint16_t _cmd = cmd[i]; \
|
||||||
|
InterMcuSend2ByAddr((uint8_t*)&_cmd);\
|
||||||
|
} \
|
||||||
|
InterMcuTrailer();\
|
||||||
|
}
|
||||||
|
|
||||||
|
#define MSG_INTERMCU_FBW_ID 0x06
|
||||||
|
#define MSG_INTERMCU_FBW_MOD(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+0))
|
||||||
|
#define MSG_INTERMCU_FBW_STAT(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+1))
|
||||||
|
#define MSG_INTERMCU_FBW_ERR(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+2))
|
||||||
|
#define MSG_INTERMCU_FBW_VOLT(_intermcu_payload) (uint8_t)(*((uint8_t*)_intermcu_payload+3))
|
||||||
|
#define MSG_INTERMCU_FBW_CURRENT(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+4)|*((uint8_t*)_intermcu_payload+1+4)<<8)
|
||||||
|
|
||||||
|
#define InterMcuSend_INTERMCU_FBW(mod,stat,err,volt,current) { \
|
||||||
|
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_FBW_ID, 6);\
|
||||||
|
uint8_t _mod = mod; InterMcuSend1ByAddr((uint8_t*)&_mod);\
|
||||||
|
uint8_t _stat = stat; InterMcuSend1ByAddr((uint8_t*)&_stat);\
|
||||||
|
uint8_t _err = err; InterMcuSend1ByAddr((uint8_t*)&_err);\
|
||||||
|
uint8_t _volt = volt; InterMcuSend1ByAddr((uint8_t*)&_volt);\
|
||||||
|
uint16_t _current = current; InterMcuSend2ByAddr((uint8_t*)&_current);\
|
||||||
|
InterMcuTrailer();\
|
||||||
|
}
|
||||||
|
|
||||||
|
#define MSG_INTERMCU_TRIM_ID 0x07
|
||||||
|
#define MSG_INTERMCU_TRIM_ROLL(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+0)|*((uint8_t*)_intermcu_payload+1)<<8)
|
||||||
|
#define MSG_INTERMCU_TRIM_PITCH(_intermcu_payload) (uint16_t)(*((uint8_t*)_intermcu_payload+2)|*((uint8_t*)_intermcu_payload+3)<<8)
|
||||||
|
|
||||||
|
#define InterMcuSend_INTERMCU_TRIM(roll,pitch) { \
|
||||||
|
InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_TRIM_ID, 4);\
|
||||||
|
uint16_t _roll = roll; InterMcuSend2ByAddr((uint8_t*)&_roll);\
|
||||||
|
uint16_t _pitch = pitch; InterMcuSend2ByAddr((uint8_t*)&_pitch);\
|
||||||
|
InterMcuTrailer();\
|
||||||
|
}
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// PARSER
|
||||||
|
|
||||||
|
/* parser status */
|
||||||
|
#define UNINIT 0
|
||||||
|
#define GOT_SYNC1 1
|
||||||
|
#define GOT_SYNC2 2
|
||||||
|
#define GOT_CLASS 3
|
||||||
|
#define GOT_ID 4
|
||||||
|
#define GOT_LEN1 5
|
||||||
|
#define GOT_LEN2 6
|
||||||
|
#define GOT_PAYLOAD 7
|
||||||
|
#define GOT_CHECKSUM1 8
|
||||||
|
|
||||||
|
|
||||||
|
#define INTERMCU_MAX_PAYLOAD 255
|
||||||
|
struct InterMcuData {
|
||||||
|
bool_t msg_available;
|
||||||
|
uint8_t msg_buf[INTERMCU_MAX_PAYLOAD] __attribute__ ((aligned));
|
||||||
|
uint8_t msg_id;
|
||||||
|
uint8_t msg_class;
|
||||||
|
|
||||||
|
uint8_t status;
|
||||||
|
uint16_t len;
|
||||||
|
uint8_t msg_idx;
|
||||||
|
uint8_t ck_a, ck_b;
|
||||||
|
uint8_t send_ck_a, send_ck_b;
|
||||||
|
uint8_t error_cnt;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct InterMcuData intermcu_data;
|
||||||
|
|
||||||
|
/* INTERMCU parsing */
|
||||||
|
void intermcu_parse( uint8_t c );
|
||||||
|
void intermcu_parse( uint8_t c ) {
|
||||||
|
if (intermcu_data.status < GOT_PAYLOAD) {
|
||||||
|
intermcu_data.ck_a += c;
|
||||||
|
intermcu_data.ck_b += intermcu_data.ck_a;
|
||||||
|
}
|
||||||
|
switch (intermcu_data.status) {
|
||||||
|
case UNINIT:
|
||||||
|
if (c == INTERMCU_SYNC1)
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_SYNC1:
|
||||||
|
if (c != INTERMCU_SYNC2) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
intermcu_data.ck_a = 0;
|
||||||
|
intermcu_data.ck_b = 0;
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_SYNC2:
|
||||||
|
if (intermcu_data.msg_available) {
|
||||||
|
/* Previous message has not yet been parsed: discard this one */
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
intermcu_data.msg_class = c;
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_CLASS:
|
||||||
|
intermcu_data.msg_id = c;
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_ID:
|
||||||
|
intermcu_data.len = c;
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_LEN1:
|
||||||
|
intermcu_data.len |= (c<<8);
|
||||||
|
if (intermcu_data.len > INTERMCU_MAX_PAYLOAD) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
intermcu_data.msg_idx = 0;
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_LEN2:
|
||||||
|
intermcu_data.msg_buf[intermcu_data.msg_idx] = c;
|
||||||
|
intermcu_data.msg_idx++;
|
||||||
|
if (intermcu_data.msg_idx >= intermcu_data.len) {
|
||||||
|
intermcu_data.status++;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case GOT_PAYLOAD:
|
||||||
|
if (c != intermcu_data.ck_a) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
intermcu_data.status++;
|
||||||
|
break;
|
||||||
|
case GOT_CHECKSUM1:
|
||||||
|
if (c != intermcu_data.ck_b) {
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
intermcu_data.msg_available = TRUE;
|
||||||
|
goto restart;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
goto error;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
error:
|
||||||
|
intermcu_data.error_cnt++;
|
||||||
|
restart:
|
||||||
|
intermcu_data.status = UNINIT;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// USER
|
||||||
|
|
||||||
|
|
||||||
|
struct link_mcu_msg link_mcu_from_ap_msg;
|
||||||
|
struct link_mcu_msg link_mcu_from_fbw_msg;
|
||||||
|
|
||||||
|
inline void parse_mavpilot_msg( void );
|
||||||
|
|
||||||
|
void link_mcu_init( void )
|
||||||
|
{
|
||||||
|
intermcu_data.status = UNINIT;
|
||||||
|
intermcu_data.msg_available = FALSE;
|
||||||
|
intermcu_data.error_cnt = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void parse_mavpilot_msg( void )
|
||||||
|
{
|
||||||
|
if (intermcu_data.msg_class == MSG_INTERMCU_ID)
|
||||||
|
{
|
||||||
|
if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID)
|
||||||
|
{
|
||||||
|
#if COMMANDS_NB > 8
|
||||||
|
#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (int i=0; i< COMMANDS_NB; i++)
|
||||||
|
{
|
||||||
|
ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i));
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef LINK_MCU_LED
|
||||||
|
LED_TOGGLE(LINK_MCU_LED);
|
||||||
|
#endif
|
||||||
|
inter_mcu_received_ap = TRUE;
|
||||||
|
}
|
||||||
|
else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID)
|
||||||
|
{
|
||||||
|
#if RADIO_CONTROL_NB_CHANNEL > 10
|
||||||
|
#error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++)
|
||||||
|
{
|
||||||
|
fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID)
|
||||||
|
{
|
||||||
|
ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
|
||||||
|
ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
|
||||||
|
}
|
||||||
|
else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID)
|
||||||
|
{
|
||||||
|
fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf);
|
||||||
|
fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf);
|
||||||
|
fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf);
|
||||||
|
fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf);
|
||||||
|
fbw_state->current = MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf);
|
||||||
|
|
||||||
|
inter_mcu_received_fbw = TRUE;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef AP
|
||||||
|
void link_mcu_send( void )
|
||||||
|
{
|
||||||
|
#ifdef LINK_MCU_LED
|
||||||
|
LED_TOGGLE(LINK_MCU_LED);
|
||||||
|
#endif
|
||||||
|
InterMcuSend_INTERMCU_COMMAND( ap_state->commands );
|
||||||
|
InterMcuSend_INTERMCU_TRIM( ap_state->command_roll_trim, ap_state->command_pitch_trim );
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef FBW
|
||||||
|
// 60 Hz
|
||||||
|
static uint8_t SixtyHzCounter = 0;
|
||||||
|
|
||||||
|
void link_mcu_periodic_task( void )
|
||||||
|
{
|
||||||
|
SixtyHzCounter++;
|
||||||
|
if (SixtyHzCounter >= 3)
|
||||||
|
{
|
||||||
|
// 20 Hz
|
||||||
|
SixtyHzCounter = 0;
|
||||||
|
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
|
||||||
|
|
||||||
|
InterMcuSend_INTERMCU_FBW(
|
||||||
|
fbw_state->ppm_cpt,
|
||||||
|
fbw_state->status,
|
||||||
|
fbw_state->nb_err,
|
||||||
|
fbw_state->vsupply,
|
||||||
|
fbw_state->current);
|
||||||
|
#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
|
||||||
|
InterMcuSend_INTERMCU_RADIO( fbw_state->channels );
|
||||||
|
#endif
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void link_mcu_event_task( void ) {
|
||||||
|
/* A message has been received */
|
||||||
|
if (InterMcuBuffer()) {
|
||||||
|
while (InterMcuLink(ChAvailable())&&!intermcu_data.msg_available)
|
||||||
|
intermcu_parse(InterMcuLink(Getch()));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (intermcu_data.msg_available) {
|
||||||
|
parse_mavpilot_msg();
|
||||||
|
intermcu_data.msg_available = FALSE;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010-2012 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.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** \brief Transport for the communication between FBW and AP via UART.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef LINK_MCU_H
|
||||||
|
#define LINK_MCU_H
|
||||||
|
|
||||||
|
#include <inttypes.h>
|
||||||
|
#include "inter_mcu.h"
|
||||||
|
|
||||||
|
struct link_mcu_msg {
|
||||||
|
union {
|
||||||
|
struct fbw_state from_fbw;
|
||||||
|
struct ap_state from_ap;
|
||||||
|
} payload;
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct link_mcu_msg link_mcu_from_ap_msg;
|
||||||
|
extern struct link_mcu_msg link_mcu_from_fbw_msg;
|
||||||
|
|
||||||
|
extern bool_t link_mcu_received;
|
||||||
|
|
||||||
|
extern void link_mcu_send( void );
|
||||||
|
extern void link_mcu_init( void );
|
||||||
|
extern void link_mcu_event_task( void );
|
||||||
|
extern void link_mcu_periodic_task( void );
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
Reference in New Issue
Block a user