From 6554b44ac7931f3574995782a6f46ccea7b7f680 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 14 Apr 2015 19:29:50 +0200 Subject: [PATCH] remove unused/unmaintained beth and fms code --- .../lisa_passthrough/ahrs_cmpl.makefile | 11 - .../booz_stabilization_int.makefile | 23 -- .../lisa_passthrough/imu_b2_v1.1.makefile | 77 ---- .../lisa_passthrough/imu_b2_v1.2.makefile | 81 ---- .../radio_control_spektrum.makefile | 22 -- .../subsystems/lisa_spi_link.makefile | 24 -- conf/messages.xml | 50 +-- conf/settings/beth_test_bench/beth.xml | 11 - conf/settings/beth_test_bench/beth_full.xml | 31 -- conf/settings/beth_test_bench/beth_pid.xml | 31 -- .../settings/beth_test_bench/beth_reduced.xml | 11 - conf/settings/beth_test_bench/beth_sfb.xml | 31 -- conf/settings/beth_test_bench/beth_twist.xml | 51 --- .../beth_test_bench/beth_twist_old.xml | 51 --- conf/settings/test_passthrough.xml | 16 - sw/airborne/boards/beth.h | 40 -- sw/airborne/firmwares/beth/README | 42 -- sw/airborne/firmwares/beth/bench_sensors.h | 43 -- .../firmwares/beth/bench_sensors_can.c | 58 --- .../firmwares/beth/bench_sensors_i2c.c | 38 -- sw/airborne/firmwares/beth/gps_hw.h | 4 - sw/airborne/firmwares/beth/main_beth.c | 60 --- sw/airborne/firmwares/beth/main_coders.c | 320 --------------- sw/airborne/firmwares/beth/main_overo.c | 312 --------------- sw/airborne/firmwares/beth/main_stm32.c | 221 ----------- sw/airborne/firmwares/beth/overo_controller.c | 145 ------- sw/airborne/firmwares/beth/overo_controller.h | 61 --- sw/airborne/firmwares/beth/overo_estimator.c | 70 ---- sw/airborne/firmwares/beth/overo_estimator.h | 28 -- .../firmwares/beth/overo_file_logger.c | 36 -- .../firmwares/beth/overo_file_logger.h | 17 - sw/airborne/firmwares/beth/overo_gcs_com.c | 110 ------ sw/airborne/firmwares/beth/overo_gcs_com.h | 29 -- .../firmwares/beth/overo_sfb_controller.c | 197 --------- .../firmwares/beth/overo_sfb_controller.h | 56 --- sw/airborne/firmwares/beth/overo_test_uart.c | 247 ------------ .../firmwares/beth/overo_twist_controller.c | 374 ------------------ .../firmwares/beth/overo_twist_controller.h | 82 ---- sw/airborne/firmwares/beth/rcv_telemetry.c | 101 ----- sw/airborne/firmwares/beth/sys_time_hw.h | 4 - sw/airborne/firmwares/beth/uart_hw.c | 277 ------------- sw/airborne/firmwares/beth/uart_hw.h | 96 ----- sw/airborne/fms/Makefile | 55 --- sw/airborne/fms/NOTES | 5 - sw/airborne/fms/fms_autopilot_msg.h | 202 ---------- sw/airborne/fms/fms_debug.h | 17 - sw/airborne/fms/fms_gs_com.c | 89 ----- sw/airborne/fms/fms_gs_com.h | 34 -- sw/airborne/fms/fms_network.c | 120 ------ sw/airborne/fms/fms_network.h | 25 -- sw/airborne/fms/fms_periodic.c | 110 ------ sw/airborne/fms/fms_periodic.h | 29 -- sw/airborne/fms/fms_spi_autopilot_msg.c | 197 --------- sw/airborne/fms/fms_spi_autopilot_msg.h | 39 -- sw/airborne/fms/fms_spi_link.c | 144 ------- sw/airborne/fms/fms_spi_link.h | 64 --- sw/airborne/fms/fms_spistream.h | 40 -- sw/airborne/fms/fms_spistream_client.c | 226 ----------- sw/airborne/fms/fms_spistream_daemon.c | 350 ---------------- sw/airborne/fms/fms_test_datalink.c | 116 ------ sw/airborne/fms/lpc_test_spi.c | 133 ------- sw/airborne/fms/onboard_logger.c | 144 ------- sw/airborne/fms/onboard_transport.c | 242 ------------ sw/airborne/fms/onboard_transport.h | 25 -- sw/airborne/fms/overo_blmc_calibrate.c | 89 ----- sw/airborne/fms/overo_blmc_calibrate.h | 14 - sw/airborne/fms/overo_test_gps_passthrough.c | 197 --------- sw/airborne/fms/overo_test_passthrough.c | 151 ------- sw/airborne/fms/overo_test_passthrough.h | 28 -- .../fms/overo_test_passthrough_telemetry.h | 31 -- sw/airborne/fms/overo_test_periodic.c | 101 ----- sw/airborne/fms/overo_test_spi_link.c | 175 -------- sw/airborne/fms/overo_test_telemetry.c | 69 ---- sw/airborne/fms/overo_test_telemetry2.c | 142 ------- sw/airborne/fms/packet_header.h | 39 -- sw/airborne/fms/test_telemetry.c | 13 - sw/airborne/fms/test_telemetry_2.c | 63 --- sw/airborne/fms/udp_transport.c | 16 - sw/airborne/fms/udp_transport.h | 194 --------- sw/airborne/fms/udp_transport2.c | 113 ------ sw/airborne/fms/udp_transport2.h | 103 ----- 81 files changed, 5 insertions(+), 7528 deletions(-) delete mode 100644 conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile delete mode 100644 conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile delete mode 100644 conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.1.makefile delete mode 100644 conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile delete mode 100644 conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile delete mode 100644 conf/firmwares/subsystems/lisa_spi_link.makefile delete mode 100644 conf/settings/beth_test_bench/beth.xml delete mode 100644 conf/settings/beth_test_bench/beth_full.xml delete mode 100644 conf/settings/beth_test_bench/beth_pid.xml delete mode 100644 conf/settings/beth_test_bench/beth_reduced.xml delete mode 100644 conf/settings/beth_test_bench/beth_sfb.xml delete mode 100644 conf/settings/beth_test_bench/beth_twist.xml delete mode 100644 conf/settings/beth_test_bench/beth_twist_old.xml delete mode 100644 conf/settings/test_passthrough.xml delete mode 100644 sw/airborne/boards/beth.h delete mode 100644 sw/airborne/firmwares/beth/README delete mode 100644 sw/airborne/firmwares/beth/bench_sensors.h delete mode 100644 sw/airborne/firmwares/beth/bench_sensors_can.c delete mode 100644 sw/airborne/firmwares/beth/bench_sensors_i2c.c delete mode 100644 sw/airborne/firmwares/beth/gps_hw.h delete mode 100644 sw/airborne/firmwares/beth/main_beth.c delete mode 100644 sw/airborne/firmwares/beth/main_coders.c delete mode 100644 sw/airborne/firmwares/beth/main_overo.c delete mode 100644 sw/airborne/firmwares/beth/main_stm32.c delete mode 100644 sw/airborne/firmwares/beth/overo_controller.c delete mode 100644 sw/airborne/firmwares/beth/overo_controller.h delete mode 100644 sw/airborne/firmwares/beth/overo_estimator.c delete mode 100644 sw/airborne/firmwares/beth/overo_estimator.h delete mode 100644 sw/airborne/firmwares/beth/overo_file_logger.c delete mode 100644 sw/airborne/firmwares/beth/overo_file_logger.h delete mode 100644 sw/airborne/firmwares/beth/overo_gcs_com.c delete mode 100644 sw/airborne/firmwares/beth/overo_gcs_com.h delete mode 100644 sw/airborne/firmwares/beth/overo_sfb_controller.c delete mode 100644 sw/airborne/firmwares/beth/overo_sfb_controller.h delete mode 100644 sw/airborne/firmwares/beth/overo_test_uart.c delete mode 100644 sw/airborne/firmwares/beth/overo_twist_controller.c delete mode 100644 sw/airborne/firmwares/beth/overo_twist_controller.h delete mode 100644 sw/airborne/firmwares/beth/rcv_telemetry.c delete mode 100644 sw/airborne/firmwares/beth/sys_time_hw.h delete mode 100644 sw/airborne/firmwares/beth/uart_hw.c delete mode 100644 sw/airborne/firmwares/beth/uart_hw.h delete mode 100644 sw/airborne/fms/Makefile delete mode 100644 sw/airborne/fms/NOTES delete mode 100644 sw/airborne/fms/fms_autopilot_msg.h delete mode 100644 sw/airborne/fms/fms_debug.h delete mode 100644 sw/airborne/fms/fms_gs_com.c delete mode 100644 sw/airborne/fms/fms_gs_com.h delete mode 100644 sw/airborne/fms/fms_network.c delete mode 100644 sw/airborne/fms/fms_network.h delete mode 100644 sw/airborne/fms/fms_periodic.c delete mode 100644 sw/airborne/fms/fms_periodic.h delete mode 100644 sw/airborne/fms/fms_spi_autopilot_msg.c delete mode 100644 sw/airborne/fms/fms_spi_autopilot_msg.h delete mode 100644 sw/airborne/fms/fms_spi_link.c delete mode 100644 sw/airborne/fms/fms_spi_link.h delete mode 100644 sw/airborne/fms/fms_spistream.h delete mode 100644 sw/airborne/fms/fms_spistream_client.c delete mode 100644 sw/airborne/fms/fms_spistream_daemon.c delete mode 100644 sw/airborne/fms/fms_test_datalink.c delete mode 100644 sw/airborne/fms/lpc_test_spi.c delete mode 100644 sw/airborne/fms/onboard_logger.c delete mode 100644 sw/airborne/fms/onboard_transport.c delete mode 100644 sw/airborne/fms/onboard_transport.h delete mode 100644 sw/airborne/fms/overo_blmc_calibrate.c delete mode 100644 sw/airborne/fms/overo_blmc_calibrate.h delete mode 100644 sw/airborne/fms/overo_test_gps_passthrough.c delete mode 100644 sw/airborne/fms/overo_test_passthrough.c delete mode 100644 sw/airborne/fms/overo_test_passthrough.h delete mode 100644 sw/airborne/fms/overo_test_passthrough_telemetry.h delete mode 100644 sw/airborne/fms/overo_test_periodic.c delete mode 100644 sw/airborne/fms/overo_test_spi_link.c delete mode 100644 sw/airborne/fms/overo_test_telemetry.c delete mode 100644 sw/airborne/fms/overo_test_telemetry2.c delete mode 100644 sw/airborne/fms/packet_header.h delete mode 100644 sw/airborne/fms/test_telemetry.c delete mode 100644 sw/airborne/fms/test_telemetry_2.c delete mode 100644 sw/airborne/fms/udp_transport.c delete mode 100644 sw/airborne/fms/udp_transport.h delete mode 100644 sw/airborne/fms/udp_transport2.c delete mode 100644 sw/airborne/fms/udp_transport2.h diff --git a/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile deleted file mode 100644 index a4c811ea073..00000000000 --- a/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile +++ /dev/null @@ -1,11 +0,0 @@ -# -# Complementary filter for attitude estimation -# - -stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL_EULER -ifneq ($(AHRS_ALIGNER_LED),none) - stm_passthrough.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c diff --git a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile deleted file mode 100644 index 9798cb93cb4..00000000000 --- a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile +++ /dev/null @@ -1,23 +0,0 @@ -stm_passthrough.srcs += $(SRC_FIRMWARE)/autopilot.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/actuators/supervision.c - -stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c - -stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c - -stm_passthrough.CFLAGS += -DUSE_NAVIGATION -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins.c -stm_passthrough.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/navigation.c -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -stm_passthrough.CFLAGS += -DUSE_VFF - -stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT -stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" -stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_REF_TYPE_H=\"stabilization/stabilization_attitude_ref_euler_int.h\" -stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c -stm_passthrough.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c diff --git a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.1.makefile b/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.1.makefile deleted file mode 100644 index e0067cae603..00000000000 --- a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.1.makefile +++ /dev/null @@ -1,77 +0,0 @@ -# -# Booz2 IMU booz2v1.1 -# -# -# required xml: -#
-# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -#
-# -# - -# imu Booz2 v1.1 - -imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -imu_CFLAGS += -DIMU_B2_VERSION_1_1 -imu_srcs += $(SRC_SUBSYSTEMS)/imu.c -imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c - -imu_srcs += peripherals/max1168.c -imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c - -imu_srcs += peripherals/ms2100.c -imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c - -ifeq ($(ARCH), lpc21) -imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=11 -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 -else ifeq ($(ARCH), stm32) -imu_CFLAGS += -DUSE_SPI2 -endif - -# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets -stm_passthrough.CFLAGS += $(imu_CFLAGS) -stm_passthrough.srcs += $(imu_srcs) - -# -# Simulator -# - -sim.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 -sim.CFLAGS += -DIMU_B2_VERSION_1_1 -sim.srcs += $(SRC_SUBSYSTEMS)/imu.c -sim.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c - -sim.srcs += peripherals/max1168.c -sim.srcs += $(SRC_ARCH)/peripherals/max1168_arch.c - -sim.CFLAGS += -DUSE_AMI601 -sim.srcs += peripherals/ami601.c -sim.CFLAGS += -DUSE_I2C1 diff --git a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile b/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile deleted file mode 100644 index 1531212233c..00000000000 --- a/conf/firmwares/subsystems/lisa_passthrough/imu_b2_v1.2.makefile +++ /dev/null @@ -1,81 +0,0 @@ -# -# Booz2 IMU booz2v1.2 -# -# -# required xml: -#
-# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -#
-# -# - -# imu Booz2 v1.2 - -imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -imu_CFLAGS += -DIMU_B2_VERSION_1_2 -imu_srcs += $(SRC_SUBSYSTEMS)/imu.c -imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c - -imu_srcs += peripherals/max1168.c -imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c - -imu_srcs += peripherals/hmc5843.c -imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c - -ifeq ($(ARCH), lpc21) -imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=11 -#FIXME ms2100 not used on this imu -imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12 -else ifeq ($(ARCH), stm32) -imu_CFLAGS += -DUSE_SPI2 -imu_CFLAGS += -DUSE_I2C2 -endif - -# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets -stm_passthrough.CFLAGS += $(imu_CFLAGS) -stm_passthrough.srcs += $(imu_srcs) - -# -# Simulator -# - -sim.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -#FIXME, should be HMC5843 -sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601 -#FIXME, should be verision 1.2 -sim.CFLAGS += -DIMU_B2_VERSION_1_1 -sim.srcs += $(SRC_SUBSYSTEMS)/imu.c -sim.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c - -sim.srcs += peripherals/max1168.c -sim.srcs += $(SRC_ARCH)/peripherals/max1168_arch.c - -sim.CFLAGS += -DUSE_AMI601 -sim.srcs += peripherals/ami601.c -sim.CFLAGS += -DUSE_I2C1 diff --git a/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile b/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile deleted file mode 100644 index 49c54b444cc..00000000000 --- a/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile +++ /dev/null @@ -1,22 +0,0 @@ -# -# Makefile for radio_control susbsytem in rotorcraft firmware -# - -RADIO_CONTROL_LED ?= none - -ifndef RADIO_CONTROL_SPEKTRUM_MODEL -RADIO_CONTROL_SPEKTRUM_MODEL=\"subsystems/radio_control/spektrum_dx7se.h\" -endif - -stm_passthrough.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind -stm_passthrough.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" -stm_passthrough.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL) -stm_passthrough.CFLAGS += -DRADIO_CONTROL_LINK=$(RADIO_CONTROL_LINK) -stm_passthrough.CFLAGS += -DUSE_$(RADIO_CONTROL_LINK) -D$(RADIO_CONTROL_LINK)_BAUD=B115200 -stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ - $(SRC_SUBSYSTEMS)/radio_control/spektrum.c \ - $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c - -ifneq ($(RADIO_CONTROL_LED,none)) -stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) -endif diff --git a/conf/firmwares/subsystems/lisa_spi_link.makefile b/conf/firmwares/subsystems/lisa_spi_link.makefile deleted file mode 100644 index e419584c1f8..00000000000 --- a/conf/firmwares/subsystems/lisa_spi_link.makefile +++ /dev/null @@ -1,24 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- -# -# Copyright (C) 2010 Antoine Drouin -# -# 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. - -ap.CFLAGS += -DUSE_SPI_LINK -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp -DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown -ap.srcs += $(SRC_FMS)/fms_spi_autopilot_msg.c -ap.srcs += $(SRC_FMS)/fms_spi_link.c diff --git a/conf/messages.xml b/conf/messages.xml index 4a1a36f9b4f..9c8bd6a155b 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -838,48 +838,10 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + @@ -901,9 +863,7 @@ - - - + diff --git a/conf/settings/beth_test_bench/beth.xml b/conf/settings/beth_test_bench/beth.xml deleted file mode 100644 index 39b60a30d23..00000000000 --- a/conf/settings/beth_test_bench/beth.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_full.xml b/conf/settings/beth_test_bench/beth_full.xml deleted file mode 100644 index 7d648b4f469..00000000000 --- a/conf/settings/beth_test_bench/beth_full.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_pid.xml b/conf/settings/beth_test_bench/beth_pid.xml deleted file mode 100644 index 14e9dfafcc5..00000000000 --- a/conf/settings/beth_test_bench/beth_pid.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_reduced.xml b/conf/settings/beth_test_bench/beth_reduced.xml deleted file mode 100644 index 95a62fef386..00000000000 --- a/conf/settings/beth_test_bench/beth_reduced.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_sfb.xml b/conf/settings/beth_test_bench/beth_sfb.xml deleted file mode 100644 index 2ff996e63f9..00000000000 --- a/conf/settings/beth_test_bench/beth_sfb.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_twist.xml b/conf/settings/beth_test_bench/beth_twist.xml deleted file mode 100644 index 7bccb41d558..00000000000 --- a/conf/settings/beth_test_bench/beth_twist.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/settings/beth_test_bench/beth_twist_old.xml b/conf/settings/beth_test_bench/beth_twist_old.xml deleted file mode 100644 index b74cc6c8cf6..00000000000 --- a/conf/settings/beth_test_bench/beth_twist_old.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/settings/test_passthrough.xml b/conf/settings/test_passthrough.xml deleted file mode 100644 index e0b47ec936b..00000000000 --- a/conf/settings/test_passthrough.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/sw/airborne/boards/beth.h b/sw/airborne/boards/beth.h deleted file mode 100644 index d6da44b71d3..00000000000 --- a/sw/airborne/boards/beth.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef CONFIG_BETH_H -#define CONFIG_BETH_H - -#define AHB_CLK 72000000 - -/* this board uses a crystal for HSE */ -//#define HSE_TYPE RCC_HSE_ON - -/* Onboard LEDs */ -#ifndef USE_LED_1 -#define USE_LED_1 1 -#endif -#define LED_1_GPIO GPIOC -#define LED_1_GPIO_PIN GPIO_Pin_12 - -#ifndef USE_LED_4 -#define USE_LED_4 1 -#endif -#define LED_4_GPIO GPIOA -#define LED_4_GPIO_PIN GPIO_Pin_6 - -#ifndef USE_LED_5 -#define USE_LED_5 1 -#endif -#define LED_5_GPIO GPIOA -#define LED_5_GPIO_PIN GPIO_Pin_7 - -#ifndef USE_LED_6 -#define USE_LED_6 1 -#endif -#define LED_6_GPIO GPIOB -#define LED_6_GPIO_PIN GPIO_Pin_0 - -#ifndef USE_LED_7 -#define USE_LED_7 1 -#endif -#define LED_7_GPIO GPIOB -#define LED_7_GPIO_PIN GPIO_Pin_1 - -#endif /* CONFIG_BETH_H */ diff --git a/sw/airborne/firmwares/beth/README b/sw/airborne/firmwares/beth/README deleted file mode 100644 index 53d6dea6687..00000000000 --- a/sw/airborne/firmwares/beth/README +++ /dev/null @@ -1,42 +0,0 @@ -Author : Paul Cox - -The Beth test bench is two things: - - * 1. A three degree of freedom mechanical test bench - * 2. A paparazzi build target that provides the code and configuration for the Lisa/Overo and Olimex STM32 boards mounted on item 1 - -More information available on the wiki page : -http://paparazzi.enac.fr/wiki/Beth_Test_Bench - -Source notes: - -/* -overo_test_uart -*/ - -overo_test_uart target opens a serial port and parses gps messages from ublox receiver - -* gps_c.patch is used to patch sw/airborne/gps.c so that the overo_test_uart target will compile -patch --dry-run -p0 sw/airborne/gps.c < sw/airborne/beth/gps_c.patch - -* currently tested on USB/serial adapters but should work just fine on the overo serial ports (assuming proper voltage level translation is in place) - -TODO: GPS_CONFIGURE currently not supported - -/* -main_overo -*/ - -This is the initial target for overo on beth that implements control stabilization using the potentiometers values from the beth boards (via CAN), a crista IMU, wifi for datalink and telemetry. - -/* -main_twisting -*/ - -Sliding mode control on the tilt axis, and keeping the rest of the main_overo target. - -/* -main_sfb -*/ - -State feedback control. Will be expanded later to use diff. plat. for trajectory control. diff --git a/sw/airborne/firmwares/beth/bench_sensors.h b/sw/airborne/firmwares/beth/bench_sensors.h deleted file mode 100644 index 99dd1786e79..00000000000 --- a/sw/airborne/firmwares/beth/bench_sensors.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef BENCH_SENSORS_H -#define BENCH_SENSORS_H - -#include "std.h" - -#if USE_I2C2 -#include "i2c.h" -#endif - -#ifdef USE_CAN1 -#include "mcu_periph/can.h" -extern uint16_t can_err_flags; -#endif - -extern void bench_sensors_init(void); -extern void read_bench_sensors(void); - -enum BenchSensorsStatus { BS_IDLE, BS_BUSY, BS_AVAILABLE}; - -struct BenchSensors { - enum BenchSensorsStatus status; - uint16_t angle_1; - uint16_t angle_2; - uint16_t angle_3; - uint16_t current; - bool_t ready; -}; - - -extern struct BenchSensors bench_sensors; - -#if USE_I2C2 -#define BenchSensorsEvent( _handler) { \ - if (bench_sensors.status == BS_BUSY && bench_sensors.ready) { \ - bench_sensors.angle_1 = i2c2.buf[0] + (i2c2.buf[1] << 8); \ - bench_sensors.angle_2 = i2c2.buf[2] + (i2c2.buf[3] << 8); \ - bench_sensors.status = BS_IDLE; \ - _handler(); \ - } \ - } -#endif - -#endif /* BENCH_SENSORS_H */ diff --git a/sw/airborne/firmwares/beth/bench_sensors_can.c b/sw/airborne/firmwares/beth/bench_sensors_can.c deleted file mode 100644 index aba54661dbc..00000000000 --- a/sw/airborne/firmwares/beth/bench_sensors_can.c +++ /dev/null @@ -1,58 +0,0 @@ -#include "bench_sensors.h" -#include "mcu_periph/can.h" -#include "led.h" - -//uint16_t halfw1,halfw2,halfw3,halfw4; -uint16_t can_err_flags = 0; - -struct BenchSensors bench_sensors; - -static void can_rx_callback(uint32_t id, uint8_t *buf, int len); -static uint8_t rx_bd1 = 0; -static uint8_t rx_bd2 = 0; - - -void bench_sensors_init(void) -{ - can_init(can_rx_callback); -} - -//for now Azimuth board(BD1) is slow so we give it more time... -#define MAX_ALLOWED_SKIPS_CANBD1 (60) -#define MAX_ALLOWED_SKIPS_CANBD2 (10) - -void read_bench_sensors(void) -{ - //rx_bd1/2 keep track of how long it's been since last receive (when it is set to 0) - //if we've lost link for 0.5 second, stop counting (to avoid overflowing our uint8) - if (rx_bd1 < 255) { rx_bd1++; } - if (rx_bd2 < 255) { rx_bd2++; } - - //if we haven't heard from a board for 15 periods (15/512=29ms) - //we flag a CAN error to show which board is at cause and how long it's been. - can_err_flags = 0; - if (rx_bd1 > MAX_ALLOWED_SKIPS_CANBD1) {can_err_flags = 1000 + rx_bd1;} - if (rx_bd2 > MAX_ALLOWED_SKIPS_CANBD2) {can_err_flags += (2000 + rx_bd2);} - //if ((rx_bd1 > 15) && (rx_bd2 > 15)) {can_err_flags = 3000 + rx_bd2 + rx_bd1;} -} - -static void can_rx_callback(uint32_t id, uint8_t *buf, int len) -{ - //LED_TOGGLE(7); - static uint16_t tempangle; - - bench_sensors.current = id >> 7; - if (bench_sensors.current == 2) { - tempangle = (buf[1] << 8) | buf[0]; - if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_2 = tempangle;} - tempangle = (buf[3] << 8) | buf[2]; - if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_3 = tempangle;} - rx_bd2 = 0; - LED_TOGGLE(4); - } else { - tempangle = (buf[3] << 8) | buf[2]; - if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x10;} else {bench_sensors.angle_1 = tempangle;} - rx_bd1 = 0; - //LED_TOGGLE(5); - } -} diff --git a/sw/airborne/firmwares/beth/bench_sensors_i2c.c b/sw/airborne/firmwares/beth/bench_sensors_i2c.c deleted file mode 100644 index eb41216a0fd..00000000000 --- a/sw/airborne/firmwares/beth/bench_sensors_i2c.c +++ /dev/null @@ -1,38 +0,0 @@ -#include "bench_sensors.h" - -struct BenchSensors bench_sensors, bench_sensors2; - - -void bench_sensors_init(void) -{ - bench_sensors.status = BS_IDLE; - bench_sensors.i2c_done = TRUE; -} - -void bench_sensors2_init(void) -{ - bench_sensors2.status = BS_IDLE; - bench_sensors2.i2c_done = TRUE; -} - - -void read_bench_sensors(void) -{ - - const uint8_t bench_addr = 0x40; - bench_sensors.status = BS_BUSY; - bench_sensors.i2c_done = FALSE; - i2c2_receive(bench_addr, 4, &bench_sensors.i2c_done); - -} - - -void read_bench_sensors2(void) -{ - - const uint8_t bench_addr2 = 0x30; - bench_sensors2.status = BS_BUSY; - bench_sensors2.i2c_done = FALSE; - i2c2_receive(bench_addr2, 4, &bench_sensors2.i2c_done); - -} diff --git a/sw/airborne/firmwares/beth/gps_hw.h b/sw/airborne/firmwares/beth/gps_hw.h deleted file mode 100644 index edf4f713fc8..00000000000 --- a/sw/airborne/firmwares/beth/gps_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/firmwares/beth/main_beth.c b/sw/airborne/firmwares/beth/main_beth.c deleted file mode 100644 index 1ca959b4854..00000000000 --- a/sw/airborne/firmwares/beth/main_beth.c +++ /dev/null @@ -1,60 +0,0 @@ -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" - -#include "subsystems/datalink/downlink.h" - -#include "beth/bench_sensors.h" - -static inline void main_init(void); -static inline void main_periodic_task(void); -static inline void main_event_task(void); - -static inline void main_on_bench_sensors(void); - - -int main(void) -{ - main_init(); - while (1) { - if (sys_time_check_and_ack_timer(0)) { - main_periodic_task(); - } - } - return 0; -} - -static inline void main_init(void) -{ - mcu_init(); - sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - mcu_int_enable(); -} - -static inline void main_periodic_task(void) -{ - - RunOnceEvery(512, { DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); - - read_bench_sensors(); - -} - - -static inline void main_event_task(void) -{ - - BenchSensorsEvent(main_on_bench_sensors); - -} - - -static inline void main_on_bench_sensors(void) -{ - - DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &bench_sensors_angle_1, - &bench_sensors_angle_2); - -} - - diff --git a/sw/airborne/firmwares/beth/main_coders.c b/sw/airborne/firmwares/beth/main_coders.c deleted file mode 100644 index 7d534137bfd..00000000000 --- a/sw/airborne/firmwares/beth/main_coders.c +++ /dev/null @@ -1,320 +0,0 @@ -#include BOARD_CONFIG -#include "mcu.h" -#include "mcu_periph/can.h" -#include "mcu_periph/sys_time.h" -#include "subsystems/datalink/downlink.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "firmwares/beth/bench_sensors.h" - -/* - * - * PC.01 (ADC Channel11) ext1-20 coder_values[1] -Paul : using channel 10 instead of 14 - * PC.04 (ADC Channel14) ext2-12 coder_values[0] - * - * PB.10 I2C2 SCL ext2-14 - * PB.11 I2C2 SDA ext2-15 - * - */ - - -static inline void main_init(void); -static inline void main_periodic(void); -static inline void main_event(void); - -static inline void main_init_adc(void); -//static inline void main_on_bench_sensors( void ); - -//static inline void main_init_i2c2(void); -//void i2c2_ev_irq_handler(void); -//void i2c2_er_irq_handler(void); - -#define ADC1_DR_Address ((uint32_t)0x4001244C) -static uint16_t coder_values[3]; - -//azimuth potentiometer board address is 0x30 -//#define I2C2_SLAVE_ADDRESS7 0x30 -//elevation and tilt pot board is 0x40 -#define I2C2_SLAVE_ADDRESS7 0x40 -#define I2C2_ClockSpeed 200000 - -#define MY_I2C2_BUF_LEN 4 -//static uint8_t i2c2_idx; -//static uint8_t i2c2_buf[MY_I2C2_BUF_LEN]; - -uint16_t servos[4]; - -int main(void) -{ - main_init(); - - servos[0] = 1; - servos[1] = 2; - servos[2] = 3; - servos[3] = 4; - - while (1) { - if (sys_time_check_and_ack_timer(0)) { - main_periodic(); - } - main_event(); - } - return 0; -} - - -static inline void main_init(void) -{ - mcu_init(); - sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - main_init_adc(); - bench_sensors_init(); - mcu_int_enable(); -} - -static inline void main_periodic(void) -{ - - /*RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});*/ - - //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], &coder_values[1]);}); - //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, &can1_pending);}); - - /*RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, - &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});*/ - - servos[0] = coder_values[0]; - servos[1] = coder_values[1]; - //use id=1 for azimuth board - can_transmit(1, (uint8_t *)servos, 8); - LED_TOGGLE(5); -} - - -static inline void main_event(void) -{ - //BenchSensorsEvent(main_on_bench_sensors); -} - - -/*static inline void main_on_bench_sensors( void ) { - -}*/ - - -#if 0 -/* - * - * I2C2 : autopilot link - * - */ -void i2c2_init(void) -{ -// static inline void main_init_i2c2(void) { - - /* System clocks configuration ---------------------------------------------*/ - /* Enable I2C2 clock */ - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); - /* Enable GPIOB clock */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - - /* NVIC configuration ------------------------------------------------------*/ - NVIC_InitTypeDef NVIC_InitStructure; - NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); - /* Configure and enable I2C2 event interrupt -------------------------------*/ - NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - /* Configure and enable I2C2 error interrupt -------------------------------*/ - NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2; - NVIC_Init(&NVIC_InitStructure); - - - /* GPIO configuration ------------------------------------------------------*/ - GPIO_InitTypeDef GPIO_InitStructure; - /* Configure I2C2 pins: SCL and SDA ----------------------------------------*/ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; - GPIO_Init(GPIOB, &GPIO_InitStructure); - - /* Enable I2C2 -------------------------------------------------------------*/ - I2C_Cmd(I2C2, ENABLE); - /* I2C2 configuration ------------------------------------------------------*/ - I2C_InitTypeDef I2C_InitStructure; - I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; - I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; - I2C_InitStructure.I2C_OwnAddress1 = I2C2_SLAVE_ADDRESS7; - I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; - I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_InitStructure.I2C_ClockSpeed = I2C2_ClockSpeed; - I2C_Init(I2C2, &I2C_InitStructure); - - /* Enable I2C1 event and buffer interrupts */ - // I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_BUF, ENABLE); - I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_ERR, ENABLE); - -} - - -void i2c2_ev_irq_handler(void) -{ - switch (I2C_GetLastEvent(I2C2)) { - /* Slave Transmitter ---------------------------------------------------*/ - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: /* EV1 */ - memcpy(i2c2_buf, coder_values, MY_I2C2_BUF_LEN); - i2c2_idx = 0; - - case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: /* EV3 */ - /* Transmit I2C2 data */ - if (i2c2_idx < MY_I2C2_BUF_LEN) { - I2C_SendData(I2C2, i2c2_buf[i2c2_idx]); - i2c2_idx++; - } - break; - - - case I2C_EVENT_SLAVE_STOP_DETECTED: /* EV4 */ - LED_ON(1); - /* Clear I2C2 STOPF flag: read of I2C_SR1 followed by a write on I2C_CR1 */ - (void)(I2C_GetITStatus(I2C2, I2C_IT_STOPF)); - I2C_Cmd(I2C2, ENABLE); - break; - - } -} - - -void i2c2_er_irq_handler(void) -{ - /* Check on I2C2 AF flag and clear it */ - if (I2C_GetITStatus(I2C2, I2C_IT_AF)) { - I2C_ClearITPendingBit(I2C2, I2C_IT_AF); - } -} - - -#endif - -/* - * - * ADC : coders - * - */ - - - - -static inline void main_init_adc(void) -{ - - /* Enable DMA1 clock */ - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - - /* Enable ADC1 and GPIOC clock */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | - RCC_APB2Periph_GPIOC, ENABLE); - - /* Configure PC.01 (ADC Channel11) and PC.04 (ADC Channel14) as analog input-*/ - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - GPIO_Init(GPIOC, &GPIO_InitStructure); - - /* DMA1 channel1 configuration ----------------------------------------------*/ - DMA_InitTypeDef DMA_InitStructure; - DMA_DeInit(DMA1_Channel1); - DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address; - DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&coder_values; - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; - DMA_InitStructure.DMA_BufferSize = 1; - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_High; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel1, &DMA_InitStructure); - - /* Enable DMA1 channel1 */ - DMA_Cmd(DMA1_Channel1, ENABLE); - - /* ADC1 configuration ------------------------------------------------------*/ - ADC_InitTypeDef ADC_InitStructure; - ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfChannel = 1; - ADC_Init(ADC1, &ADC_InitStructure); - - /* ADC1 regular channel14 configuration */ - //ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 1, ADC_SampleTime_239Cycles5); - //Paul: Changing to use chan 10 instead - ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_239Cycles5); - - /* Enable ADC1 DMA */ - ADC_DMACmd(ADC1, ENABLE); - - - /* ADC2 configuration ------------------------------------------------------*/ - ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult; - ADC_InitStructure.ADC_ScanConvMode = ENABLE; - ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; - ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; - ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; - ADC_InitStructure.ADC_NbrOfChannel = 1; - ADC_Init(ADC2, &ADC_InitStructure); - /* ADC2 regular channels configuration */ - ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_239Cycles5); - /* Enable ADC2 external trigger conversion */ - ADC_ExternalTrigConvCmd(ADC2, ENABLE); - - - /* Enable ADC1 */ - ADC_Cmd(ADC1, ENABLE); - - /* Enable ADC1 reset calibaration register */ - ADC_ResetCalibration(ADC1); - /* Check the end of ADC1 reset calibration register */ - while (ADC_GetResetCalibrationStatus(ADC1)); - - /* Start ADC1 calibaration */ - ADC_StartCalibration(ADC1); - /* Check the end of ADC1 calibration */ - while (ADC_GetCalibrationStatus(ADC1)); - - /* Enable ADC2 */ - ADC_Cmd(ADC2, ENABLE); - - /* Enable ADC2 reset calibaration register */ - ADC_ResetCalibration(ADC2); - /* Check the end of ADC2 reset calibration register */ - while (ADC_GetResetCalibrationStatus(ADC2)); - - /* Start ADC2 calibaration */ - ADC_StartCalibration(ADC2); - /* Check the end of ADC2 calibration */ - while (ADC_GetCalibrationStatus(ADC2)); - - - /* Start ADC1 Software Conversion */ - ADC_SoftwareStartConvCmd(ADC1, ENABLE); - -} - diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c deleted file mode 100644 index 256af99c595..00000000000 --- a/sw/airborne/firmwares/beth/main_overo.c +++ /dev/null @@ -1,312 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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 -#include -#include -#include -#include -#include - -#include - -#include "messages2.h" -#include "generated/airframe.h" - -#include "fms_periodic.h" -#include "fms_debug.h" -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" -#include "subsystems/imu.h" - -#include "overo_file_logger.h" -#include "overo_gcs_com.h" -#include "overo_estimator.h" -#include CONTROLLER_H - - -static void main_periodic(int); -//static void main_parse_cmd_line(int argc, char *argv[]); -static void drive_output(void); -static void main_exit(int sig); -static void main_talk_with_stm32(void); - - -static struct AutopilotMessageCRCFrame msg_in; -static struct AutopilotMessageCRCFrame msg_out; - -struct Imu imu; -struct ImuFloat imu_float; - - -static uint32_t foo = 0; -static uint8_t spi_crc_ok = 1; -static uint8_t last_state = 1; - -int main(int argc, char *argv[]) -{ - - (void) signal(SIGINT, main_exit); - - //set IMU neutrals - RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); - VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); - VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); - - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return -1; - } - - /* Initalize the event library */ - event_init(); - - control_init(); - estimator_init(); - - // file_logger_init("my_log.data"); - - gcs_com_init(); - - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return -1; - } - - //main_parse_cmd_line(argc, argv); - - event_dispatch(); - //should never occur! - printf("goodbye! (%d)\n", foo); - - return 0; -} - -#define PITCH_MAGIC_NUMBER (121) - -static void main_periodic(int my_sig_num) -{ - - /* static int bar=0; - if (!(foo%2000)) { - if (bar) { - controller.tilt_sp = -0.4; bar=0; - }else{ - controller.tilt_sp = 0.4; bar=1; - } - } - */ - //if (foo >2000 ) { controller.armed=1;} - - RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); - - main_talk_with_stm32(); - - imu_scale_gyro(&imu); - - RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport, - &msg_in.payload.msg_up.bench_sensor.x, &msg_in.payload.msg_up.bench_sensor.y, - &msg_in.payload.msg_up.bench_sensor.z, &msg_in.payload.msg_up.cnt, - &msg_in.payload.msg_up.can_errs, &msg_in.payload.msg_up.spi_errs, - &msg_in.payload.msg_up.thrust_out, &msg_in.payload.msg_up.pitch_out); - }); - - estimator_run(msg_in.payload.msg_up.bench_sensor.z, msg_in.payload.msg_up.bench_sensor.y, - msg_in.payload.msg_up.bench_sensor.x); - - if (msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is lost! %d %d\n", - msg_in.payload.msg_up.cnt, msg_in.payload.msg_up.can_errs); - if (msg_in.payload.msg_up.cnt == 1) printf("STM indicates overo link is regained. %d %d\n", - msg_in.payload.msg_up.cnt, msg_in.payload.msg_up.can_errs); - - //If the stm32 cut the motors due to an error, we force the state machine into spinup mode. - //when the stm32 resumes after the error, the system will need to be rearmed by the user. - if ((controller.armed != 0) && (msg_in.payload.msg_up.pitch_out == PITCH_MAGIC_NUMBER)) { - controller.armed = 0; last_state = 1; - printf("STM cut motor power. %d %d\n", - msg_in.payload.msg_up.cnt, msg_in.payload.msg_up.can_errs); - } - - - drive_output(); - - control_send_messages(); - - estimator_send_messages(); - - //file_logger_periodic(); - - /* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport, - //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) - &imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);}); - RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, - //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z - &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) - RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, - //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) - &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); - - RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, - &estimator.tilt, &estimator.elevation, &estimator.azimuth );}); - RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, - //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z - &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ - - RunOnceEvery(33, gcs_com_periodic()); - -} - -#if 0 -static void main_parse_cmd_line(int argc, char *argv[]) -{ - - if (argc > 1) { - controller.kp = atof(argv[1]); - // printf("kp set to %f\n",kp); - if (argc > 2) { - controller.kd = atof(argv[2]); - // printf("kd set to %f\n",kd); - } else { - controller.kd = 1.0; - // printf("using default value of kd %f\n",kd); - } - } else { - controller.kp = 0.05; - // printf("using default value of kp %f\n",kp); - } - /* - if (argc>1){ - printf("args not currently supported\n"); - }*/ -} -#endif - -static void drive_output() -{ - switch (controller.armed) { - case 0: - if (last_state == 2) { - controller.armed = 2; - printf("Not allowed. Enter standby first.\n"); - //controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10); - control_run(); - } else { - if (last_state == 1) { - printf("Entering spinup mode.\n"); - } - controller.cmd_pitch = 1; - controller.cmd_thrust = 1; - last_state = 0; - } - break; - case 1: - if (last_state != 1) { - printf("Entering standby mode.\n"); - last_state = 1; - if (last_state == 0) { - controller.elevation_ref = estimator.elevation; - controller.elevation_dot_ref = estimator.elevation_dot; - } - } - controller.elevation_sp = RadOfDeg(-28); - controller.tilt_sp = 0; - control_run(); - break; - case 2: - if (last_state == 0) { - printf("Not allowed. Enter standby first.\n"); - controller.armed = 0; - } else { - if (last_state == 1) { - printf("Entering flight mode.\n"); - } - //controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10); - control_run(); - last_state = 2; - } - break; - default: - break; - } -} - -static void main_exit(int sig) -{ - printf("Initiating BETH shutdown...\n"); - -//since the periodic event is no longer running when we get here right now, -//this code doesn't do anything so removed for now. -#if 0 - printf("Zeroing setpoints..."); - uint32_t startfoo = foo; - while (foo < (startfoo + 2000)) { - controller.tilt_sp = 0; - controller.elevation_sp = RadOfDeg(-25); - } - printf("done\n"); -#endif - - //If a logfile is being used, close it. - //file_logger_exit() - printf("Main Overo Application Exiting...\n"); - exit(EXIT_SUCCESS); -} - - - -static void main_talk_with_stm32() -{ - - //static int8_t adder = 1; - //uint8_t *fooptr; - - //msg_out.payload.msg_down.thrust = 0; - msg_out.payload.msg_down.thrust = (int8_t)controller.cmd_thrust; - - //TODO: change motor config to remove minus here. - msg_out.payload.msg_down.pitch = -(int8_t)controller.cmd_pitch; - //msg_out.payload.msg_down.pitch = 0; - - - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame) , &msg_in, &spi_crc_ok); - - /* for debugging overo/stm spi link - if (msg_in.payload.msg_up.bench_sensor.x == 0){ - fooptr = &msg_in; - for (int i=0; i - * - * 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 BOARD_CONFIG -#include "std.h" -#include "mcu.h" -#include "mcu_periph/can.h" -#include "mcu_periph/sys_time.h" -#include "subsystems/datalink/downlink.h" -#include "subsystems/commands.h" -#include "firmwares/rotorcraft/actuators.h" -//#include "booz/booz_radio_control.h" -#include "subsystems/imu.h" -#include "lisa/lisa_overo_link.h" -#include "bench_sensors.h" - -static inline void main_init(void); -static inline void main_periodic(void); -static inline void main_event(void); - -static inline void on_gyro_accel_event(void); -static inline void on_accel_event(void); -static inline void on_mag_event(void); - -static inline void main_on_overo_msg_received(void); -static inline void main_on_overo_link_lost(void); -static inline void main_on_overo_link_error(void); - -static uint32_t spi_msg_cnt = 0; -static uint16_t spi_errors = 0; - -int main(void) -{ - main_init(); - - while (1) { - if (sys_time_check_and_ack_timer(0)) { - main_periodic(); - } - main_event(); - } - return 0; -} - -static inline void main_init(void) -{ - mcu_init(); - sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - actuators_init(); - //radio_control_init(); - imu_init(); - overo_link_init(); - bench_sensors_init(); - commands[COMMAND_ROLL] = 0; - commands[COMMAND_YAW] = 0; -} - -#define PITCH_MAGIC_NUMBER (121) - -static inline void main_periodic(void) -{ - int8_t pitch_out, thrust_out; - imu_periodic(); - - OveroLinkPeriodic(main_on_overo_link_lost) - - RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); - - //RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, - // &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);}); - - //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &overo_link.down.msg.foo,&overo_link.down.msg.bar);}); - - read_bench_sensors(); - - pitch_out = (int8_t)((0xFF) & overo_link.down.msg.pitch); - thrust_out = (int8_t)((0xFF) & overo_link.down.msg.thrust); - - Bound(pitch_out, -80, 80); - Bound(thrust_out, 0, 100); - - overo_link.up.msg.thrust_out = thrust_out; - overo_link.up.msg.pitch_out = pitch_out; - - //stop the motors if we've lost SPI or CAN link - //If SPI has had CRC error we don't stop motors - if ((spi_msg_cnt == 0) || (can_err_flags != 0)) { - commands[COMMAND_PITCH] = 0; - commands[COMMAND_THRUST] = 0; - actuators_set(FALSE); - overo_link.up.msg.can_errs = can_err_flags; - overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER; - } else { - commands[COMMAND_PITCH] = pitch_out; - commands[COMMAND_THRUST] = thrust_out; - actuators_set(TRUE); - } -} - -static inline void main_event(void) -{ - ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); - OveroLinkEvent(main_on_overo_msg_received, main_on_overo_link_error); -} - -static inline void main_on_overo_msg_received(void) -{ - - overo_link.up.msg.bench_sensor.x = bench_sensors.angle_1; - overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2; - overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3; - - overo_link.up.msg.accel.x = imu.accel_unscaled.x; - overo_link.up.msg.accel.y = imu.accel_unscaled.y; - overo_link.up.msg.accel.z = imu.accel_unscaled.z; - - overo_link.up.msg.gyro.p = imu.gyro_unscaled.p; - overo_link.up.msg.gyro.q = imu.gyro_unscaled.q; - overo_link.up.msg.gyro.r = imu.gyro_unscaled.r; - - //can_err_flags (uint16) represents the board number that is not communicating regularly - //spi_errors (uint16) reflects the number of crc errors on the spi link - //TODO: if >10% of messages are coming in with crc errors, assume something is really wrong - //and disable the motors. - overo_link.up.msg.can_errs = can_err_flags; - overo_link.up.msg.spi_errs = spi_errors; - - //spi_msg_cnt shows number of spi transfers since last link lost event - overo_link.up.msg.cnt = spi_msg_cnt++; - -} - -static inline void main_on_overo_link_lost(void) -{ - //actuators_set(FALSE); - spi_msg_cnt = 0; -} - - -static inline void on_accel_event(void) -{ - -} - -static inline void on_gyro_accel_event(void) -{ - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - - //LED_TOGGLE(2); - static uint8_t cnt; - cnt++; - if (cnt > 15) { cnt = 0; } - - if (cnt == 0) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - } else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); - - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); - } -} - - -static inline void on_mag_event(void) -{ - imu_scale_mag(&imu); - static uint8_t cnt; - cnt++; - if (cnt > 1) { cnt = 0; } - - if (cnt % 2) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - } else { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); - } -} - -static inline void main_on_overo_link_error(void) -{ - spi_errors++; -} diff --git a/sw/airborne/firmwares/beth/overo_controller.c b/sw/airborne/firmwares/beth/overo_controller.c deleted file mode 100644 index f07c89ae4c1..00000000000 --- a/sw/airborne/firmwares/beth/overo_controller.c +++ /dev/null @@ -1,145 +0,0 @@ -#include "overo_controller.h" - -#include "overo_estimator.h" -#include "std.h" -#include "stdio.h" - -#include "messages2.h" -#include "overo_gcs_com.h" - -struct OveroController controller; - -void control_init(void) -{ -// controller.kp = 0.05; -// controller.kd = 0.01; - - controller.tilt_sp = 0.; - controller.elevation_sp = RadOfDeg(10); - controller.azimuth_sp = 0.; - - controller.omega_tilt_ref = RadOfDeg(600); - controller.omega_elevation_ref = RadOfDeg(120); - controller.omega_azimuth_ref = RadOfDeg(60); - controller.xi_ref = 1.; - - controller.tilt_ref = estimator.tilt; - controller.tilt_dot_ref = estimator.tilt_dot; - controller.tilt_ddot_ref = 0.; - - //controller.elevation_ref = estimator.elevation; - controller.elevation_ref = RadOfDeg(-28); - controller.elevation_dot_ref = estimator.elevation_dot; - controller.elevation_ddot_ref = 0.; - - controller.azimuth_ref = estimator.azimuth; - controller.azimuth_dot_ref = 0.; - controller.azimuth_ddot_ref = 0.; - - controller.one_over_J = 2.; - controller.mass = 5.; - controller.azim_gain = 0.005; - - controller.omega_cl = RadOfDeg(600); - controller.xi_cl = 1.; - - controller.cmd_pitch_ff = 0.; - controller.cmd_pitch_fb = 0.; - - controller.cmd_thrust_ff = 0.; - controller.cmd_thrust_fb = 0.; - - controller.cmd_pitch = 0.; - controller.cmd_thrust = 0.; - - controller.armed = 0; -} - -void control_send_messages(void) -{ - - RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport, - &controller.cmd_pitch, &controller.cmd_thrust, - &controller.cmd_pitch_ff, &controller.cmd_pitch_fb, - &controller.cmd_thrust_ff, &controller.cmd_thrust_fb, - &controller.tilt_sp, &controller.tilt_ref, &controller.tilt_dot_ref, - &controller.elevation_sp, &controller.elevation_ref, &controller.elevation_dot_ref, - &controller.azimuth_sp); - }); -} - -void control_run(void) -{ - - /* - * propagate reference - */ - const float dt_ctl = 1. / 512.; - const float thrust_constant = 40.; - - controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl; - controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref * dt_ctl; - controller.tilt_ddot_ref = -2 * controller.omega_tilt_ref * controller.xi_ref * controller.tilt_dot_ref - - controller.omega_tilt_ref * controller.omega_tilt_ref * (controller.tilt_ref - controller.tilt_sp); - - controller.elevation_ref = controller.elevation_ref + controller.elevation_dot_ref * dt_ctl; - controller.elevation_dot_ref = controller.elevation_dot_ref + controller.elevation_ddot_ref * dt_ctl; - controller.elevation_ddot_ref = -2 * controller.omega_elevation_ref * controller.xi_ref * controller.elevation_dot_ref - - controller.omega_elevation_ref * controller.omega_elevation_ref * (controller.elevation_ref - - controller.elevation_sp); - - controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl; - controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl; - controller.azimuth_ddot_ref = -2 * controller.omega_azimuth_ref * controller.xi_ref * controller.azimuth_dot_ref - - controller.omega_azimuth_ref * controller.omega_azimuth_ref * (controller.azimuth_ref - controller.azimuth_sp); - - static int foo = 0; - - /* - * calculate errors - */ - - const float err_tilt = estimator.tilt - controller.tilt_ref; - const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref; - - const float err_elevation = estimator.elevation - controller.elevation_ref; - const float err_elevation_dot = estimator.elevation_dot - controller.elevation_dot_ref; - - const float err_azimuth = estimator.azimuth - controller.azimuth_ref; - const float err_azimuth_dot = estimator.azimuth_dot - controller.azimuth_dot_ref; - - /* - * Compute feedforward and feedback commands - */ - - controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref; - controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt); - - controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref; - controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) - - controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation); - - controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref; - controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); - - controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;// + - controller.tilt_sp = - controller.azim_gain * (-controller.cmd_azimuth_fb); //+ controller.cmd_azimuth_ff); - controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant; - controller.cmd_thrust = controller.cmd_thrust * (1 / cos(estimator.elevation)); - - //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0; - Bound(controller.cmd_thrust, 0, 100); - Bound(controller.cmd_pitch, -100, 100); - - if (!(foo % 100)) { - //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot); - //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot); - //printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref); - } - foo++; - -} - diff --git a/sw/airborne/firmwares/beth/overo_controller.h b/sw/airborne/firmwares/beth/overo_controller.h deleted file mode 100644 index 2aae1ffd775..00000000000 --- a/sw/airborne/firmwares/beth/overo_controller.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef OVERO_CONTROLLER_H -#define OVERO_CONTROLLER_H - -struct OveroController { -// float kp; -// float kd; - - float tilt_sp; - float elevation_sp; - float azimuth_sp; - - /* modele de reference */ - float tilt_ref; - float tilt_dot_ref; - float tilt_ddot_ref; - - float elevation_ref; - float elevation_dot_ref; - float elevation_ddot_ref; - - float azimuth_ref; - float azimuth_dot_ref; - float azimuth_ddot_ref; - - float omega_tilt_ref; - float omega_elevation_ref; - float omega_azimuth_ref; - float xi_ref; - - /* invert control law parameter */ - float one_over_J; - float mass; - - /* closed loop parameters */ - float omega_cl; - float xi_cl; - float azim_gain; - - float cmd_pitch_ff; - float cmd_pitch_fb; - - float cmd_thrust_ff; - float cmd_thrust_fb; - - float cmd_azimuth_ff; - float cmd_azimuth_fb; - - float cmd_pitch; - float cmd_thrust; - - int armed; -}; - - -extern struct OveroController controller; - -extern void control_init(void); -extern void control_send_messages(void); -extern void control_run(void); - -#endif /* OVERO_CONTROLLER_H */ diff --git a/sw/airborne/firmwares/beth/overo_estimator.c b/sw/airborne/firmwares/beth/overo_estimator.c deleted file mode 100644 index 1ea449029a9..00000000000 --- a/sw/airborne/firmwares/beth/overo_estimator.c +++ /dev/null @@ -1,70 +0,0 @@ -#include "overo_estimator.h" - -#include "subsystems/imu.h" -#include - -#include "messages2.h" -#include "overo_gcs_com.h" - -struct OveroEstimator estimator; - -void estimator_init(void) -{ - estimator.tilt_lp_coeff = 0.5; - estimator.elevation_lp_coeff = 0.5; - estimator.azimuth_lp_coeff = 0.5; -} - -void estimator_send_messages(void) -{ - - RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport, - &estimator.tilt, &estimator.tilt_dot, - &estimator.elevation, &estimator.elevation_dot, - &estimator.azimuth, &estimator.azimuth_dot); - }); -} - -//bench sensors z,y,x values passed in -void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) -{ - - const int32_t tilt_neutral = 1970; - const float tilt_scale = 1. / 580.; - const int32_t azimuth_neutral = 2875; - const float azimuth_scale = 1. / 580.; - const int32_t elevation_neutral = 670; - const float elevation_scale = 1. / 580.; - - - estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure) * tilt_scale; - Bound(estimator.tilt, -89, 89); - //low pass filter tilt gyro - estimator.tilt_dot = estimator.tilt_dot + - estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.q) - estimator.tilt_dot); - /* Second order filter yet to be tested - estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) + - estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(imu.gyro.q) - - estimator.tilt_dot_old * (1 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 + - estimator.tilt_lp_coeff1*estimator.tilt_lp_coeff2); - */ - - estimator.elevation = (elevation_neutral - (int32_t)elevation_measure) * elevation_scale; - Bound(estimator.elevation, -45, 45); - - //rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes - float rotated_elev_dot = RATE_FLOAT_OF_BFP(imu.gyro.p) * cos(estimator.tilt) + - RATE_FLOAT_OF_BFP(imu.gyro.r) * sin(estimator.tilt); - //low pass filter -- should probably increase order and maybe move filtering to measured values. - estimator.elevation_dot = estimator.elevation_dot + - estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot); - - estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure) * azimuth_scale; - - //low pass filter azimuth gyro - //TODO: compensate rotation and increase order - estimator.azimuth_dot = estimator.azimuth_dot + - estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.r) - estimator.azimuth_dot); - -} - diff --git a/sw/airborne/firmwares/beth/overo_estimator.h b/sw/airborne/firmwares/beth/overo_estimator.h deleted file mode 100644 index dd937631798..00000000000 --- a/sw/airborne/firmwares/beth/overo_estimator.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef OVERO_ESTIMATOR_H -#define OVERO_ESTIMATOR_H - -#include - -struct OveroEstimator { - - float azimuth; - float elevation; - float tilt; - - float azimuth_dot; - float elevation_dot; - float tilt_dot; - - float tilt_lp_coeff; - float elevation_lp_coeff; - float azimuth_lp_coeff; -}; - - -extern struct OveroEstimator estimator; - -extern void estimator_init(void); -extern void estimator_send_messages(void); -extern void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure); - -#endif /* OVERO_CONTROLLER_H */ diff --git a/sw/airborne/firmwares/beth/overo_file_logger.c b/sw/airborne/firmwares/beth/overo_file_logger.c deleted file mode 100644 index be1ece3a104..00000000000 --- a/sw/airborne/firmwares/beth/overo_file_logger.c +++ /dev/null @@ -1,36 +0,0 @@ -#include "overo_file_logger.h" - -#include "subsystems/imu.h" - -struct FileLogger file_logger; - -void file_logger_init(char *filename) -{ - - file_logger.outfile = fopen(filename, "w+"); - -} - - - -void file_logger_periodic(void) -{ - static uint32_t foo = 0; - foo++; - fprintf(file_logger.outfile, "%f %d IMU_ACCEL_RAW %d %d %d\n", foo / 512., 42, imu.accel_unscaled.x, - imu.accel_unscaled.y, imu.accel_unscaled.z); - fprintf(file_logger.outfile, "%f %d IMU_GYRO_RAW %d %d %d\n", foo / 512., 42, imu.gyro_unscaled.p, imu.gyro_unscaled.q, - imu.gyro_unscaled.r); - -} - -void file_logger_exit(void) -{ - - fclose(file_logger.outfile); - -} - - - - diff --git a/sw/airborne/firmwares/beth/overo_file_logger.h b/sw/airborne/firmwares/beth/overo_file_logger.h deleted file mode 100644 index 11425c8ce9f..00000000000 --- a/sw/airborne/firmwares/beth/overo_file_logger.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef OVERO_FILE_LOGGER_H -#define OVERO_FILE_LOGGER_H - -#include - -struct FileLogger { - FILE *outfile; -}; - -extern struct FileLogger file_logger; - -extern void file_logger_init(char *filename); -extern void file_logger_periodic(void); -extern void file_logger_exit(void); - -#endif /* OVERO_FILE_LOGGER_H */ - diff --git a/sw/airborne/firmwares/beth/overo_gcs_com.c b/sw/airborne/firmwares/beth/overo_gcs_com.c deleted file mode 100644 index 2427f1b609b..00000000000 --- a/sw/airborne/firmwares/beth/overo_gcs_com.c +++ /dev/null @@ -1,110 +0,0 @@ -#include "overo_gcs_com.h" - -#include -#include - -#include "downlink_transport.h" -#include "messages2.h" -#include "udp_transport2.h" -#include "dl_protocol.h" -#include "generated/settings.h" - -//bill laptop -//#define GCS_HOST "10.31.4.5" -//#define GCS_HOST "192.168.2.10" -//#define GCS_HOST "192.168.2.1" -//auto4 -//#define GCS_HOST "10.31.4.104" -#define GCS_HOST "10.31.4.107" -#define GCS_PORT 4242 -#define DATALINK_PORT 4243 - -static void dl_handle_msg(struct DownlinkTransport *tp); -static inline int checked_read(int fd, char *buf, size_t len); -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg); - -struct OveroGcsCom gcs_com; - -void gcs_com_init(void) -{ - - gcs_com.network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); - gcs_com.udp_transport = udp_transport_new(gcs_com.network); - - event_set(&gcs_com.datalink_event, gcs_com.network->socket_in, EV_READ | EV_PERSIST, on_datalink_event, - gcs_com.udp_transport); - event_add(&gcs_com.datalink_event, NULL); - -} - -void gcs_com_periodic(void) -{ - if (gcs_com.udp_transport->Periodic) { - gcs_com.udp_transport->Periodic(gcs_com.udp_transport->impl); - } -} - - -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) -{ - - char buf[255]; - int bytes_read; - bytes_read = checked_read(fd, buf, sizeof(buf) - 1); - struct DownlinkTransport *tp = (struct DownlinkTransport *) arg; - struct udp_transport *udp_impl = tp->impl; - //printf("on datalink event: %d bytes\n",bytes_read); - int i = 0; - while (i < bytes_read) { - parse_udp_dl(udp_impl, buf[i]); - i++; - if (udp_impl->udp_dl_msg_received) { - memcpy(gcs_com.my_dl_buffer, udp_impl->udp_dl_payload, udp_impl->udp_dl_payload_len); - dl_handle_msg(tp); - udp_impl->udp_dl_msg_received = FALSE; - } - } - -} - -#define IdOfMsg(x) (x[1]) - -static void dl_handle_msg(struct DownlinkTransport *tp) -{ - uint8_t msg_id = IdOfMsg(gcs_com.my_dl_buffer); - switch (msg_id) { - - case DL_PING: - DOWNLINK_SEND_PONG(tp); - break; - - case DL_SETTING : { - uint8_t i = DL_SETTING_index(gcs_com.my_dl_buffer); - float var = DL_SETTING_value(gcs_com.my_dl_buffer); - DlSetting(i, var); - printf("datalink : %d %f\n", i, var); - DOWNLINK_SEND_DL_VALUE(tp, &i, &var); - } - break; - - default : - printf("datalink did nothing\n"); - break; - } - -} - - -static inline int checked_read(int fd, char *buf, size_t len) -{ - int bytes = read(fd, buf, len); - - if (bytes == 0) { - fprintf(stderr, "Connection closed\n"); - } else if (bytes == -1) { - perror("read"); - } - return bytes; -} - - diff --git a/sw/airborne/firmwares/beth/overo_gcs_com.h b/sw/airborne/firmwares/beth/overo_gcs_com.h deleted file mode 100644 index b55abb054d6..00000000000 --- a/sw/airborne/firmwares/beth/overo_gcs_com.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef OVERO_GCS_COM_H -#define OVERO_GCS_COM_H - -#include -#include "fms_network.h" -#include "downlink_transport.h" - -#include "std.h" - -#define GCS_COM_DL_BUF_SIZE 128 - -struct OveroGcsCom { - - struct FmsNetwork *network; - struct DownlinkTransport *udp_transport; - struct event datalink_event; - /* bool_t my_dl_msg_available; */ - uint8_t my_dl_buffer[GCS_COM_DL_BUF_SIZE] __attribute__((aligned)); - -}; - - -extern struct OveroGcsCom gcs_com; - -extern void gcs_com_init(void); -extern void gcs_com_periodic(void); - - -#endif /* OVERO_GCS_COM_H */ diff --git a/sw/airborne/firmwares/beth/overo_sfb_controller.c b/sw/airborne/firmwares/beth/overo_sfb_controller.c deleted file mode 100644 index 3d4145a075c..00000000000 --- a/sw/airborne/firmwares/beth/overo_sfb_controller.c +++ /dev/null @@ -1,197 +0,0 @@ -#include "overo_sfb_controller.h" - -#include "overo_estimator.h" -#include "std.h" -#include "stdio.h" -#include "stdlib.h" - -#include "messages2.h" -#include "overo_gcs_com.h" - -#define _CO (controller) - -struct OveroController _CO; - -#define GAIN (RadOfDeg(15)) - -static float z0 = 0, z1 = GAIN, z2 = 0, z3 = -GAIN, z4 = 0; -//static float x2=0, x3=-GAIN, x4=0; -static float x0 = GAIN, x1 = 0, x2 = -GAIN, x3 = 0, x4 = 0; - -void control_send_messages(void) -{ - - RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport, &z0, &z1, &z2, &z3);}); -} - - -void control_init(void) -{ - - _CO.tilt_sp = 0.; - _CO.elevation_sp = RadOfDeg(10); - _CO.azimuth_sp = 0.; - - _CO.tilt_ref = 0.; - _CO.elevation_ref = 0.; - _CO.azimuth_ref = 0.; - - _CO.tilt_dot_ref = 0.; - _CO.elevation_dot_ref = 0.; - _CO.azimuth_dot_ref = 0.; - - _CO.cmd_sfb_pitch = 0.; - _CO.cmd_sfb_thrust = 0.; - - _CO.cmd_df_pitch = 0.; - _CO.cmd_df_thrust = 0.; - - _CO.cmd_pitch = 0.; - _CO.cmd_thrust = 0.; - - _CO.a = 0.03;//theoretical=19.62 - _CO.a = 0.06; - //_CO.b = 0.27;//theoretical=157.21 - _CO.b = 0.86; - _CO.u_t_ref = 70; - - /*omegas - natural frequencies*/ - _CO.o_tilt = RadOfDeg(300);//was 100 - _CO.o_elev = RadOfDeg(100); - _CO.o_azim = RadOfDeg(100); - - /*zetas - damping ratios*/ - _CO.z_tilt = 1.; - _CO.z_elev = 1.; - _CO.z_azim = 1.; - - _CO.armed = 0; -} - - - -void control_run(void) -{ - - static int foo = 0; - - calc_df_cmd(); - - _CO.u_t_ref = _CO.cmd_df_thrust; - - calc_sfb_cmd(); - - _CO.cmd_pitch = _CO.cmd_sfb_pitch + _CO.cmd_df_pitch; - _CO.cmd_thrust = _CO.cmd_sfb_thrust + _CO.cmd_df_thrust; - - if (!(foo % 100)) { - printf("P:%f T:%f \n", _CO.cmd_df_pitch, _CO.cmd_df_thrust); - } - foo++; - - Bound(_CO.cmd_thrust, 0, 100); - Bound(_CO.cmd_pitch, -100, 100); - -} - -void calc_df_cmd(void) -{ - - static uint32_t timecnt = 0; - static float time = 0; - - const float dt = 1. / 512.; - const float g = 9.8; - const float freq1 = 1. / (2. * 3.14159); - const float freq2 = 1. / (1. * 3.14159); - const float const1 = 9.8 / 75.; - const float const2 = 0.04; - - if (_CO.armed) { - time = timecnt++ * dt; - } - - /* x2 = x2 + x3*dt; - x3 = x3 + x4*dt;*/ - //x4 = GAIN*sin (2 * 3.14159 * freq2 * time); - x0 = GAIN * cos(2 * 3.14159 * freq2 * time); - x1 = -GAIN * sin(2 * 3.14159 * freq2 * time); - x2 = -GAIN * cos(2 * 3.14159 * freq2 * time); - x3 = GAIN * sin(2 * 3.14159 * freq2 * time); - x4 = GAIN * cos(2 * 3.14159 * freq2 * time); - - /* z0 = z0 + z1*dt ; - z1 = z1 + z2*dt ; - z2 = z2 + z3*dt ; - z3 = z3 + z4*dt ;*/ - z0 = GAIN * sin(2 * 3.14159 * freq1 * time); - z1 = GAIN * cos(2 * 3.14159 * freq1 * time); - z2 = -GAIN * sin(2 * 3.14159 * freq1 * time); - z3 = -GAIN * cos(2 * 3.14159 * freq1 * time); - z4 = GAIN * sin(2 * 3.14159 * freq1 * time); - - _CO.cmd_df_thrust = (1 / const1) * sqrt(powf(x2, 2) + powf((z2 + g) , 2)) ; - _CO.cmd_df_pitch = (1 / const2) * - ((x4 * (z2 + 1) - z4 * x2) * (powf(z2 + g, 2) + powf(x2, - 2)) - (2 * (z2 + g) * z3 + 2 * x2 * x3) * (x3 * (z2 + g) - z3 * x2)) / - powf((powf(z2 + g, 2) + powf(x2, 2)) , 2); - - Bound(_CO.cmd_df_thrust, 0, 100); - Bound(_CO.cmd_df_pitch, -100, 100); -} - -void calc_sfb_cmd(void) -{ - /* - * calculate errors - */ - - /* const float err_tilt = estimator.tilt - _CO.tilt_ref; - const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;*/ - const float err_tilt = estimator.tilt - x0; - const float err_tilt_dot = estimator.tilt_dot - x1; - - /* const float err_elevation = estimator.elevation - _CO.elevation_ref; - const float err_elevation_dot = estimator.elevation_dot - _CO.elevation_dot_ref;*/ - - const float err_elevation = estimator.elevation - z0; - const float err_elevation_dot = estimator.elevation_dot - z1; - - const float err_azimuth = estimator.azimuth - _CO.azimuth_ref; - const float err_azimuth_dot = estimator.azimuth_dot - _CO.azimuth_dot_ref; - - /* - * Compute state feedback - */ - - _CO.cmd_sfb_pitch = -1 * (-1 * - err_azimuth - * (_CO.o_tilt * _CO.o_tilt * _CO.o_azim * _CO.o_azim * cos(estimator.tilt)) - / (_CO.b * _CO.a * _CO.u_t_ref) + - err_elevation - * (_CO.o_tilt * _CO.o_tilt * _CO.o_elev * _CO.o_elev * sin(estimator.tilt)) - / (_CO.b * _CO.a * _CO.u_t_ref) - - err_tilt - * (_CO.o_tilt * _CO.o_tilt) / (_CO.b) - //+ - err_azimuth_dot - * (_CO.o_tilt * _CO.o_tilt * 2 * _CO.z_azim * _CO.o_azim * cos(estimator.tilt)) - / (_CO.b * _CO.a * _CO.u_t_ref) + - err_elevation_dot - * (_CO.o_tilt * _CO.o_tilt * 2 * _CO.z_elev * _CO.o_elev * sin(estimator.tilt)) - / (_CO.b * _CO.a * _CO.u_t_ref) - - err_tilt_dot - * (2 * _CO.o_tilt * _CO.z_tilt) - / (_CO.b)); - - _CO.cmd_sfb_thrust = - err_azimuth * _CO.o_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a - - err_elevation * _CO.o_elev * _CO.o_elev * cos(estimator.tilt) / _CO.a + - err_azimuth_dot * 2 * _CO.z_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a - - err_elevation_dot * 2 * _CO.z_elev * _CO.o_elev * cos(estimator.tilt) / _CO.a ; - - _CO.cmd_sfb_thrust = _CO.cmd_sfb_thrust * (1 / cos(estimator.elevation)); - - //if (_CO.cmd_thrust<0.) _CO.cmd_thrust = 0; - Bound(_CO.cmd_sfb_thrust, 0, 100); - Bound(_CO.cmd_sfb_pitch, -100, 100); -} diff --git a/sw/airborne/firmwares/beth/overo_sfb_controller.h b/sw/airborne/firmwares/beth/overo_sfb_controller.h deleted file mode 100644 index f3e10129f30..00000000000 --- a/sw/airborne/firmwares/beth/overo_sfb_controller.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef OVERO_CONTROLLER_H -#define OVERO_CONTROLLER_H - -struct OveroController { - - float tilt_sp; - float elevation_sp; - float azimuth_sp; - - float tilt_ref; - float elevation_ref; - float azimuth_ref; - - float tilt_dot_ref; - float elevation_dot_ref; - float azimuth_dot_ref; - - /*omegas - natural frequencies*/ - float o_tilt; - float o_elev; - float o_azim; - - /*zetas - damping ratios*/ - float z_tilt; - float z_elev; - float z_azim; - - /*constants*/ - - float a; // C_t0 / M - float b; // l * C_t0 / J - - float u_t_ref; - - float cmd_sfb_pitch; - float cmd_sfb_thrust; - - float cmd_df_pitch; - float cmd_df_thrust; - - float cmd_pitch; - float cmd_thrust; - - int armed; -}; - - -extern struct OveroController controller; - -extern void control_init(void); -extern void control_send_messages(void); -extern void control_run(void); -void calc_df_cmd(void); -void calc_sfb_cmd(void); - -#endif /* OVERO_CONTROLLER_H */ diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c deleted file mode 100644 index 7f98cdb2963..00000000000 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ /dev/null @@ -1,247 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include -#include - -#include - -#include "subsystems/gps.h" - -#include "messages2.h" -//#include "dl_protocol2.h" -#include "generated/airframe.h" - -#include "fms_periodic.h" -#include "fms_debug.h" -#include "serial_port.h" - -#include "overo_gcs_com.h" -#include "uart_hw.h" -#include "subsystems/datalink/pprz_transport.h" - - -struct OveroController { - int armed; -} controller; - -static void main_periodic(int); -static void main_exit(int sig); -static void main_talk_with_tiny(void); -void check_gps(void); - -//make gps.c happy without including navigation code -uint8_t nav_utm_zone0 = 31; - -static uint16_t foo = 0; -//struct SerialPort* fmssp; -//int spfd; -uint8_t portnum; -#ifdef GPS_CONFIGURE -static uint8_t donegpsconf = 0; -#endif -static uint8_t configgps = 0; - - -extern float phi; -extern float psi; -extern float theta; -extern uint16_t throttle; -extern uint16_t voltage; -extern uint16_t amps; -extern uint16_t energy; -extern uint16_t adc1; -extern uint16_t adc2; - -int main(int argc, char *argv[]) -{ - portnum = 0; - - if (argc > 1) { - portnum = atoi(argv[1]); - if (portnum > 10) { - printf("Port number must be <11\n"); - return -1; - } - if (argc > 2) { configgps = atoi(argv[2]); } - if (configgps) -#ifdef GPS_CONFIGURE - printf("Will configure GPS.\n"); -#else - printf("Rebuild with GPS configure support.\n"); -#endif - } - - printf("Using /dev/ttyUSB%d for GPS\n", portnum); - - - (void) signal(SIGINT, main_exit); - - uart_init(); - gps_init(); - - /* Initalize the event library */ - event_init(); - - gcs_com_init(); - - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return -1; - } - -#ifdef GPS_CONFIGURE - //periodic task is launched so we are now ready to use uart to request gps baud change... - if (configgps) { gps_configure_uart(); } -#endif - event_dispatch(); - //should never occur! - printf("goodbye! (%d)\n", foo); - - return 0; -} - - -static void main_periodic(int my_sig_num) -{ - - - RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); - RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(gcs_com.udp_transport, &adc1, &adc2);}); - -#if USE_UART0 - uart0_handler(); -#endif -#if USE_UART1 - uart1_handler(); -#endif - DatalinkEvent() - main_talk_with_tiny(); - check_gps(); - - RunOnceEvery(20, gcs_com_periodic()); - -} - -#include "subsystems/datalink/downlink.h" -#if 0 -uint8_t downlink.nb_ovrn; -uint16_t downlink.nb_bytes; -uint16_t downlink.nb_msgs; - -#define __Transport(dev, _x) dev##_x -#define _Transport(dev, _x) __Transport(dev, _x) -#define Transport(_chan, _fun) _Transport(_chan, _fun) -#define DownlinkIDsSize(_chan, _x) (_x+2) -#define DownlinkSizeOf(_chan, _x) Transport(_chan, SizeOf(DownlinkIDsSize(_chan, _x))) - -#define DownlinkCheckFreeSpace(_chan, _x) Transport(_chan, CheckFreeSpace((uint8_t)(_x))) - -#define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x)) -#define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x)) - -#define DownlinkOverrun(_chan) downlink.nb_ovrn++; -#define DownlinkCountBytes(_chan, _n) downlink.nb_bytes += _n; - -#define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \ - downlink.nb_msgs++; \ - Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ - Transport(_chan, PutUint8(AC_ID)); \ - Transport(_chan, PutNamedUint8(_name, msg_id)); \ - } - -#define DownlinkEndMessage(_chan) Transport(_chan, Trailer()) - - -#define __DOWNLINK_SEND_HITL_UBX(_chan, class, id, ac_id, nb_ubx_payload, ubx_payload){ \ - if (DownlinkCheckFreeSpace(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1))) {\ - DownlinkCountBytes(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1)); \ - DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \ - DownlinkPutUint8ByAddr(_chan, (class)); \ - DownlinkPutUint8ByAddr(_chan, (id)); \ - DownlinkPutUint8ByAddr(_chan, (ac_id)); \ - DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \ - DownlinkEndMessage(_chan ) \ - } else \ - DownlinkOverrun(_chan ); \ - } -#endif - -void check_gps(void) -{ - - /* if (GpsTimeoutError) { - printf("gps timeout\n"); - }*/ - if (GpsBuffer()) { - ReadGpsBuffer(); - } - - if (gps_msg_received) { -#ifdef GPS_CONFIGURE - if (gps_configuring) { - gps_configure(); - } else { - if (!donegpsconf) { - printf("Finished GPS configuration.\n"); - donegpsconf = 1; - } - parse_gps_msg(); - } -#else - parse_gps_msg(); -#endif - printf("gps msg rx %x %x\n", ubx_class, ubx_id); - const uint8_t ac_id = 3; - //DOWNLINK_SEND_HITL_UBX(gcs_com.udp_transport, &ubx_class, &ubx_id, &ac_id, &ubx_len ,ubx_msg_buf); - DOWNLINK_SEND_HITL_UBX(PprzTransport, &ubx_class, &ubx_id, &ac_id, ubx_len , ubx_msg_buf); - gps_msg_received = FALSE; - if (gps_pos_available) { - printf("gps pos avail\n"); - gps_verbose_downlink = 0; -// UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); - gps_pos_available = FALSE; - } - } -} - -static void main_exit(int sig) -{ - printf("Application Exiting...\n"); - exit(EXIT_SUCCESS); -} - -static void main_talk_with_tiny() -{ - //unsigned char c='D'; - //write(spfd,&c,1); - //if (read(spfd,&c,1)>0) write(STDOUT_FILENO,&c,1); - //fprintf(spfd,"testing\n"); - //printf("."); - foo++; -} - diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.c b/sw/airborne/firmwares/beth/overo_twist_controller.c deleted file mode 100644 index 35dece277a4..00000000000 --- a/sw/airborne/firmwares/beth/overo_twist_controller.c +++ /dev/null @@ -1,374 +0,0 @@ -#include "overo_twist_controller.h" - -#include "overo_estimator.h" -#include "std.h" -#include "stdio.h" -#include "stdlib.h" //for abs() - -#include "messages2.h" -#include "overo_gcs_com.h" - -struct OveroTwistController controller; - -void control_send_messages(void) -{ - - RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport, - &controller.cmd_pitch, &controller.cmd_thrust, - &controller.cmd_pitch_ff, &controller.cmd_pitch_fb, - &controller.cmd_thrust_ff, &controller.cmd_thrust_fb, - &controller.tilt_sp, &controller.tilt_ref, &controller.tilt_dot_ref, - &controller.elevation_sp, &controller.elevation_ref, &controller.elevation_dot_ref, - &controller.azimuth_sp); - }); - - RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport, - &controller.S[1], &controller.S_dot, &controller.U_twt[1], &controller.error); - }); -} - - -void control_init(void) -{ - - printf("Twisting controller initializing\n"); - - controller.tilt_sp = 0.; - controller.elevation_sp = RadOfDeg(10); - controller.azimuth_sp = 0.; - -// controller.omega_tilt_ref = RadOfDeg(600); - controller.omega_tilt_ref = RadOfDeg(1200); - controller.omega_elevation_ref = RadOfDeg(120); - controller.omega_azimuth_ref = RadOfDeg(60); - controller.xi_ref = 1.; - - controller.tilt_ref = estimator.tilt; - controller.tilt_dot_ref = estimator.tilt_dot; - controller.tilt_ddot_ref = 0.; - - //controller.elevation_ref = estimator.elevation; - controller.elevation_ref = RadOfDeg(-28); - controller.elevation_dot_ref = estimator.elevation_dot; - controller.elevation_ddot_ref = 0.; - - controller.azimuth_ref = estimator.azimuth; - controller.azimuth_dot_ref = 0.; - controller.azimuth_ddot_ref = 0.; - - controller.one_over_J = 2.; - controller.mass = 5.; - controller.azim_gain = 0.005; - - controller.omega_cl = RadOfDeg(600); - controller.xi_cl = 1.; - - controller.cmd_pitch_ff = 0.; - controller.cmd_pitch_fb = 0.; - - controller.cmd_thrust_ff = 0.; - controller.cmd_thrust_fb = 0.; - - controller.cmd_pitch = 0.; - controller.cmd_thrust = 0.; - - controller.armed = 0; - - /***** Coef twisting ****/ - controller.ulim = 1.0; - controller.Vm = 0.1; //should this now be 1/512? - controller.VM = 300.0; - - controller.S[1] = 0.0; - controller.S[0] = 0.0; - - controller.U_twt[1] = 0.0; - controller.U_twt[0] = 0.0; - - controller.satval1 = 0.087; - controller.satval2 = 0.141; - - controller.c = 4.5; - controller.error = 0; - - printf("Vm=%f VM=%f satval1=%f satval2=%f c=%f\n", controller.Vm, controller.VM, controller.satval1, controller.satval2, - controller.c); -} - - - -void control_run(void) -{ - static int foo = 0; - /* - * propagate reference - */ - const float dt_ctl = 1. / 512.; - const float thrust_constant = 40.; - - //make setpoint a cosine ; for testing - float new_sp = controller.tilt_sp * cos(6.28 / 5.*foo / 512.); - - controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl; - controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref * dt_ctl; - controller.tilt_ddot_ref = -2 * controller.omega_tilt_ref * controller.xi_ref * controller.tilt_dot_ref - - controller.omega_tilt_ref * controller.omega_tilt_ref * (controller.tilt_ref - new_sp); - - controller.elevation_ref = controller.elevation_ref + controller.elevation_dot_ref * dt_ctl; - controller.elevation_dot_ref = controller.elevation_dot_ref + controller.elevation_ddot_ref * dt_ctl; - controller.elevation_ddot_ref = -2 * controller.omega_elevation_ref * controller.xi_ref * controller.elevation_dot_ref - - controller.omega_elevation_ref * controller.omega_elevation_ref * (controller.elevation_ref - - controller.elevation_sp); - -#if USE_AZIMUTH - controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl; - controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl; - controller.azimuth_ddot_ref = -2 * controller.omega_azimuth_ref * controller.xi_ref * controller.azimuth_dot_ref - - controller.omega_azimuth_ref * controller.omega_azimuth_ref * (controller.azimuth_ref - controller.azimuth_sp); -#endif - - - /* - * calculate errors - */ - - /* const float err_tilt = estimator.tilt - controller.tilt_ref; - const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;*/ - - const float err_elevation = estimator.elevation - controller.elevation_ref; - const float err_elevation_dot = estimator.elevation_dot - controller.elevation_dot_ref; - -#if USE_AZIMUTH - const float err_azimuth = estimator.azimuth - controller.azimuth_ref; - const float err_azimuth_dot = estimator.azimuth_dot - controller.azimuth_dot_ref; -#endif - - /* - * Compute feedforward and feedback commands - */ - - controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref; - - /* controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt);*/ - - controller.cmd_pitch_fb = get_U_twt(); - - controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref; - controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) - - controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation); - -#if USE_AZIMUTH - controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref; - controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); -#endif - - controller.cmd_pitch = /*controller.cmd_pitch_ff*/ + controller.cmd_pitch_fb; - -#if USE_AZIMUTH - controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb); -#endif - - controller.cmd_thrust = get_U_twt2(); -// controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant; - //not needed as twisting doesn't care about this nonlinearity -// controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation)); - - //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0; - Bound(controller.cmd_thrust, 0, 100); - Bound(controller.cmd_pitch, -100, 100); - - if (!(foo % 128)) { - //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot); - //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot); - //printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref); - //printf("t: %f\n",controller.cmd_thrust); - } - foo++; - -} - -//TODO: replace both functions with one that takes parameters (or a structure of params) - -/*Fonction qui obtient la commande twisiting à appliquer chaque periode*/ -float get_U_twt() -{ - - /**Definition des constantes du modèle**/ - const float Gain = -65; - const float Te = 1 / 512.; - - /**Variables utilisés par la loi de commande**/ - static volatile float yd[2] = {0.0, 0.0}; - static volatile float y[2] = {0., 0.}; - //static float emax = 0.035; // en rad, initialement 2 - - /**Variables pour l'algorithme**/ - float udot; - float sens; - - //Acquisition consigne - yd[1] = controller.tilt_ref; - //Acquisition mesure - y[1] = estimator.tilt; - - /***************************/ - - /**Calcul Surface et derive Surface**/ - // S[1],y[1],yd[1] new value - // S[0],y[0],yd[0] last value - - //gain K=Te - //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ; - //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ; - controller.S[1] = (float)(controller.c * (y[1] - yd[1]) + estimator.tilt_dot - controller.tilt_dot_ref); - controller.S_dot = (controller.S[1] - controller.S[0]); - /*************************************/ - - //On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure - /* if ( abs(y[1] - yd[1]) < emax ) { - U_twt[1] = U_twt[0]; - } else {*/ - /**Algorithme twisting**/ - if (controller.S[1] < 0.0) { - sens = -1.0; - } else { - sens = 1.0; - } - - if (abs(controller.U_twt[1]) < controller.ulim) { - if ((controller.S[1] * controller.S_dot) > 0) { - udot = -controller.VM * sens; - } else { - udot = -controller.Vm * sens; - } - } else { - udot = -controller.U_twt[1]; - } - - // Integration de u, qu'avec 2 valeurs, penser à faire plus - // u[1] new , u[0] old - controller.U_twt[1] = controller.U_twt[0] + (Te * udot); - //} - /**********************/ - - /**Saturation de l'integrateur**/ - - if ((controller.S[1] > -controller.satval1) && (controller.S[1] < controller.satval1)) { - Bound(controller.U_twt[1], -controller.satval1, controller.satval1); - } else { - Bound(controller.U_twt[1], -controller.satval2, controller.satval2); - } - /********************************/ - - /**Mises à jour**/ - controller.U_twt[0] = controller.U_twt[1]; - yd[0] = yd[1]; - y[0] = y[1]; - - controller.S[0] = controller.S[1]; - - return Gain * controller.U_twt[1]; - -} - -float get_U_twt2() -{ - - /**Definition des constantes du modèle**/ - const float Gain = 800.; - const float Te = 1 / 512.; - - /**Variables utilisés par la loi de commande**/ - static volatile float yd2[2] = {0.0, 0.0}; - static volatile float y2[2] = {0., 0.}; - //static float emax = 0.035; // en rad, initialement 2 - - /**Variables pour l'algorithme**/ - float udot2; - float sens2; - static float S2[2] = {0.0, 0.0}; - static float S2_dot = 0; - static float U2_twt[2] = {0.0, 0.0}; - - //Acquisition consigne - yd2[1] = controller.elevation_ref; - //Acquisition mesure - y2[1] = estimator.elevation; - - /***************************/ - - /**Calcul Surface et derive Surface**/ - // S[1],y[1],yd[1] new value - // S[0],y[0],yd[0] last value - - //gain K=Te - //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ; - //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ; - S2[1] = (float)(controller.c * (y2[1] - yd2[1]) + estimator.elevation_dot - controller.elevation_dot_ref); - S2_dot = (S2[1] - S2[0]); - /*************************************/ - - //On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure - /* if ( abs(y[1] - yd[1]) < emax ) { - U_twt[1] = U_twt[0]; - } else {*/ - /**Algorithme twisting**/ - if (S2[1] < 0.0) { - sens2 = -1.0; - } else { - sens2 = 1.0; - } - - if (abs(U2_twt[1]) < controller.ulim) { - if ((S2[1] * S2_dot) > 0) { - udot2 = -controller.VM * sens2; - } else { - udot2 = -controller.Vm * sens2; - } - } else { - udot2 = -U2_twt[1]; - } - - // Integration de u, qu'avec 2 valeurs, penser à faire plus - // u[1] new , u[0] old - U2_twt[1] = U2_twt[0] + (Te * udot2); - //} - /**********************/ - - /**Saturation de l'integrateur**/ - - if ((S2[1] > -controller.satval1) && (S2[1] < controller.satval1)) { - Bound(U2_twt[1], -controller.satval1, controller.satval1); - } else { - Bound(U2_twt[1], -controller.satval2, controller.satval2); - } - /********************************/ - - /**Mises à jour**/ - U2_twt[0] = U2_twt[1]; - yd2[0] = yd2[1]; - y2[0] = y2[1]; - - S2[0] = S2[1]; - -#define NUMSAMPS (1000) - float retval = Gain * U2_twt[1]; - - static int sum = 0; - static int i = 0; - - sum = sum + retval; - - if (i == (NUMSAMPS - 1)) { - i = 0; - printf("avg: %f\n", sum / (float)NUMSAMPS); - sum = 0; - } else { - i++; - } - - return retval; - -} diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.h b/sw/airborne/firmwares/beth/overo_twist_controller.h deleted file mode 100644 index 11e6819a8f6..00000000000 --- a/sw/airborne/firmwares/beth/overo_twist_controller.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef OVERO_TWIST_CONTROLLER_H -#define OVERO_TWIST_CONTROLLER_H - -struct OveroTwistController { -// float kp; -// float kd; - - float tilt_sp; - float elevation_sp; - float azimuth_sp; - - /* modele de reference */ - float tilt_ref; - float tilt_dot_ref; - float tilt_ddot_ref; - - float elevation_ref; - float elevation_dot_ref; - float elevation_ddot_ref; - - float azimuth_ref; - float azimuth_dot_ref; - float azimuth_ddot_ref; - - float omega_tilt_ref; - float omega_elevation_ref; - float omega_azimuth_ref; - float xi_ref; - - /* invert control law parameter */ - float one_over_J; - float mass; - - /* closed loop parameters */ - float omega_cl; - float xi_cl; - float azim_gain; - - float cmd_pitch_ff; - float cmd_pitch_fb; - - float cmd_thrust_ff; - float cmd_thrust_fb; - - float cmd_azimuth_ff; - float cmd_azimuth_fb; - - float cmd_pitch; - float cmd_thrust; - - int armed; - - /***Twisting stuff***/ - - float S[2]; - float S_dot; - float U_twt[2]; - - /***** Coeficients twisting ****/ - float ulim; - float Vm; - float VM; - - float satval1; - float satval2; - - float c; - - float error; - -}; - - -extern struct OveroTwistController controller; - -extern void control_init(void); -extern void control_send_messages(void); -extern void control_run(void); -float get_U_twt(void); -float get_U_twt2(void); - -#endif /* OVERO_TWIST_CONTROLLER_H */ diff --git a/sw/airborne/firmwares/beth/rcv_telemetry.c b/sw/airborne/firmwares/beth/rcv_telemetry.c deleted file mode 100644 index 6b2de3f7c78..00000000000 --- a/sw/airborne/firmwares/beth/rcv_telemetry.c +++ /dev/null @@ -1,101 +0,0 @@ -/* - * Copyright (C) 2005 Pascal Brisset, Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ -/** \file rcv_telemetry.c - * \brief Handling of messages coming from other A/Cs - * - */ -#define DATALINK_C - -#define MODULES_DATALINK_C - -#include -#include -#include "subsystems/datalink/datalink.h" - -#include "generated/modules.h" - -#include -#include "subsystems/navigation/common_nav.h" -#include "generated/settings.h" - - -#include "mcu_periph/uart.h" -#include "subsystems/datalink/downlink.h" - - - -#define MOfCm(_x) (((float)(_x))/100.) - -#define SenderIdOfMsg(x) (x[0]) -#define IdOfMsg(x) (x[1]) - -float phi; -float psi; -float theta; -uint16_t throttle; -uint16_t voltage; -uint16_t amps; -uint16_t energy; -uint16_t adc1; -uint16_t adc2; - -/* - 6 ATTITUDE - 8 GPS -10 NAVIGATION -12 BAT -16 DESIRED -35 -36 -37 ENERGY -42 ESTIMATOR -*/ -void dl_parse_msg(void) -{ - datalink_time = 0; - uint8_t msg_id = IdOfMsg(dl_buffer); - printf("Tiny rx id: %d\n", msg_id); - - if (msg_id == DL_ATTITUDE) { - phi = DL_ATTITUDE_phi(dl_buffer); - psi = DL_ATTITUDE_psi(dl_buffer); - theta = DL_ATTITUDE_theta(dl_buffer); - printf("Attitude: %f %f %f\n", phi, psi, theta); - } - if (msg_id == DL_BAT) { - throttle = DL_BAT_throttle(dl_buffer); - voltage = DL_BAT_voltage(dl_buffer); - amps = DL_BAT_amps(dl_buffer); - energy = DL_BAT_energy(dl_buffer); - printf("BAT: %d %d %d %d\n", throttle, voltage, amps, energy); - } - if (msg_id == DL_ADC_GENERIC) { - adc1 = DL_ADC_GENERIC_val1(dl_buffer); - adc2 = DL_ADC_GENERIC_val2(dl_buffer); - printf("ADC: %d %d\n", adc1, adc2); - } - - else { - //printf("Tiny msg_id %d unknown\n",msg_id); - } - -} diff --git a/sw/airborne/firmwares/beth/sys_time_hw.h b/sw/airborne/firmwares/beth/sys_time_hw.h deleted file mode 100644 index 9fe7c2c894b..00000000000 --- a/sw/airborne/firmwares/beth/sys_time_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef SYS_TIME_HW_H -#define SYS_TIME_HW_H - -#endif /* SYS_TIME_HW_H */ diff --git a/sw/airborne/firmwares/beth/uart_hw.c b/sw/airborne/firmwares/beth/uart_hw.c deleted file mode 100644 index 2afcc1a18c5..00000000000 --- a/sw/airborne/firmwares/beth/uart_hw.c +++ /dev/null @@ -1,277 +0,0 @@ -/* - * Copyright (C) 2009 Antoine Drouin - * - * 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 "mcu_periph/uart.h" - -#include -#include -#include -#include -#include - -#include "serial_port.h" - - -#if USE_UART0 - -volatile uint16_t uart0_rx_insert_idx, uart0_rx_extract_idx; -uint8_t uart0_rx_buffer[UART0_RX_BUFFER_SIZE]; - -volatile uint16_t uart0_tx_insert_idx, uart0_tx_extract_idx; -volatile bool_t uart0_tx_running; -uint8_t uart0_tx_buffer[UART0_TX_BUFFER_SIZE]; - -struct SerialPort *fmssp0; -int uart0_fd; -extern uint8_t portnum; - - -//This function will close our UART and reopen with the new baud rate -#ifdef GPS_CONFIGURE -void uart0_init_param(uint16_t baud, uint8_t mode, uint8_t fmode) -{ - - //serial_port_flush_output(fmssp0); - serial_port_close(fmssp0); - fmssp0 = serial_port_new(); - - if (portnum == 0) { - printf("opening ttyUSB0 on uart0 at %d\n", GPS_BAUD); - serial_port_open_raw(fmssp0, "/dev/ttyUSB0", GPS_BAUD); - } - if (portnum == 1) { - printf("opening ttyUSB1 on uart0 at %d\n", GPS_BAUD); - serial_port_open_raw(fmssp0, "/dev/ttyUSB1", GPS_BAUD); - } - - uart0_fd = (int)fmssp0->fd; - - // initialize the transmit data queue - uart0_tx_extract_idx = 0; - uart0_tx_insert_idx = 0; - uart0_tx_running = FALSE; - - // initialize the receive data queue - uart0_rx_extract_idx = 0; - uart0_rx_insert_idx = 0; - -} -#endif - -void uart0_init(void) -{ - - fmssp0 = serial_port_new(); - - -//TODO: set device name in application and pass as argument - if (portnum == 0) { - printf("opening ttyUSB0 on uart0 at %d\n", UART0_BAUD); - serial_port_open_raw(fmssp0, "/dev/ttyUSB0", UART0_BAUD); - } - if (portnum == 1) { - printf("opening ttyUSB1 on uart0 at %d\n", UART0_BAUD); - serial_port_open_raw(fmssp0, "/dev/ttyUSB1", UART0_BAUD); - } - uart0_fd = (int)fmssp0->fd; - - // initialize the transmit data queue - uart0_tx_extract_idx = 0; - uart0_tx_insert_idx = 0; - uart0_tx_running = FALSE; - - // initialize the receive data queue - uart0_rx_extract_idx = 0; - uart0_rx_insert_idx = 0; - -} - -void uart0_transmit(uint8_t data) -{ - - uint16_t temp = (uart0_tx_insert_idx + 1) % UART0_TX_BUFFER_SIZE; - - if (temp == uart0_tx_extract_idx) { - return; // no room - } - - // check if in process of sending data - if (uart0_tx_running) { // yes, add to queue - uart0_tx_buffer[uart0_tx_insert_idx] = data; - uart0_tx_insert_idx = temp; - } else { // no, set running flag and write to output register - uart0_tx_running = TRUE; - write(uart0_fd, &data, 1); - //printf("w %x\n",data); - } - -} - -bool_t uart0_check_free_space(uint8_t len) -{ - int16_t space = uart0_tx_extract_idx - uart0_tx_insert_idx; - if (space <= 0) { - space += UART0_TX_BUFFER_SIZE; - } - return (uint16_t)(space - 1) >= len; -} - -void uart0_handler(void) -{ - unsigned char c = 'D'; - - // check if more data to send - if (uart0_tx_insert_idx != uart0_tx_extract_idx) { - write(uart0_fd, &uart0_tx_buffer[uart0_tx_extract_idx], 1); - //printf("w %x\n",uart0_tx_buffer[uart0_tx_extract_idx]); - uart0_tx_extract_idx++; - uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; - } else { - uart0_tx_running = FALSE; // clear running flag - } - - if (read(uart0_fd, &c, 1) > 0) { - //printf("r %x %c\n",c,c); - uint16_t temp = (uart0_rx_insert_idx + 1) % UART0_RX_BUFFER_SIZE; - uart0_rx_buffer[uart0_rx_insert_idx] = c; - // check for more room in queue - if (temp != uart0_rx_extract_idx) { - uart0_rx_insert_idx = temp; // update insert index - } - } - -} - -#endif /* USE_UART0 */ - -#if USE_UART1 - -volatile uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx; -uint8_t uart1_rx_buffer[UART1_RX_BUFFER_SIZE]; - -volatile uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx; -volatile bool_t uart1_tx_running; -uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; - -struct SerialPort *fmssp1; -int uart1_fd; - -void uart1_init(void) -{ - - fmssp1 = serial_port_new(); - - if (portnum == 0) { - printf("opening ttyUSB1 on uart1 at %d\n", UART1_BAUD); - serial_port_open_raw(fmssp1, "/dev/ttyUSB1", UART1_BAUD); - } - if (portnum == 1) { - printf("opening ttyUSB0 on uart1 at %d\n", UART1_BAUD); - serial_port_open_raw(fmssp1, "/dev/ttyUSB0", UART1_BAUD); - } - - uart1_fd = (int)fmssp1->fd; - - // initialize the transmit data queue - uart1_tx_extract_idx = 0; - uart1_tx_insert_idx = 0; - uart1_tx_running = FALSE; - - // initialize the receive data queue - uart1_rx_extract_idx = 0; - uart1_rx_insert_idx = 0; - -} - -void uart1_transmit(uint8_t data) -{ - - uint16_t temp = (uart1_tx_insert_idx + 1) % UART1_TX_BUFFER_SIZE; - - if (temp == uart1_tx_extract_idx) { - return; // no room - } - - // check if in process of sending data - if (uart1_tx_running) { // yes, add to queue - uart1_tx_buffer[uart1_tx_insert_idx] = data; - uart1_tx_insert_idx = temp; - } else { // no, set running flag and write to output register - uart1_tx_running = TRUE; - //printf("z %x\n",data); - write(uart1_fd, &data, 1); - } - -} - -bool_t uart1_check_free_space(uint8_t len) -{ - int16_t space = uart1_tx_extract_idx - uart1_tx_insert_idx; - if (space <= 0) { - space += UART1_TX_BUFFER_SIZE; - } - return (uint16_t)(space - 1) >= len; -} - -void uart1_handler(void) -{ - unsigned char c = 'D'; - - // check if more data to send - if (uart1_tx_insert_idx != uart1_tx_extract_idx) { - write(uart1_fd, &uart1_tx_buffer[uart1_tx_extract_idx], 1); - //printf("z %x\n",uart1_tx_buffer[uart1_tx_extract_idx]); - uart1_tx_extract_idx++; - uart1_tx_extract_idx %= UART1_TX_BUFFER_SIZE; - } else { - uart1_tx_running = FALSE; // clear running flag - } - - if (read(uart1_fd, &c, 1) > 0) { - //printf("s %x %c\n",c,c); - uint16_t temp = (uart1_rx_insert_idx + 1) % UART1_RX_BUFFER_SIZE;; - uart1_rx_buffer[uart1_rx_insert_idx] = c; - // check for more room in queue - if (temp != uart1_rx_extract_idx) { - uart1_rx_insert_idx = temp; // update insert index - } - } - -} - -#endif /* USE_UART1 */ - -void uart_init(void) -{ -#if USE_UART0 - uart0_init(); -#endif -#if USE_UART1 - uart1_init(); -#endif -//TODO: add uart2 and greater -#if USE_UART2 - uart2_init(); -#endif -#if USE_UART3 - uart3_init(); -#endif -} diff --git a/sw/airborne/firmwares/beth/uart_hw.h b/sw/airborne/firmwares/beth/uart_hw.h deleted file mode 100644 index e36d1fad481..00000000000 --- a/sw/airborne/firmwares/beth/uart_hw.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright (C) 2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef UART_HW_H -#define UART_HW_H - -#include "std.h" -//coment to avoid redefinition -/*#define B9600 9600 -#define B38400 38400 - #define B57600 57600 -#define B115200 115200 -*/ - -//junk for gps_configure_uart in gps_ubx.c to compile -#define UART_8N1 1 -#define UART_FIFO_8 1 - -#define UART1_irq_handler usart1_irq_handler -#define UART2_irq_handler usart2_irq_handler -#define UART3_irq_handler usart3_irq_handler -#define UART5_irq_handler usart5_irq_handler - -#if USE_UART0 -extern void uart0_handler(void); -#endif - - -#if USE_UART0 -#define UART0_RX_BUFFER_SIZE 128 -#define UART0_TX_BUFFER_SIZE 128 - -extern volatile uint16_t uart0_rx_insert_idx, uart0_rx_extract_idx; -extern uint8_t uart0_rx_buffer[UART0_RX_BUFFER_SIZE]; - -extern volatile uint16_t uart0_tx_insert_idx, uart0_tx_extract_idx; -extern volatile bool_t uart0_tx_running; -extern uint8_t uart0_tx_buffer[UART0_TX_BUFFER_SIZE]; - -#define UART0ChAvailable() (uart0_rx_insert_idx != uart0_rx_extract_idx) -#define UART0Getch() ({ \ - uint8_t ret = uart0_rx_buffer[uart0_rx_extract_idx]; \ - uart0_rx_extract_idx = (uart0_rx_extract_idx + 1)%UART0_RX_BUFFER_SIZE; \ - ret; \ - }) - -#endif /* USE_UART0 */ - -#if USE_UART1 -extern void uart1_handler(void); -#endif - - -#if USE_UART1 -#define UART1_RX_BUFFER_SIZE 128 -#define UART1_TX_BUFFER_SIZE 128 - -extern volatile uint16_t uart1_rx_insert_idx, uart1_rx_extract_idx; -extern uint8_t uart1_rx_buffer[UART1_RX_BUFFER_SIZE]; - -extern volatile uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx; -extern volatile bool_t uart1_tx_running; -extern uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; - -#define UART1ChAvailable() (uart1_rx_insert_idx != uart1_rx_extract_idx) -#define UART1Getch() ({ \ - uint8_t ret = uart1_rx_buffer[uart1_rx_extract_idx]; \ - uart1_rx_extract_idx = (uart1_rx_extract_idx + 1)%UART1_RX_BUFFER_SIZE; \ - ret; \ - }) - -#endif /* USE_UART1 */ - - -void uart_init(void); -void uart0_init_param(uint16_t baud, uint8_t mode, uint8_t fmode); - -#endif /* UART_HW_H */ diff --git a/sw/airborne/fms/Makefile b/sw/airborne/fms/Makefile deleted file mode 100644 index 588e846c28f..00000000000 --- a/sw/airborne/fms/Makefile +++ /dev/null @@ -1,55 +0,0 @@ - -# Quiet compilation -Q=@ - -CFLAGS = -Wall $(shell pkg-config --cflags glib-2.0) -g -LDFLAGS = $(shell pkg-config --libs glib-2.0) - -fms: fms_main.c fms_ap_link.c fms_serial_port.c fms_gs_link.c fms_network.c - $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) - - - -TT_CFLAGS = -Wall -TT_CFLAGS += -I../../include -TT_CFLAGS += -I.. -TT_CFLAGS += -I../../../var/include -TT_CFLAGS += -I../../../var/BOOZ2_A2 -TT_SRCS = test_telemetry.c -TT_LDFLAGS = -levent -TT_CFLAGS += -DDOWNLINK -TT_CFLAGS += -DDOWNLINK_TRANSPORT=UdpTransport -TT_SRCS += fms_network.c udp_transport.c ../downlink.c -#test_telemetry: $(TT_SRCS) -# $(CC) $(TT_CFLAGS) -o $@ $^ $(TT_LDFLAGS) - -# http://groups.google.com/group/beagleboard/browse_frm/thread/15d9488c1ec314ef/ca50640b9c51cef2 -# http://www.nabble.com/Overo-oe-SPI-questions-td22194463.html -# http://docwiki.gumstix.org/Sample_code/C/SPI -# http://www.nabble.com/SPI-Bus-td23307852.html -# http://groups.google.com/group/beagleboard/browse_thread/thread/42988f0e14db0f01/3200cce7400bb6b8?lnk=gst&q=mcspi#3200cce7400bb6b8 -# -# http://www.nabble.com/Making-Changes-permanent-td23358477.html - -# "export PATH=${PATH}:/home/gumstix/oe/tmp/staging/i686-linux/usr/bin" -# I just use "export PATH=${PATH}:/home/gumstix/oe/tmp/cross/armv7a/bin" and -# or "make ARCH=arm CROSS_COMPILE=arm-angstrom-linux-gnueabi-" if I have a makefile - -OVERO_OE=/overo-oe - -CROSS_CC=$(OVERO_OE)/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc - -CROSS_CFLAGS = -Wall -CROSS_LDFLAGS = - -test_telemetry: test_telemetry.c - $(CROSS_CC) $(CROSS_CFLAGS) -o $@ $^ $(CROSS_LDFLAGS) - -test_spi: test_spi.c - $(CROSS_CC) $(CROSS_CFLAGS) -o $@ $^ $(CROSS_LDFLAGS) - -onboard_logger: onboard_logger.c - $(CC) $(CFLAGS) -o $@ $^ -lpcap - -clean: - $(Q)rm -f *~ fms test_telemetry diff --git a/sw/airborne/fms/NOTES b/sw/airborne/fms/NOTES deleted file mode 100644 index 5488799c9e9..00000000000 --- a/sw/airborne/fms/NOTES +++ /dev/null @@ -1,5 +0,0 @@ -http://www.gumstix.net/Software/view/Software-Overo/Setting-up-a-build-environment/111.html - -http://www.gumstix.net/Software/view/Build-system-overview/Hello-world-tutorial/111.html - -Å“ diff --git a/sw/airborne/fms/fms_autopilot_msg.h b/sw/airborne/fms/fms_autopilot_msg.h deleted file mode 100644 index 50b9e61d058..00000000000 --- a/sw/airborne/fms/fms_autopilot_msg.h +++ /dev/null @@ -1,202 +0,0 @@ -#ifndef FMS_AUTOPILOT_H -#define FMS_AUTOPILOT_H - -#include -#include "math/pprz_algebra_int.h" -#include "math/pprz_geodetic_int.h" -// FIXME: why is including airframe.h needed ? -//#include "generated/airframe.h" -//#include "adc.h" -#define NB_ADC 8 - - -#define LISA_PWM_OUTPUT_NB 10 - -/* - * Testing - */ - -struct __attribute__((packed)) AutopilotMessageFoo { - uint32_t foo; - uint32_t bar; - uint32_t bla; - uint32_t ble; - uint32_t bli; - uint32_t blo; - uint32_t blu; - uint32_t bly; -}; - -/* - * BETH - */ -struct __attribute__((packed)) AutopilotMessageBethUp { - struct Int16Rates gyro; - struct Int16Vect3 accel; - struct Int16Vect3 bench_sensor; - uint16_t can_errs; - uint16_t spi_errs; - uint32_t cnt; - int8_t thrust_out; - int8_t pitch_out; -}; - -struct __attribute__((packed)) AutopilotMessageBethDown { - uint8_t thrust; - uint8_t pitch; - uint32_t errors; - uint32_t cnt; -}; - -/* - * STM Telemetry through wifi - */ -#define TW_BUF_LEN 63 -struct __attribute__((packed)) AutopilotMessageTWUp { - uint8_t tw_len; - uint8_t data[TW_BUF_LEN]; -}; - -struct __attribute__((packed)) AutopilotMessageTWDown { - uint8_t tw_len; - uint8_t data[TW_BUF_LEN]; -}; - -/* - * Passthrough, aka biplan - */ - -struct __attribute__((packed)) ADCMessage { - uint16_t channels[NB_ADC]; -}; - -/* used to indicate parts of the message which actually represent a new measurement */ -struct __attribute__((packed)) PTUpValidFlags { - unsigned rc: 1; - unsigned pressure_absolute: 1; - unsigned pressure_differential: 1; - unsigned vane: 1; - unsigned imu: 1; - unsigned adc: 1; -}; - -struct __attribute__((packed)) AutopilotMessagePTUp { - struct Int32Rates gyro; - struct Int32Vect3 accel; - struct Int32Vect3 mag; - uint32_t imu_tick; - int32_t pressure_absolute; - int32_t pressure_differential; - int16_t rc_pitch; - int16_t rc_roll; - int16_t rc_yaw; - int16_t rc_thrust; - int16_t rc_mode; - int16_t rc_kill; - int16_t rc_gear; - int16_t rc_aux2; - int16_t rc_aux3; - uint8_t rc_status; - float vane_angle1; - float vane_angle2; - struct ADCMessage adc; - - struct PTUpValidFlags valid; - uint32_t stm_msg_cnt; - uint32_t stm_crc_err_cnt; -}; - -struct __attribute__((packed)) AutopilotMessagePTDown { - uint16_t pwm_outputs_usecs[LISA_PWM_OUTPUT_NB]; -}; - - - -#define VI_IMU_DATA_VALID 0 -#define VI_MAG_DATA_VALID 1 -#define VI_GPS_DATA_VALID 2 -#define VI_BARO_ABS_DATA_VALID 3 - -struct __attribute__((packed)) AutopilotMessageVIUp { - struct Int16Rates gyro; - struct Int16Vect3 accel; - struct Int16Vect3 mag; - struct EcefCoor_i ecef_pos; /* pos ECEF in cm */ - struct EcefCoor_i ecef_vel; /* speed ECEF in cm/s */ - int16_t pressure_absolute; /* */ - uint8_t valid_sensors; -}; - -struct __attribute__((packed)) AutopilotMessageVIDown { - -}; - - -/* - * For messages of arbitrary length using fixed DMA - * buffer size. - * A message consists of any amount of packages and - * is recomposed to a raw byte array on application - * level. - * Advantage: Interleaving message exchange, constant - * latency - * Disadvantage: Overhead of message / package counters - * - * If there is no message to be transferred, an empty - * package with message_cnt = 0 is sent. - * The last package of a message has a negative - * package_cntd that indicates the number of padding - * (zero) bytes it contains at the end. - * Example for a 22-byte message transfer with packages - * of 8 bytes for one side: - * - * message_cnt: 0 message_cnt: 4 - * package_cntd: x package_cntd: 6 - * data: uint8_t[8]=0 data: uint8_t[8] - * - * ^- invalid package, ignored - * - * message_cnt: 2 message_cnt: 4 - * package_cntd: 3 package_cnt: 5 - * data: uint8_t[8] data: uint8_t[8] - * - * message_cnt: 2 message_cnt: 4 - * package_cntd: 2 package_cnt: 4 - * data: uint8_t[8] data: uint8_t[8] - * - * message_cnt: 2 message_cnt: 4 - * package_cntd: -5 package_cnt: 3 - * data: uint8_t[8] data: uint8_t[8] - * - * --> last package in message, padding in - * current message is 5 bytes (-5), so - * message length is (3*8)-5 = 22. - * - * -- next message - * - * message_cnt: 3 message_cnt: 4 - * package_cntd: 4 package_cnt: 2 - * data: uint8_t[8] data: uint8_t[8] - * ... - */ -#ifndef SPISTREAM_PACKAGE_SIZE -#define SPISTREAM_PACKAGE_SIZE 32 -#endif -struct __attribute__((packed)) AutopilotMessagePTStream { - uint8_t message_cnt; - int8_t package_cntd; - uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE]; -}; - -/* Union for computing size of SPI transfer (largest of either up or down message) */ -union AutopilotMessage { - struct OVERO_LINK_MSG_UP msg_up; - struct OVERO_LINK_MSG_DOWN msg_down; -}; - -struct __attribute__((packed)) AutopilotMessageCRCFrame { - union AutopilotMessage payload; - uint8_t crc; -}; - -#endif /* FMS_AUTOPILOT_H */ diff --git a/sw/airborne/fms/fms_debug.h b/sw/airborne/fms/fms_debug.h deleted file mode 100644 index 4d7e78f9116..00000000000 --- a/sw/airborne/fms/fms_debug.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef FMS_DEBUG_H -#define FMS_DEBUG_H - -#include - -#define TRACE_DEBUG 1 -#define TRACE_ERROR 2 - -#if 1 -#define TRACE(type,fmt,args...) { \ - fprintf(stderr, fmt, args); \ - } -#else -#define TRACE(type,fmt,args...) {} -#endif - -#endif /* FMS_DEBUG_H */ diff --git a/sw/airborne/fms/fms_gs_com.c b/sw/airborne/fms/fms_gs_com.c deleted file mode 100644 index 94f94eb39f4..00000000000 --- a/sw/airborne/fms/fms_gs_com.c +++ /dev/null @@ -1,89 +0,0 @@ -#include "fms/fms_gs_com.h" - -#include - -#include "udp_transport2.h" - -/* generated : holds PeriodicSendMain */ -#include "generated/periodic_telemetry.h" -/* holds the definitions of PERIODIC_SEND_XXX */ -#include "overo_test_passthrough_telemetry.h" -/* holds the definitions of DOWNLINK_SEND_XXX */ -#include "messages2.h" - -#include "dl_protocol.h" -/* generated : holds DlSetting() and PeriodicSendDlValue() */ -#include "generated/settings.h" - -struct FmsGsCom fms_gs_com; -uint8_t telemetry_mode_Main_DefaultChannel; - -#define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan) - -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg); -static void on_datalink_message(void); - -uint8_t fms_gs_com_init(const char *gs_host, uint16_t gs_port, - uint16_t datalink_port, uint8_t broadcast) -{ - - fms_gs_com.network = network_new(gs_host, gs_port, datalink_port, broadcast); - fms_gs_com.udp_transport = udp_transport_new(fms_gs_com.network); - event_set(&fms_gs_com.datalink_event, fms_gs_com.network->socket_in, EV_READ | EV_PERSIST, - on_datalink_event, fms_gs_com.udp_transport); - event_add(&fms_gs_com.datalink_event, NULL); - - return 0; -} - -void fms_gs_com_periodic(void) -{ - - PeriodicSendMain(fms_gs_com.udp_transport); - - RunOnceEvery(10, {fms_gs_com.udp_transport->Periodic(fms_gs_com.udp_transport->impl);}); - -} - - -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) -{ - char buf[512]; - int bytes_read = read(fd, buf, 512); - uint16_t i = 0; - struct udp_transport *tp = fms_gs_com.udp_transport->impl; - while (i < bytes_read) { - parse_udp_dl(tp, buf[i]); - if (tp->udp_dl_msg_received) { - on_datalink_message(); - tp->udp_dl_msg_received = FALSE; - } - i++; - } - -} - -static void on_datalink_message(void) -{ - - struct udp_transport *tp = fms_gs_com.udp_transport->impl; - uint8_t msg_id = tp->udp_dl_payload[1]; - - switch (msg_id) { - case DL_PING: - DOWNLINK_SEND_PONG(fms_gs_com.udp_transport); - break; - case DL_SETTING : { - uint8_t i = DL_SETTING_index(tp->udp_dl_payload); - float var = DL_SETTING_value(tp->udp_dl_payload); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(fms_gs_com.udp_transport, &i, &var); - } - break; - - default : - break; - } - -} - diff --git a/sw/airborne/fms/fms_gs_com.h b/sw/airborne/fms/fms_gs_com.h deleted file mode 100644 index 168afa3abe7..00000000000 --- a/sw/airborne/fms/fms_gs_com.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * This module handles communications with ground segment - * - * for now one channel of telemetry/datalink via wifi - * - */ - -#ifndef FMS_GS_COM_H -#define FMS_GS_COM_H - -#include -#include "fms_network.h" -#include "downlink_transport.h" - -#include "std.h" - -struct FmsGsCom { - - struct FmsNetwork *network; - struct DownlinkTransport *udp_transport; - struct event datalink_event; - -}; - - -extern struct FmsGsCom fms_gs_com; -/* remove me */ -extern uint8_t telemetry_mode_Main_DefaultChannel; - -extern uint8_t fms_gs_com_init(const char *gs_host, uint16_t gs_port, - uint16_t datalink_port, uint8_t broadcast); -extern void fms_gs_com_periodic(void); - -#endif /* FMS_GS_COM_H */ diff --git a/sw/airborne/fms/fms_network.c b/sw/airborne/fms/fms_network.c deleted file mode 100644 index 2f6a361aa01..00000000000 --- a/sw/airborne/fms/fms_network.c +++ /dev/null @@ -1,120 +0,0 @@ -#include "fms_network.h" - -#include -#include -#include -#include - -#include "fms_debug.h" - -static inline void create_socket(int *my_socket, const int protocol, const int reuse_addr, const int broadcast) -{ - // Create the socket with the correct protocl - *my_socket = socket(PF_INET, SOCK_DGRAM, protocol); - - // Enable reusing of addres (must be exactly 1) - if (reuse_addr == 1) { - setsockopt(*my_socket, SOL_SOCKET, SO_REUSEADDR, &reuse_addr, sizeof(reuse_addr)); - } - - // Enable broadcasting - if (broadcast == 1) { - setsockopt(*my_socket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); - } -} - -struct FmsNetwork *network_new(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) -{ - - struct FmsNetwork *me = malloc(sizeof(struct FmsNetwork)); - - if (port_out >= 0) { - // Create the output socket (enable reuse of the address, and broadcast if necessary) - create_socket(&me->socket_out, 0, 1, broadcast); - - // Setup the output address - me->addr_out.sin_family = PF_INET; - me->addr_out.sin_port = htons(port_out); - me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out); - } - - if (port_in >= 0) { - // Creat the input socket (enable reuse of the address, and disable broadcast) - create_socket(&me->socket_in, 0, 1, 0); - - // Create the input address - me->addr_in.sin_family = PF_INET; - me->addr_in.sin_port = htons(port_in); - me->addr_in.sin_addr.s_addr = htonl(INADDR_ANY); - - bind(me->socket_in, (struct sockaddr *)&me->addr_in, sizeof(me->addr_in)); - } - - return me; -} - -int network_subscribe_multicast(struct FmsNetwork *me, const char *multicast_addr) -{ - // Create the request - struct ip_mreq mreq; - mreq.imr_multiaddr.s_addr = inet_addr(multicast_addr); - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - - // Send the request - return setsockopt(me->socket_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq)); -} - -int network_set_recvbuf(struct FmsNetwork *me, int buf_size) -{ - // Set and check - unsigned int optval_size = 4; - int buf_ret; - setsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_size, optval_size); - getsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_ret, &optval_size); - - if (buf_size != buf_ret) { - return -1; - } - - return 0; -} - -int network_write(struct FmsNetwork *me, char *buf, int len) -{ - // Check if the output address is set - if (!me->socket_out) { - return -1; - } - - ssize_t byte_written = sendto(me->socket_out, buf, len, MSG_DONTWAIT, - (struct sockaddr *)&me->addr_out, sizeof(me->addr_out)); - if (byte_written != len) { - TRACE(TRACE_ERROR, "error sending to network %d (%d)\n", (int)byte_written, errno); - } - return len; -} - -///< returns: -1 = error, 0 = no data, >0 = nrofbytesread - -int network_read(struct FmsNetwork *me, unsigned char *buf, int len) -{ - // Check if the input address is set - if (!me->socket_in) { - return -1; - } - - socklen_t slen = sizeof(struct sockaddr_in); - // MSG_DONTWAIT => nonblocking flag - ssize_t byte_read = recvfrom(me->socket_in, buf, len, MSG_DONTWAIT, - (struct sockaddr *)&me->addr_in, &slen); - - if (byte_read == -1) { - if (errno == EWOULDBLOCK) { // If not data is available, simply return zero bytes read, no error - return 0; - } else { - TRACE(TRACE_ERROR, "error reading from network error %d \n", errno); - } - } - - return byte_read; -} diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h deleted file mode 100644 index ce4c8d5a408..00000000000 --- a/sw/airborne/fms/fms_network.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef FMS_NETWORK_H -#define FMS_NETWORK_H - -#include -#include - -#define FMS_UNICAST 0 -#define FMS_BROADCAST 1 - -struct FmsNetwork { - int socket_in; - int socket_out; - struct sockaddr_in addr_in; - struct sockaddr_in addr_out; -}; - - -extern struct FmsNetwork *network_new(const char *str_ip_out, const int port_out, const int port_in, - const int broadcast); -extern int network_subscribe_multicast(struct FmsNetwork *me, const char *multicast_addr); -extern int network_set_recvbuf(struct FmsNetwork *me, int buf_size); -extern int network_write(struct FmsNetwork *me, char *buf, int len); -extern int network_read(struct FmsNetwork *me, unsigned char *buf, int len); - -#endif /* FMS_NETWORK_H */ diff --git a/sw/airborne/fms/fms_periodic.c b/sw/airborne/fms/fms_periodic.c deleted file mode 100644 index 39be84a8050..00000000000 --- a/sw/airborne/fms/fms_periodic.c +++ /dev/null @@ -1,110 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * 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 "fms_periodic.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "fms_debug.h" - -static void fms_periodic_run(void); -//static void fms_periodic_delete(void); - -struct FmsPeriodic { - pid_t ap_pid; -}; - - -int fms_periodic_init(void(*periodic_handler)(int)) -{ - - pid_t my_pid = fork(); - if (my_pid == -1) { - TRACE(TRACE_ERROR, "fms_periodic : unable to fork : %s (%d)\n", strerror(errno), errno); - return -1; - } - /* child process */ - else if (my_pid == 0) { - fms_periodic_run(); - } - /* succesful fork parent process */ - - /* install signal handler */ - struct sigaction my_sigaction = {.sa_handler = periodic_handler }; - if (sigaction(SIGUSR1, &my_sigaction, NULL)) { - TRACE(TRACE_ERROR, "fms_periodic : unable to install signal handler : %s (%d)\n", strerror(errno), errno); - return -1; - } - - /* set main process priority */ - struct sched_param param; - param.sched_priority = 49; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - TRACE(TRACE_ERROR, "fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); - } - - return 0; -} - -//static void fms_periodic_delete(void) { -// -//} - - -#define NS_PER_SEC 1000000000 -#define PERIODIC_DT_NSEC (NS_PER_SEC/(FMS_PERIODIC_FREQ)) - -static void fms_periodic_run(void) -{ - - struct sched_param param; - param.sched_priority = 95; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - TRACE(TRACE_ERROR, "fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); - } - - pid_t father_pid = getppid(); - - struct timespec periodic_next; - clock_gettime(CLOCK_MONOTONIC, &periodic_next); - - while (1) { - periodic_next.tv_nsec += PERIODIC_DT_NSEC; - while (periodic_next.tv_nsec > NS_PER_SEC) { - periodic_next.tv_nsec -= NS_PER_SEC; - periodic_next.tv_sec++; - } - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &periodic_next, NULL); - kill(father_pid, SIGUSR1); - } - - _exit(EXIT_SUCCESS); - -} diff --git a/sw/airborne/fms/fms_periodic.h b/sw/airborne/fms/fms_periodic.h deleted file mode 100644 index f2ae59dfb3d..00000000000 --- a/sw/airborne/fms/fms_periodic.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright (C) 2010 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - - -#ifndef FMS_PERIODIC_H -#define FMS_PERIODIC_H - -extern int fms_periodic_init(void(*periodic_handler)(int)); - -#endif /* FMS_PERIODIC_H */ diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c deleted file mode 100644 index c2c26622db5..00000000000 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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 -#include -#include -#include -#include - - -#include "fms_debug.h" -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" -#include "fms_spi_autopilot_msg.h" - -/* all these for telemetry */ -#include "messages2.h" -#include "downlink_transport.h" -#include "udp_transport2.h" -#include "fms/fms_network.h" -#include "ready_main.h" - -#include "generated/airframe.h" -/* sort of a hack, we're not really fixed wing here but we need their declarations */ -#include "firmwares/fixedwing/actuators.h" -#include "rdyb_booz_imu.h" -#include "subsystems/radio_control.h" -#include "rdyb_mahrs.h" - -static struct ImuFloat imuFloat; -static struct FloatQuat body_to_imu_quat = IMU_POSE_BODY_TO_IMU_QUAT; - -static void (* vane_callback)(uint8_t vane_id, float alpha, float beta) = NULL; -static void (* pressure_absolute_callback)(uint8_t pressure_id, uint32_t pressure) = NULL; -static void (* pressure_differential_callback)(uint8_t pressure_id, uint32_t pressure) = NULL; -static void (* radio_control_callback)(void) = NULL; -static void (* adc_callback)(uint16_t *adc_channels) = NULL; - -void spi_ap_link_downlink_send(struct DownlinkTransport *tp) -{ - uint32_t timestamp = 0; - DOWNLINK_SEND_EKF7_Y(tp, ×tamp, &imuFloat.accel.x, &imuFloat.accel.y, &imuFloat.accel.z, - &imuFloat.mag.x, &imuFloat.mag.y, &imuFloat.mag.z, - &imuFloat.gyro.p, &imuFloat.gyro.q, &imuFloat.gyro.r); -} - -void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float alpha, float beta)) -{ - vane_callback = vane_cb; -} - -void spi_ap_link_set_pressure_absolute_callback(void (* pressure_absolute_cb)(uint8_t pressure_id, uint32_t pressure)) -{ - pressure_absolute_callback = pressure_absolute_cb; -} - -void spi_ap_link_set_pressure_differential_callback(void (* pressure_differential_cb)(uint8_t pressure_id, - uint32_t pressure)) -{ - pressure_differential_callback = pressure_differential_cb; -} - -void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void)) -{ - radio_control_callback = radio_control_cb; -} - -void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t *adc_channels)) -{ - adc_callback = adc_callback_fun; -} - -int spi_ap_link_init() -{ - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return -1; - } - - // Initialize IMU->Body orientation - imuFloat.body_to_imu_quat = body_to_imu_quat; - imuFloat.sample_count = 0; - -#ifdef IMU_ALIGN_BENCH - // This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame) - struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 }; - FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); -#endif - - FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); - FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); - FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); - - struct FloatRates bias0 = { 0., 0., 0.}; - rdyb_mahrs_init(imuFloat.body_to_imu_quat, bias0); - - return 0; -} - -static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) -{ - - if (msg_up->valid.vane && vane_callback) { - vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2); - } - - // Fill pressure data - if (msg_up->valid.pressure_absolute && pressure_absolute_callback) { - pressure_absolute_callback(0, msg_up->pressure_absolute); - } - - if (msg_up->valid.pressure_differential && pressure_differential_callback) { - pressure_differential_callback(0, (32768 + msg_up->pressure_differential)); - } - - if (msg_up->valid.adc) { - if (adc_callback) { - adc_callback(msg_up->adc.channels); - } - } - - // Fill radio data - if (msg_up->valid.rc && radio_control_callback) { - radio_control.values[RADIO_ROLL] = msg_up->rc_roll; - radio_control.values[RADIO_PITCH] = msg_up->rc_pitch; - radio_control.values[RADIO_YAW] = msg_up->rc_yaw; - radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust; - radio_control.values[RADIO_MODE] = msg_up->rc_mode; - radio_control.values[RADIO_KILL] = msg_up->rc_kill; - radio_control.values[RADIO_GEAR] = msg_up->rc_gear; - radio_control.values[RADIO_AUX2] = msg_up->rc_aux2; - radio_control.values[RADIO_AUX3] = msg_up->rc_aux3; - radio_control_callback(); - } - // always fill status, it may change even when in the case when there is no new data - radio_control.status = msg_up->rc_status; - - // Fill IMU data - imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p); - imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q); - imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r); - - imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x); - imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y); - imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z); - - imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x); - imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y); - imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z); - - imuFloat.sample_count = msg_up->imu_tick; - - if (msg_up->valid.imu) { - rdyb_booz_imu_update(&imuFloat); - } -} - -static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_down) -{ - for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) { - msg_down->pwm_outputs_usecs[i] = actuators[i]; - } -} - -void spi_ap_link_periodic() -{ - static struct AutopilotMessageCRCFrame msg_up; - static struct AutopilotMessageCRCFrame msg_down; - uint8_t crc_valid; - - passthrough_down_fill(&msg_down.payload.msg_down); - - // SPI transcieve - spi_link_send(&msg_down, sizeof(struct AutopilotMessageCRCFrame), &msg_up, &crc_valid); - - if (crc_valid) { - passthrough_up_parse(&msg_up.payload.msg_up); - } -} diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.h b/sw/airborne/fms/fms_spi_autopilot_msg.h deleted file mode 100644 index 6ca32b82a2b..00000000000 --- a/sw/airborne/fms/fms_spi_autopilot_msg.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -#ifndef FMS_SPI_AUTOPILOT_MSG_H -#define FMS_SPI_AUTOPILOT_MSG_H -#include "downlink_transport.h" - -int spi_ap_link_init(void); -void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float alpha, float beta)); -void spi_ap_link_set_pressure_absolute_callback(void (* pressure_cb)(uint8_t pressure_id, uint32_t pressure)); -void spi_ap_link_set_pressure_differential_callback(void (* pressure_cb)(uint8_t pressure_id, uint32_t pressure)); -void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void)); -void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t *adc_channels)); -void spi_ap_link_periodic(void); -void spi_ap_link_downlink_send(struct DownlinkTransport *tp); - -#ifdef USE_SPI_LINK -#define PERIODIC_SEND_EKF7_Y(_chan) spi_ap_link_downlink_send(_chan); -#endif - -#endif // FMS_SPI_AUTOPILOT_MSG_H diff --git a/sw/airborne/fms/fms_spi_link.c b/sw/airborne/fms/fms_spi_link.c deleted file mode 100644 index e0c20a79350..00000000000 --- a/sw/airborne/fms/fms_spi_link.c +++ /dev/null @@ -1,144 +0,0 @@ -#include "fms_spi_link.h" - -#include -#include -#include -#include -#include -#include -#include -#include - - -int spi_link_init(void) -{ - - spi_link.device = "/dev/spidev1.1"; - spi_link.mode = SPI_CPHA; - spi_link.bits = 8; - spi_link.speed = 12000000; - spi_link.delay = 1; - - spi_link.msg_cnt = 0; - spi_link.crc_err_cnt = 0; - - spi_link.fd = open(spi_link.device, O_RDWR); - if (spi_link.fd < 0) { - return -1; - } - - int ret = 0; - ret = ioctl(spi_link.fd, SPI_IOC_WR_MODE, &spi_link.mode); - if (ret == -1) { - return -2; - } - - ret = ioctl(spi_link.fd, SPI_IOC_WR_BITS_PER_WORD, &spi_link.bits); - if (ret == -1) { - return -3; - } - - ret = ioctl(spi_link.fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_link.speed); - if (ret == -1) { - return -4; - } - - return 0; -} - -int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t *crc_valid) -{ - - int ret; - - struct spi_ioc_transfer tr = { - .tx_buf = (unsigned long)buf_out, - .rx_buf = (unsigned long)buf_in, - .len = count, - .delay_usecs = spi_link.delay, - .speed_hz = spi_link.speed, - .bits_per_word = spi_link.bits, - }; - - ((uint8_t *)buf_out)[count - 1] = crc_calc_block_crc8(buf_out, count - 1); - ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr); - spi_link.msg_cnt++; - - uint8_t computed_crc = crc_calc_block_crc8(buf_in, count - 1); - if (computed_crc == ((uint8_t *)buf_in)[count - 1]) { - *crc_valid = 1; - } else { - *crc_valid = 0; - spi_link.crc_err_cnt++; - } - - return ret; - -} - - -#define POLYNOMIAL 0x31 -#define WIDTH (8 * sizeof(uint8_t)) -#define TOPBIT (1 << (WIDTH - 1)) -uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) -{ - uint8_t _remainder = 0; - for (uint32_t byte = 0; byte < len; ++byte) { - _remainder ^= (buf[byte] << (WIDTH - 8)); - for (uint8_t bit = 8; bit > 0; --bit) { - if (_remainder & TOPBIT) { - _remainder = (_remainder << 1) ^ POLYNOMIAL; - } else { - _remainder = (_remainder << 1); - } - } - } - return (_remainder); -} - - -#if 0 -/* for reference: need to write a more efficient crc computation */ -crc_t crc__table[256]; - -void crc__init(uint32_t polynomial) -{ - crc_t crc_remainder; - uint32_t crc_dividend; - crc_t top_bit = (1 << (CRC__WIDTH - 1)); - uint8_t bit; - for (crc_dividend = 0; crc_dividend < 256; crc_dividend++) { - crc_remainder = crc_dividend << (CRC__WIDTH - 8); - for (bit = 8; bit > 0; bit--) { - if (crc_remainder & top_bit) { - crc_remainder = (crc_remainder << 1) ^ polynomial; - } else { - crc_remainder = (crc_remainder << 1); - } - } - crc__table[crc_dividend] = crc_remainder; - } - -#if 0 - int i = 0; - while (i < 256) { - printf("%03d ", crc__table[i]); - if ((i % 8) == 7) { printf("\n"); } - i++; - } -#endif - -} - -uint8_t crc__calc_block_crc8(const uint8_t buffer[], uint32_t buffer_length) -{ - int counter; - uint16_t crc = 0; - for (counter = 0; counter < buffer_length; counter++) { - crc = crc ^ crc__table[(crc ^ * (char *)(buffer)++) & 0x00FF ]; - } - return crc; -} - - -#endif diff --git a/sw/airborne/fms/fms_spi_link.h b/sw/airborne/fms/fms_spi_link.h deleted file mode 100644 index eab2a3d234c..00000000000 --- a/sw/airborne/fms/fms_spi_link.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright (C) 2009-20010 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. - * - */ - -#ifndef FMS_SPI_LINK_H -#define FMS_SPI_LINK_H - -#include -#include - -struct SpiLink { - int fd; - char *device; - uint8_t mode; - uint8_t bits; - uint32_t speed; - uint16_t delay; - /* number of message exchanged since initialization */ - uint32_t msg_cnt; - /* number of crc errors on received messages */ - uint32_t crc_err_cnt; -}; - -struct SpiLink spi_link; - -/* - * initialize peripheral - */ -extern int spi_link_init(void); - -/* - * exchange a data buffer - * the last byte of buf_out will be overwiten with a crc - * the last byte of buf_in will contain the received crc - * count is the size of buf_out and buf_in, that is - * the count of data to exchange+1 - */ -extern int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t *crc_valid); - -/* - * just for debuging purposes - */ -extern uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len); - - -#endif /* FMS_SPI_LINK_H */ diff --git a/sw/airborne/fms/fms_spistream.h b/sw/airborne/fms/fms_spistream.h deleted file mode 100644 index 22a98964ee7..00000000000 --- a/sw/airborne/fms/fms_spistream.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef FMS_SPISTREAM_H__ -#define FMS_SPISTREAM_H__ - -#define CLIENT_SOCKET_PATH "./spistreamc.socket" -#define DAEMON_SOCKET_PATH "./spistreamd.socket" - -#define min(a,b) ((a>b)? (b) : (a)) - -void print_message(char prefix[], uint8_t msg_id, uint8_t *data, uint16_t num_bytes); -void print_message(char prefix[], uint8_t msg_id, uint8_t *data, uint16_t num_bytes) -{ - /* - struct tm * timeinfo; - time_t c_time; - char time_str[30]; - */ - uint8_t cnt; - uint8_t log_bytes = num_bytes; - if (log_bytes > 16) { log_bytes = 16; } - /* - time(&c_time); - timeinfo = localtime(&c_time); - strftime(time_str, 30, " %X ", timeinfo); - - printf("%s %s bytes: %3d | id: %3d | UART%d | ", - prefix, time_str, num_bytes, msg_id, data[0]); - */ - printf("%s bytes: %3d | id: %3d | UART%d | ", - prefix, num_bytes, msg_id, data[0]); - for (cnt = 1; cnt < log_bytes; cnt++) { - printf("%02X ", data[cnt]); - if (cnt >= 24 && cnt % 24 == 0 && cnt + 1 < log_bytes) { - printf("\n "); - } - } - printf("\n"); -} - -#endif - diff --git a/sw/airborne/fms/fms_spistream_client.c b/sw/airborne/fms/fms_spistream_client.c deleted file mode 100644 index 5b572981fcc..00000000000 --- a/sw/airborne/fms/fms_spistream_client.c +++ /dev/null @@ -1,226 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include -#include - -// Socket stuff -#include -#include -#include -#include -#include -#include -// End socket stuff - -#include - -#include "std.h" -#include "fms_debug.h" -#include "fms_periodic.h" - -/* stuff for io processor link */ -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -#define OVERO_ENV -#include "lisa/lisa_spistream.h" -#include "fms_spistream.h" - - -static void parse_command_line(int argc, char **argv); -static void main_init(void); -static void main_exit(void); -static void main_periodic(int my_sig_num); - -static int open_stream(void); - -static void on_kill(int signum); - -static int dfifo[4]; -static int cfifo[4]; -static char dfifo_files[4][40]; -static char cfifo_files[4][40]; - - - -int main(int argc, char *argv[]) -{ - - parse_command_line(argc, argv); - - main_init(); - TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); - - /* Enter our mainloop */ - event_dispatch(); - while (1) { - sleep(100); - } - - main_exit(); - - TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); - - return 0; -} - -static void main_periodic(int my_sig_num) -{ - uint8_t fifo_idx; - uint8_t msg_id; - uint16_t num_bytes; - int16_t ret; - static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH * 10]; - - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - // The periodic is triggered before fifo - // connections have been initialized, so - // check for a valid fd first: - if (dfifo[fifo_idx] > 0) { - ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2); - ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1); - - memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH); - if (num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { - fprintf(stderr, "Warning: Message has length %d, but limit " - "is %d\n", - num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH); - num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH; - } - ret = read(dfifo[fifo_idx], &buf, num_bytes); - if (ret > 0 && ret == num_bytes) { - // Message received - print_message(">> Client", msg_id, buf, num_bytes); - } else if (ret > 0 && ret < num_bytes) { - fprintf(stderr, "Tried to read %d bytes, but only got %d\n", - num_bytes, ret); - } - } else { - // FIFO file descriptor is invalid, - // retry to open it: - dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); - } - } - -} - -static void main_init(void) -{ - - TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); - - /* Initalize the event library */ - event_init(); - - /* Initalize our ô so accurate periodic timer */ - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return; - } - - signal(SIGKILL, on_kill); - signal(SIGINT, on_kill); - signal(SIGILL, on_kill); - signal(SIGHUP, on_kill); - signal(SIGQUIT, on_kill); - signal(SIGTERM, on_kill); - signal(SIGSEGV, on_kill); - - if (!open_stream()) { - fprintf(stderr, "Could not open stream, sorry\n"); - exit(1); - } - - TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); -} - -/** - * For every FIFO, a non-blocking connection try is called - * via open(..., O_NONBLOCK). - * This immediately returns a file descriptor or 0 if - * the other end of the fifo is closed. - * In the transmission, we check the FIFO file descriptors - * and retry to open them, in case they are invalid. - * You can also just open() them without O_NONBLOCK in - * the client app, but the daemon should be running before - * starting the client then, otherwise open() would block. - * - * When using this strategy, we get connection - * recovery for free when either daemon or client die. - * - * After the connections are established, you can use them - * for read() and write(), as well as register an event - * trigger on them, like select() or libevent. - * - */ -static int open_stream(void) -{ - uint8_t fifo_idx; - - strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data - strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client) - strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); - strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo"); - strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands - strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM) - strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); - strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo"); - - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - fprintf(stderr, "Open data stream %s ... \n", dfifo_files[fifo_idx]); - dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); - fprintf(stderr, " ...\n"); - } - - return 1; - - for (fifo_idx = 0; fifo_idx < 3; fifo_idx++) { - fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]); - cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY); - if (cfifo[fifo_idx] < 0) { - fprintf(stderr, " failed\n"); - return 0; - } - } - return 1; -} - -static void main_exit(void) -{ - fprintf(stderr, "Bye!\n"); -} - -static void parse_command_line(int argc, char **argv) -{ -} - -static void on_kill(int signum) -{ - fprintf(stderr, "Exiting, got signal %d\n", signum); - main_exit(); - exit(1); -} diff --git a/sw/airborne/fms/fms_spistream_daemon.c b/sw/airborne/fms/fms_spistream_daemon.c deleted file mode 100644 index f4792efc99a..00000000000 --- a/sw/airborne/fms/fms_spistream_daemon.c +++ /dev/null @@ -1,350 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include -#include - -// Socket stuff -#include -#include -#include -#include -#include -#include -// End socket stuff - -#include - -#include "std.h" -#include "fms_debug.h" -#include "fms_periodic.h" - -/* stuff for io processor link */ -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -/* stuff for telemetry/datalink */ - -#define OVERO_ENV -#include "lisa/lisa_spistream.h" -#include "fms_spistream.h" - -#define LOG_OUT stdout - - -static void parse_command_line(int argc, char **argv); -static void main_init(void); -static void main_exit(void); -static void main_periodic(int my_sig_num); -static void spistream_event(void); - -static int open_stream(void); -static void close_stream(void); - -static void on_timeout(int signum); -static void on_kill(int signum); -static void on_dead_pipe(int signum); - -static void on_spistream_msg_received(uint8_t msg_id, uint8_t *data, uint16_t num_bytes); -static void on_spistream_msg_sent(uint8_t msg_id); - -static void send_to_client(uint8_t *data, uint16_t num_bytes, uint8_t fifo_idx); - -static uint8_t spistream_msg[123]; - -static int dfifo[4]; -static int cfifo[4]; -static char dfifo_files[4][40]; -static char cfifo_files[4][40]; - - - -int main(int argc, char *argv[]) -{ - - parse_command_line(argc, argv); - - main_init(); - TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); - - /* Enter our mainloop */ - event_dispatch(); - while (1) { - sleep(100); - } - - main_exit(); - - TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); - - return 0; -} - -static void main_periodic(int my_sig_num) -{ - - static int32_t every_100 = 1000; - if (every_100-- == 0) { - every_100 = 1000; - spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT); - } - - spistream_event(); -} - -static void spistream_event() -{ - - static struct AutopilotMessagePTStream msg_in; - static struct AutopilotMessagePTStream msg_out; - static uint8_t crc_valid; - - spistream_read_pkg(&msg_in); - /* - uint8_t cnt; - static uint8_t pkg_size = sizeof(msg_in.pkg_data); - if(msg_out.message_cnt != 0) { - printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", - pkg_size, msg_out.message_cnt, msg_out.package_cntd); - for(cnt = 0; cnt < pkg_size; cnt++) { - printf("%3d ", msg_out.pkg_data[cnt]); - } - printf("\n"); - } - */ - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); - /* - if(msg_in.message_cnt != 0) { - printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE, - pkg_size, msg_in.message_cnt, msg_in.package_cntd); - for(cnt = 0; cnt < pkg_size; cnt++) { - printf("%02X ", msg_in.pkg_data[cnt]); - } - printf("\n"); - } - */ - spistream_write_pkg(&msg_out); -} - -static void on_spistream_msg_received(uint8_t msg_id, - uint8_t *data, - uint16_t num_bytes) -{ - uint8_t uart; - uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH + 3]; - - print_message("<< Daemon", msg_id, data, num_bytes); - - uart = data[0]; - // Check for valid uart ID - if (uart >= 0 && uart <= 3) { - if (msg_id > 0) { - buf[0] = (uint8_t)(num_bytes & 0x00ff); - buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff); - buf[2] = msg_id; - if (num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { - fprintf(LOG_OUT, "Warning: Message has length %d, but limit " - "is %d - truncating message\n", - num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH); - num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH; - } - memcpy(buf + 3, data, num_bytes); - send_to_client(buf, num_bytes + 3, uart); - } - } -} - -static void send_to_client(uint8_t *data, uint16_t num_bytes, uint8_t fifo_idx) -{ - if (dfifo[fifo_idx] <= 0) { - // No client connected to this fifo, yet - dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK); - if (dfifo[fifo_idx] <= 0) { - fprintf(LOG_OUT, "No client for data fifo %d (%s)\n", - fifo_idx, dfifo_files[fifo_idx]); - return; - } - } else { - // Client connected to this fifo - if (write(dfifo[fifo_idx], data, num_bytes) == -1) { - fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx); - } - } -} - -static void on_spistream_msg_sent(uint8_t msg_id) -{ -// TRACE(TRACE_DEBUG, "%s", "SPI message sent \n"); -} - -static void main_init(void) -{ - uint8_t byte_idx; - - TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); - - /* Initalize our SPI link to IO processor */ - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return; - } - - spistream_init(&on_spistream_msg_received, - &on_spistream_msg_sent); - - for (byte_idx = 1; byte_idx < 123; byte_idx++) { - spistream_msg[byte_idx] = byte_idx; - } - /* Initalize the event library */ - event_init(); - - /* Initalize our ô so accurate periodic timer */ - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return; - } - - signal(SIGKILL, on_kill); - signal(SIGINT, on_kill); - signal(SIGILL, on_kill); - signal(SIGHUP, on_kill); - signal(SIGQUIT, on_kill); - signal(SIGTERM, on_kill); - signal(SIGSEGV, on_kill); - signal(SIGPIPE, on_dead_pipe); - - if (!open_stream()) { - fprintf(LOG_OUT, "Could not open stream, sorry\n"); - exit(1); - } - - TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); -} - -static void main_exit(void) -{ - fprintf(LOG_OUT, "Closing socket\n"); - close_stream(); -} - -static void parse_command_line(int argc, char **argv) -{ - /* - while ((ch = getopt(argc, argv, "d:")) != -1) { - switch (ch) { - case 'd': - daemon_mode = 1; - break; - } - } - */ -} - -static int open_stream(void) -{ - uint8_t fifo_idx; - int ret; - - strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data - strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client) - strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); - strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo"); - strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands - strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM) - strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); - strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo"); - - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]); - if ((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) { - fprintf(LOG_OUT, " failed\n"); - fprintf(LOG_OUT, "Could not create data fifo %d: %s\n", - fifo_idx, dfifo_files[fifo_idx]); - close_stream(); - return 0; - } else { - fprintf(LOG_OUT, " ok\n"); - dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK); - } - } - - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]); - if ((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) { - fprintf(LOG_OUT, " failed\n"); - fprintf(LOG_OUT, "Could not create command fifo %d: %s\n", - fifo_idx, cfifo_files[fifo_idx]); - close_stream(); - return 0; - } else { - fprintf(LOG_OUT, " ok\n"); - cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); - } - } - return 1; -} - -static void close_stream(void) -{ - uint8_t fifo_idx; - fprintf(LOG_OUT, "Closing streams\n"); - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - if (dfifo[fifo_idx] >= 0) { - close(dfifo[fifo_idx]); - } - unlink(dfifo_files[fifo_idx]); - if (cfifo[fifo_idx] >= 0) { - close(cfifo[fifo_idx]); - } - unlink(cfifo_files[fifo_idx]); - } -} - -static void on_timeout(int signum) -{ - fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n"); - exit(6); -} - -static void on_kill(int signum) -{ - fprintf(LOG_OUT, "Exiting, got signal %d\n", signum); - main_exit(); - exit(1); -} - -static void on_dead_pipe(int signum) -{ - uint8_t fifo_idx; - fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum); - // *Pop* goes the pipe. Looks like our client got AWOL. - // Let's be nice and invalidate the file descriptors: - for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { - close(dfifo[fifo_idx]); - dfifo[fifo_idx] = -1; - } -} - diff --git a/sw/airborne/fms/fms_test_datalink.c b/sw/airborne/fms/fms_test_datalink.c deleted file mode 100644 index eccbf2ff5d7..00000000000 --- a/sw/airborne/fms/fms_test_datalink.c +++ /dev/null @@ -1,116 +0,0 @@ -#include -#include -#include -#include -#include - -#include - -#define LINK_HOST "127.0.0.1" -#define LINK_PORT 4242 -#define DATALINK_PORT 4243 -#include "udp_transport.h" -#include "fms_network.h" -#include "subsystems/datalink/downlink.h" - -#define PERIODIC_SEC 0 -#define PERIODIC_USEC 50000 -#define PERIODIC_NSEC (PERIODIC_USEC * 1000) -#define ONE_SEC_NS 1000000000 -static struct event periodic_event; -static struct event datalink_event; -static struct timespec periodic_date; - -static struct FmsNetwork *network; - -#define PERIODIC_START() { \ - clock_gettime(CLOCK_MONOTONIC, &periodic_date); \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = PERIODIC_SEC; \ - tv.tv_usec = PERIODIC_USEC; \ - event_add(&periodic_event, &tv); \ - } - -#define PERIODIC_RESCHEDULE() { \ - periodic_date.tv_nsec += PERIODIC_NSEC; \ - if (periodic_date.tv_nsec>= ONE_SEC_NS) { \ - periodic_date.tv_nsec -= ONE_SEC_NS; \ - periodic_date.tv_sec++; \ - } \ - struct timespec time_now; \ - clock_gettime(CLOCK_MONOTONIC, &time_now); \ - int32_t dt_ns = (int32_t)periodic_date.tv_nsec - (int32_t)time_now.tv_nsec; \ - if (time_now.tv_sec != periodic_date.tv_sec) \ - dt_ns += ONE_SEC_NS; \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = PERIODIC_SEC; \ - tv.tv_usec = dt_ns / 1000; \ - event_add(&periodic_event, &tv); \ - } - - -static void periodic_task(int fd, short event, void *arg) -{ - - DOWNLINK_SEND_ALIVE(16, MD5SUM); - - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - DOWNLINK_SEND_FMS_TIME(&ts.tv_sec, &ts.tv_nsec); - - PERIODIC_RESCHEDULE(); - -} - - -static void on_datalink_event(int fd, short event, void *arg) -{ - char buf[255]; - int bytes_read; - bytes_read = read(fd, buf, sizeof(buf) - 1); - if (bytes_read == -1) { - perror("read"); - return; - } else if (bytes_read == 0) { - fprintf(stderr, "Connection closed\n"); - return; - } - printf("on_datalink_event, read %d\n", bytes_read); - - struct event *ev = arg; - event_add(ev, NULL); -} - - -int main(int argc , char **argv) -{ - - /* Set real time priority */ - struct sched_param param; - param.sched_priority = 90; - if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - // TRACE(TRACE_ERROR, "sched_setscheduler failed"); - perror("sched_setscheduler failed"); - exit(EXIT_FAILURE); - } - - /* Initalize event library */ - event_init(); - - /* Add our periodic event */ - evtimer_set(&periodic_event, periodic_task, &periodic_event); - PERIODIC_START(); - - /* Add our datalink event */ - network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_UNICAST); - event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, &datalink_event); - event_add(&datalink_event, NULL); - - event_dispatch(); - - return 0; -} - - diff --git a/sw/airborne/fms/lpc_test_spi.c b/sw/airborne/fms/lpc_test_spi.c deleted file mode 100644 index 26a4216b157..00000000000 --- a/sw/airborne/fms/lpc_test_spi.c +++ /dev/null @@ -1,133 +0,0 @@ -/* - * Copyright (C) 2008-2009 Antoine Drouin - * - * 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 - -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "armVIC.h" -#include "LPC21xx.h" - -static inline void main_init(void); -static inline void main_periodic_task(void); -static inline void main_event_task(void); - - -static inline void main_spi_init(void); -static void SPI0_ISR(void) __attribute__((naked)); - - -int main(void) -{ - main_init(); - while (1) { - if (sys_time_check_and_ack_timer(0)) { - main_periodic_task(); - } - main_event_task(); - } - return 0; -} - -static inline void main_init(void) -{ - mcu_init(); - sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); - main_spi_init(); - mcu_int_enable(); -} - -static inline void main_periodic_task(void) -{ - -} - -static inline void main_event_task(void) -{ - -} - -#define PINSEL0_SCK (1<<8) -#define PINSEL0_MISO (1<<10) -#define PINSEL0_MOSI (1<<12) -#define PINSEL0_SSEL (1<<14) - -#define S0SPCR_bit_enable (0<<2) /* 8 bits */ -#define S0SPCR_CPHA (0<<3) /* sample on first edge */ -#define S0SPCR_CPOL (0<<4) /* clock idles low */ -#define S0SPCR_MSTR (0<<5) /* slave mode */ -#define S0SPCR_LSBF (0<<6) /* lsb first */ -#define S0SPCR_SPIE (1<<7) /* interrupt enable */ - -#define S0SPCR_LSF_VAL (S0SPCR_bit_enable | S0SPCR_CPHA | \ - S0SPCR_CPOL | S0SPCR_MSTR | \ - S0SPCR_LSBF | S0SPCR_SPIE); - -#define CPSDVSR 64 - -/* S0SPR bits */ -#define ROVR 5 -#define WCOL 6 -#define SPIF 7 -/* S0SPCR bits */ -#define SPIE 7 - - -static inline void main_spi_init(void) -{ - /* setup pins for SPI0 (SCK, MISO, MOSI, SS) */ - PINSEL0 |= PINSEL0_SCK | PINSEL0_MISO | PINSEL0_MOSI | PINSEL0_SSEL; - - S0SPCR = S0SPCR_LSF_VAL; - S0SPCCR = CPSDVSR; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI1 selected as IRQ - VICIntEnable = VIC_BIT(VIC_SPI0); // SPI1 interrupt enabled - VICVectCntl7 = VIC_ENABLE | VIC_SPI0; - VICVectAddr7 = (uint32_t)SPI0_ISR; // address of the ISR - - SetBit(S0SPCR, SPIE); - -} - - -static void SPI0_ISR(void) -{ - ISR_ENTRY(); - - static uint8_t cnt = 0; - LED_TOGGLE(1); - - if (bit_is_set(S0SPSR, SPIF)) { /* transfer complete */ - uint8_t foo = S0SPDR; - S0SPDR = cnt; - cnt++; - } - - /* clear_it */ - S0SPINT = 1 << SPI0IF; - - - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} diff --git a/sw/airborne/fms/onboard_logger.c b/sw/airborne/fms/onboard_logger.c deleted file mode 100644 index 9f749d958d3..00000000000 --- a/sw/airborne/fms/onboard_logger.c +++ /dev/null @@ -1,144 +0,0 @@ -#include -#include -#include - -#include "packet_header.h" - -static const char filter_exp[] = "dst port 4242 && udp"; - -#define MIN_MSG_LENGTH 3 - -static void got_pprz_message(const u_char *buf, const struct timeval *ts) -{ - int i = 0; - u_char length; - u_char ck_A = 0, ck_B = 0; - static unsigned int start_secs = 0; - - if (start_secs == 0) { - start_secs = ts->tv_sec; - } - - length = buf[i]; - - for (i = 0; i < length - 3; i++) { - ck_A += buf[i]; - ck_B += ck_A; - } - - if (ck_A != buf[length - 3] || ck_B != buf[length - 2]) { - printf("checksum mismatch\n"); - return; - } - // printf("Got pprz msg len %i, ckA %02x, ckB %02x (%02x %02x)\n", length, ck_A, ck_B, buf[length - 3], buf[length - 2]); - printf("%i.%06i ", (unsigned) ts->tv_sec - start_secs, (unsigned) ts->tv_usec); - printf("%d ", (uint32_t) buf[1]); // paparazzi timestamp; see udp_transport.h - printf("%i %i ", buf[5], buf[6]); // AC_ID MSG_ID - for (i = 6; i < length - 3; i++) { - printf("%02x ", buf[i]); - } - - printf("\n"); -} - -static void got_packet(u_char *args, const struct pcap_pkthdr *header, - const u_char *packet) -{ - const u_char *payload; - const struct ethernet_header *ethernet; - const struct ip_header *ip; - const struct udp_header *udp; - unsigned int msg_length; - - u_short udp_length; - u_int size_ip; - int i; - - ethernet = (struct ethernet_header *) packet; - ip = (struct ip_header *)(packet + ETHERNET_HEADER_LENGTH); - size_ip = IP_HL_WORDS(ip) * 4; - if (size_ip < 20) { - printf("invalid IP hdr length: %u bytes\n", size_ip); - return; - } - - udp = (struct udp_header *)((u_char *)ip + size_ip); - - payload = (u_char *)((u_char *)udp + sizeof(struct udp_header)); - - udp_length = htons(udp->uh_len); - //printf ("Got udp packet length %i\n", udp_length); - //for ( i = 0; i < udp_length; i++) - //printf ("[%02i] \n", payload[i]); - - i = 0; - while (i < udp_length) { - if (payload[i] != 0x98) { - // printf("missing start byte\n"); - break; - } - i++; - msg_length = payload[i]; - if ((i + msg_length <= udp_length) && msg_length >= MIN_MSG_LENGTH) { - got_pprz_message(payload + i, &header->ts); - i += payload[i] - 1; - } - } -} - -static int init_capture(char *device, pcap_t **handle) -{ - char errbuf[PCAP_ERRBUF_SIZE]; - - bpf_u_int32 mask; - bpf_u_int32 net; - - struct bpf_program fp; - - if (pcap_lookupnet(device, &net, &mask, errbuf) == -1) { - fprintf(stderr, "Couldn't get netmask for device %s %s\n", device, errbuf); - net = 0; - mask = 0; - } - - *handle = pcap_open_live(device, BUFSIZ, 1, 1000, errbuf); - if (handle == NULL) { - fprintf(stderr, "Couldn't open device: %s\n", errbuf); - return 2; - } - - if (pcap_compile(*handle, &fp, filter_exp, 0, net) == -1) { - fprintf(stderr, "Couldn't parse filter %s: %s\n", filter_exp, pcap_geterr(*handle)); - return 2; - } - - if (pcap_setfilter(*handle, &fp) == -1) { - fprintf(stderr, "Couldn't install filter %s: %s\n", filter_exp, pcap_geterr(*handle)); - return 2; - } - - return 0; -} - -static void print_usage(char *appname) -{ - printf("Usage: %s INTERFACE\n", appname); -} - -int main(int argc, char *argv[]) -{ - pcap_t *handle; - - if (argc != 2) { - print_usage(argv[0]); - return 2; - } - - init_capture(argv[1], &handle); - - pcap_loop(handle, -1, got_packet, NULL); - - pcap_close(handle); - - return 0; -} diff --git a/sw/airborne/fms/onboard_transport.c b/sw/airborne/fms/onboard_transport.c deleted file mode 100644 index c49d1b1a345..00000000000 --- a/sw/airborne/fms/onboard_transport.c +++ /dev/null @@ -1,242 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "onboard_transport.h" -#include "downlink_transport.h" - -#define LOG_BUFLEN 512 -#define PATH_LEN 256 -#define FILENAME_LEN 64 -#define TIMESTAMP_SCALE 10000 - -static void put_bytes(void *impl, enum DownlinkDataType data_type, uint8_t len __attribute__((unused)), - const void *bytes) -{ - struct onboard_transport *onboard = (struct onboard_transport *) impl; - uint32_t length = 0; - - if (data_type == DL_TYPE_ARRAY_LENGTH) { - onboard->array_length = *((const uint8_t *) bytes); - return; - } - - while (length++ <= onboard->array_length) { - if (onboard->array_length > 0 && length > 1) { - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, ","); - } else { - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, " "); - } - switch (data_type) { - case DL_TYPE_UINT8: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhu", - * (const uint8_t *)bytes); - bytes = (const uint8_t *) bytes + 1; - break; - case DL_TYPE_UINT16: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hu", - * (const uint16_t *)bytes); - bytes = (const uint16_t *) bytes + 2; - break; - case DL_TYPE_UINT32: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u", - * (const uint32_t *)bytes); - bytes = (const uint32_t *) bytes + 4; - break; - case DL_TYPE_UINT64: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%llu", - *(const uint64_t *)bytes); - bytes = (const uint64_t *) bytes + 8; - break; - case DL_TYPE_INT8: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhi", - * (const int8_t *)bytes); - bytes = (const int8_t *) bytes + 1; - break; - case DL_TYPE_INT16: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hi", - * (const int16_t *)bytes); - bytes = (const int16_t *) bytes + 2; - break; - case DL_TYPE_INT32: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%i", - * (const int32_t *)bytes); - bytes = (const int32_t *) bytes + 4; - break; - case DL_TYPE_INT64: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%lli", - *(const int64_t *)bytes); - bytes = (const int64_t *) bytes + 8; - break; - case DL_TYPE_FLOAT: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", - *(const float *)bytes); - bytes = (const float *) bytes + 4; - break; - case DL_TYPE_DOUBLE: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", - *(const double *)bytes); - bytes = (const double *) bytes + 8; - break; - case DL_TYPE_TIMESTAMP: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, - "%u.%04u", (*(const uint32_t *)bytes) / TIMESTAMP_SCALE, (*(const uint32_t *)bytes) % TIMESTAMP_SCALE); - bytes = (const uint32_t *) bytes + 4; - break; - case DL_TYPE_ARRAY_LENGTH: - break; - } - } -} - -static void start_message(void *impl, char *name, uint8_t msg_id __attribute__((unused)), - uint8_t payload_len __attribute__((unused))) -{ - uint8_t ac_id = AC_ID; - struct onboard_transport *onboard = (struct onboard_transport *) impl; - onboard->buffer_idx = 0; - onboard->array_length = 0; - - put_bytes(onboard, DL_TYPE_TIMESTAMP, 4, (uint8_t *) onboard->timestamp); - put_bytes(onboard, DL_TYPE_UINT8, 1, (uint8_t *) &ac_id); - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, " %s", - name); -} - -static void end_message(void *impl) -{ - struct onboard_transport *onboard = (struct onboard_transport *) impl; - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "\n"); - if (write(onboard->fd, onboard->buffer, onboard->buffer_idx) < 0) { - onboard->overrun++; - } -} - -static void overrun(void *impl) -{ - struct onboard_transport *onboard = (struct onboard_transport *) impl; - onboard->overrun++; -} - -static void count_bytes(void *onboard __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ - -} - -static int check_free_space(void *onboard __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ - return TRUE; -} - -static uint8_t size_of(void *onboard __attribute__((unused)), uint8_t len) -{ - return len + 2; -} - -static int open_piped(char *filepath) -{ - int fd; - int pipe_fd[2]; - int flags; - ssize_t count; - char buffer[LOG_BUFLEN]; - - if (pipe(pipe_fd) == -1) { - perror("onboard transport: pipe"); - } - - fd = open(filepath, O_CREAT | O_RDWR, 0644); - if (fd < 0) { - perror("onboard log: open"); - } - - if (fork() == 0) { - // This is the child, close the write side of the pipe - close(pipe_fd[1]); - int retval; - - /* Lower our priority -- logging is not that important */ - if (setpriority(PRIO_PROCESS, 0, 10) < 0) { - fprintf(stderr, "Couldn't renice logger for some reason!\n"); - } - - // copy from the read side of the pipe to the log - while (1) { - count = read(pipe_fd[0], buffer, LOG_BUFLEN); - if (count < 0) { - // error, presumably the pipe is closed - break; - } - retval = write(fd, buffer, count); - } - } else { - // This is the parent, close the read side of the pipe - close(pipe_fd[0]); - - // Close the log file - close(fd); - - // set non blocking on the write side of the pipe - flags = fcntl(pipe_fd[1], F_GETFL); - fcntl(pipe_fd[1], F_SETFL, flags | O_NONBLOCK); - - // return the write side of the pipe - fd = pipe_fd[1]; - } - - return fd; -} - -static void make_filename(char *filename) -{ - time_t t; - t = time(NULL); - struct tm *tmp; - - tmp = localtime(&t); - if (tmp == NULL) { - perror("localtime"); - } - - // format DM_HHMM_SS - if (strftime(filename, FILENAME_LEN, "log_%d_%H%M_%S.data", tmp) == 0) { - fprintf(stderr, "strftime returned 0"); - } -} - -struct DownlinkTransport *onboard_transport_new(char *filepath, uint32_t *timestamp) -{ - struct DownlinkTransport *tp = calloc(1, sizeof(struct DownlinkTransport)); - struct onboard_transport *onboard = calloc(1, sizeof(struct onboard_transport)); - - char full_filename[PATH_LEN]; - char filename[FILENAME_LEN]; - - strncpy(full_filename, filepath, PATH_LEN); - make_filename(filename); - strncat(full_filename, filename, FILENAME_LEN); - int fd = open_piped(full_filename); - - tp->impl = onboard; - onboard->fd = fd; - onboard->timestamp = timestamp; - - tp->StartMessage = start_message; - tp->EndMessage = end_message; - tp->PutBytes = put_bytes; - - tp->Overrun = overrun; - tp->CountBytes = count_bytes; - tp->SizeOf = size_of; - tp->CheckFreeSpace = check_free_space; - - return tp; -} diff --git a/sw/airborne/fms/onboard_transport.h b/sw/airborne/fms/onboard_transport.h deleted file mode 100644 index dab9ee2d56c..00000000000 --- a/sw/airborne/fms/onboard_transport.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef ONBOARD_TRANSPORT_H -#define ONBOARD_TRANSPORT_H - -#include "std.h" -#include "generated/airframe.h" - -#ifndef MSG_TIMESTAMP -#define MSG_TIMESTAMP 0 -#endif - -#define ONBOARD_BUFFER_LEN 1500 - -struct DownlinkTransport *onboard_transport_new(char *path, uint32_t *timestamp); - -struct onboard_transport { - int fd; - char buffer[ONBOARD_BUFFER_LEN]; - uint32_t buffer_idx; - uint32_t array_length; - uint32_t *timestamp; - uint32_t overrun; -}; - -#endif /* ONBOARD_TRANSPORT_H */ - diff --git a/sw/airborne/fms/overo_blmc_calibrate.c b/sw/airborne/fms/overo_blmc_calibrate.c deleted file mode 100644 index f1e86c56845..00000000000 --- a/sw/airborne/fms/overo_blmc_calibrate.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include - -#include - -#include "fms/overo_blmc_calibrate.h" - -#include "std.h" -#include "fms_debug.h" - -/* stuff for io processor link */ -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -struct OveroBLMCCalibrate blmc_calibrate; - -static void main_init(void); -static void dialog_with_io_proc(void); - -int main(int argc, char *argv[]) -{ - - main_init(); - - return 0; - -} - -static void dialog_with_io_proc() -{ - - struct AutopilotMessageCRCFrame msg_in; - struct AutopilotMessageCRCFrame msg_out; - uint8_t crc_valid; - - for (uint8_t i = 0; i < LISA_PWM_OUTPUT_NB; i++) { msg_out.payload.msg_down.pwm_outputs_usecs[i] = blmc_calibrate.servos_outputs_usecs[i]; } - - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); - -} - -static void main_init(void) -{ - - /* Initalize our SPI link to IO processor */ - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return; - } - - printf("Starting at 2000us\n"); - /* Initialize blaaa */ - for (uint8_t i = 0; i < LISA_PWM_OUTPUT_NB; i++) { blmc_calibrate.servos_outputs_usecs[i] = 2000; } - dialog_with_io_proc(); - getchar(); - printf("At 1000us\n"); - for (uint8_t i = 0; i < LISA_PWM_OUTPUT_NB; i++) { blmc_calibrate.servos_outputs_usecs[i] = 1000; } - dialog_with_io_proc(); - getchar(); - printf("At 1500us\n"); - for (uint8_t i = 0; i < LISA_PWM_OUTPUT_NB; i++) { blmc_calibrate.servos_outputs_usecs[i] = 1500; } - dialog_with_io_proc(); - -} diff --git a/sw/airborne/fms/overo_blmc_calibrate.h b/sw/airborne/fms/overo_blmc_calibrate.h deleted file mode 100644 index 1138dfdff16..00000000000 --- a/sw/airborne/fms/overo_blmc_calibrate.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef OVERO_BLMC_CALIBRATE_H -#define OVERO_BLMC_CALIBRATE_H - -#include "std.h" -#include "fms/fms_autopilot_msg.h" - -struct OveroBLMCCalibrate { - /* our actuators */ - uint16_t servos_outputs_usecs[LISA_PWM_OUTPUT_NB]; /* FIXME */ -}; - -extern struct OveroBLMCCalibrate blmc_calibrate; - -#endif /* OVERO_BLMC_CALIBRATE_H */ diff --git a/sw/airborne/fms/overo_test_gps_passthrough.c b/sw/airborne/fms/overo_test_gps_passthrough.c deleted file mode 100644 index 08e51ead16f..00000000000 --- a/sw/airborne/fms/overo_test_gps_passthrough.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include -#include - -#include - -#include "std.h" -#include "fms_debug.h" -#include "fms_periodic.h" - -/* stuff for io processor link */ -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -/* stuff for telemetry/datalink */ - -#define OVERO_ENV -#include "lisa/lisa_spistream.h" - -static void parse_command_line(int argc, char **argv); -static void main_init(void); -static void main_periodic(int my_sig_num); -static void spistream_event(void); - -static void on_spistream_msg_received(uint8_t msg_id, uint8_t *data, uint16_t num_bytes); -static void on_spistream_msg_sent(void); - -static uint8_t spistream_msg[123]; - -int main(int argc, char *argv[]) -{ - - parse_command_line(argc, argv); - - main_init(); - TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); - - /* Enter our mainloop */ - event_dispatch(); - while (1) { - sleep(100); - } - - TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); - - return 0; -} - -static void main_periodic(int my_sig_num) -{ -#if 0 - static int32_t every_100 = 1000; - if (every_100-- == 0) { - every_100 = 1000; - spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT); - /* - spistream_send_msg(spistream_msg, 15); - spistream_send_msg(spistream_msg, 25); - */ - } -#endif - spistream_event(); -} - -static void spistream_event() -{ - - struct AutopilotMessagePTStream msg_in; - struct AutopilotMessagePTStream msg_out; - static uint8_t pkg_size = sizeof(msg_in.pkg_data); - uint8_t crc_valid; - uint8_t cnt; - - spistream_write_pkg(&msg_out); - - if (msg_out.message_cnt != 0) { - printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", - pkg_size, msg_out.message_cnt, msg_out.package_cntd); - for (cnt = 0; cnt < pkg_size; cnt++) { - printf("%3d ", msg_out.pkg_data[cnt]); - } - printf("\n"); - } - - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); - /* - if(msg_in.message_cnt != 0) { - printf("Package in: Size: %3d MID: %3d PCNTD: %3d | ", - pkg_size, msg_in.message_cnt, msg_in.package_cntd); - for(cnt = 0; cnt < pkg_size; cnt++) { - printf("%3d ", msg_in.pkg_data[cnt]); - } - printf("\n"); - } - */ - spistream_read_pkg(&msg_in); -} - -static void on_spistream_msg_received(uint8_t msg_id, - uint8_t *data, - uint16_t num_bytes) -{ - static uint16_t plot_freq = 100; - - uint16_t log_bytes; - uint8_t cnt; - struct tm *timeinfo; - time_t c_time; - char time_str[30]; - - plot_freq = 100; - time(&c_time); - - timeinfo = localtime(&c_time); - strftime(time_str, 30, " %X ", timeinfo); - - log_bytes = num_bytes; - if (log_bytes > 48) { log_bytes = 48; } - printf("SPI message received: "); - printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]); - for (cnt = 1; cnt < log_bytes; cnt++) { - printf("%02X ", data[cnt]); - } - printf("\n"); -} - -static void on_spistream_msg_sent(void) -{ -// TRACE(TRACE_DEBUG, "%s", "SPI message sent \n"); -} - -static void main_init(void) -{ - uint8_t byte_idx; - - TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); - - /* Initalize our SPI link to IO processor */ - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return; - } - - spistream_init(&on_spistream_msg_received, - &on_spistream_msg_sent); - /* - spistream_msg[0] = 0; - for(byte_idx=1; byte_idx < 123; byte_idx += 4) { - spistream_msg[byte_idx] = 0xDE; - spistream_msg[byte_idx+1] = 0xAD; - spistream_msg[byte_idx+2] = 0xBE; - spistream_msg[byte_idx+3] = 0xEF; - } - */ - for (byte_idx = 1; byte_idx < 123; byte_idx++) { - spistream_msg[byte_idx] = byte_idx; - } - /* Initalize the event library */ - event_init(); - - /* Initalize our ô so accurate periodic timer */ - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return; - } - - TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); -} - -static void parse_command_line(int argc, char **argv) -{ -} diff --git a/sw/airborne/fms/overo_test_passthrough.c b/sw/airborne/fms/overo_test_passthrough.c deleted file mode 100644 index 7e80dd58f62..00000000000 --- a/sw/airborne/fms/overo_test_passthrough.c +++ /dev/null @@ -1,151 +0,0 @@ -/* - * Copyright (C) 2010 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 -#include -#include -#include -#include - -#include - -#include "fms/overo_test_passthrough.h" - -#include "std.h" -#include "fms_debug.h" -#include "fms_periodic.h" - -/* stuff for io processor link */ -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -/* stuff for telemetry/datalink */ -#include "fms_gs_com.h" - -struct OveroTestPassthrough otp; - -static void parse_command_line(int argc, char **argv); -static void main_init(void); -static void main_periodic(int my_sig_num); -static void dialog_with_io_proc(void); - - -int main(int argc, char *argv[]) -{ - - parse_command_line(argc, argv); - - main_init(); - TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); - - /* Enter our mainloop */ - event_dispatch(); - - TRACE(TRACE_DEBUG, "%s", "leaving mainloop... goodbye!\n"); - - return 0; - -} - -static void main_periodic(int my_sig_num) -{ - - dialog_with_io_proc(); - - fms_gs_com_periodic(); - -} - - - -static void dialog_with_io_proc() -{ - - struct AutopilotMessageCRCFrame msg_in; - struct AutopilotMessageCRCFrame msg_out; - uint8_t crc_valid; - - for (uint8_t i = 0; i < 6; i++) { msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i]; } -// for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i]; - - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); - - struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up; - RATES_FLOAT_OF_BFP(otp.imu.gyro, in->gyro); - ACCELS_FLOAT_OF_BFP(otp.imu.accel, in->accel); - MAGS_FLOAT_OF_BFP(otp.imu.mag, in->mag); - - otp.io_proc_msg_cnt = in->stm_msg_cnt; - otp.io_proc_err_cnt = in->stm_crc_err_cnt; - - otp.rc_status = in->rc_status; - - otp.baro_abs = in->pressure_absolute; - otp.baro_diff = in->pressure_differential; - -} - - -static void main_init(void) -{ - - TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); - - /* Initalize our SPI link to IO processor */ - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return; - } - - /* Initalize the event library */ - event_init(); - - /* Initalize our ô so accurate periodic timer */ - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return; - } - - /* Initialize our communications with ground segment */ - fms_gs_com_init(otp.gs_gw, 4242, 4243, FALSE); - - /* Initialize blaaa */ - for (uint8_t i = 0; i < 6; i++) { otp.servos_outputs_usecs[i] = 1500; } - for (uint8_t i = 0; i < 4; i++) { otp.csc_servo_outputs[i] = 1500; } - - TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); -} - - - -static void parse_command_line(int argc, char **argv) -{ - - if (argc > 1) { - otp.gs_gw = strdup(argv[1]); - } else { - otp.gs_gw = strdup("10.31.4.7"); - } - TRACE(TRACE_DEBUG, "%s", "Parsing command line:\n"); - TRACE(TRACE_DEBUG, " gw: %s\n", otp.gs_gw); - -} diff --git a/sw/airborne/fms/overo_test_passthrough.h b/sw/airborne/fms/overo_test_passthrough.h deleted file mode 100644 index bbe605e9b54..00000000000 --- a/sw/airborne/fms/overo_test_passthrough.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef OVERO_TEST_PASSTHROUGH_H -#define OVERO_TEST_PASSTHROUGH_H - -#include "std.h" -#include "subsystems/imu.h" - -struct OveroTestPassthrough { - /* our network connection */ - char *gs_gw; - - /* our sensors */ - struct ImuFloat imu; - uint8_t rc_status; - int32_t baro_abs; - int32_t baro_diff; - - /* our actuators */ - uint16_t servos_outputs_usecs[6]; /* FIXME */ - uint16_t csc_servo_outputs[4]; - - /* the io proc status */ - uint32_t io_proc_msg_cnt; - uint32_t io_proc_err_cnt; -}; - -extern struct OveroTestPassthrough otp; - -#endif /* OVERO_TEST_PASSTHROUGH_H */ diff --git a/sw/airborne/fms/overo_test_passthrough_telemetry.h b/sw/airborne/fms/overo_test_passthrough_telemetry.h deleted file mode 100644 index 4aec58feba6..00000000000 --- a/sw/airborne/fms/overo_test_passthrough_telemetry.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef OVERO_TEST_PASSTHROUGH_TELEMETRY_H -#define OVERO_TEST_PASSTHROUGH_TELEMETRY_H - -#define PERIODIC_SEND_ALIVE(_transport) DOWNLINK_SEND_ALIVE(_transport, 16, MD5SUM) - -#include "fms/overo_test_passthrough.h" -#include "fms/fms_spi_link.h" -#include "fms/fms_gs_com.h" -#define PERIODIC_SEND_TEST_PASSTHROUGH_STATUS(_transport) \ - DOWNLINK_SEND_TEST_PASSTHROUGH_STATUS(_transport, \ - &otp.io_proc_msg_cnt, \ - &otp.io_proc_err_cnt, \ - &spi_link.msg_cnt, \ - &spi_link.crc_err_cnt, \ - &otp.rc_status) - - -#define PERIODIC_SEND_IMU_GYRO(_transport) \ - DOWNLINK_SEND_IMU_GYRO(_transport, &otp.imu.gyro.p, &otp.imu.gyro.q, &otp.imu.gyro.r) - -#define PERIODIC_SEND_IMU_ACCEL(_transport) \ - DOWNLINK_SEND_IMU_ACCEL(_transport, &otp.imu.accel.x, &otp.imu.accel.y, &otp.imu.accel.z) - -#define PERIODIC_SEND_IMU_MAG(_transport) \ - DOWNLINK_SEND_IMU_MAG(_transport, &otp.imu.mag.x, &otp.imu.mag.y, &otp.imu.mag.z) - -#define PERIODIC_SEND_BARO_RAW(_transport) \ - DOWNLINK_SEND_BARO_RAW(_transport, &otp.baro_abs, &otp.baro_diff) - - -#endif /* OVERO_TEST_PASSTHROUGH_TELEMETRY_H */ diff --git a/sw/airborne/fms/overo_test_periodic.c b/sw/airborne/fms/overo_test_periodic.c deleted file mode 100644 index 6cbd499c11c..00000000000 --- a/sw/airborne/fms/overo_test_periodic.c +++ /dev/null @@ -1,101 +0,0 @@ - - -#include -#include -#include -#include - -//#define LINK_HOST "10.31.4.7" -//#define LINK_HOST "stripe" -#define LINK_HOST "192.168.1.0" -#define LINK_PORT 4242 -#define DATALINK_PORT 4243 -#define FMS_NETWORK_BROADCAST TRUE - -#include "generated/airframe.h" - -#include "fms_periodic.h" -#include "subsystems/datalink/downlink.h" -#include "udp_transport.h" -#include "fms_network.h" -#include "fms_spi_link.h" -#include "fms_autopilot_msg.h" - -static void main_periodic(int); -static void main_send_to_stm(void); - -static void on_datalink_event(int fd, short event, void *arg); -static struct FmsNetwork *network; - -int main(int argc, char **argv) -{ - - /* Initalize event library */ - event_init(); - - if (fms_periodic_init(main_periodic)) { - TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n"); - return -1; - } - - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link\n"); - return -1; - } - - /* Add our datalink event */ - network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_NETWORK_BROADCAST); - struct event datalink_event; - event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, &datalink_event); - event_add(&datalink_event, NULL); - - event_dispatch(); - - return 0; -} - - -static void main_periodic(int my_sig_num) -{ - - DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); - - main_send_to_stm(); - - UdpTransportPeriodic(); - -} - - -static void main_send_to_stm(void) -{ - - struct OVERO_LINK_MSG_UP msg_in; - struct OVERO_LINK_MSG_DOWN msg_out; - spi_link_send(&msg_out, sizeof(union AutopilotMessage), &msg_in); - - // printf("spi telemetry got %d\n", msg_in.up.tw_len); - // for (int i=0; i -#include -#include -#include -#include - - -#include "fms/fms_debug.h" -#include "fms/fms_spi_link.h" -#include "fms/fms_autopilot_msg.h" - -#define fill_msg fill_msg_random -static void fill_msg_counter(struct AutopilotMessageCRCFrame *msg); -static void fill_msg_cst(struct AutopilotMessageCRCFrame *msg); -static void fill_msg_random(struct AutopilotMessageCRCFrame *msg); -static void print_up_msg(struct AutopilotMessageCRCFrame *msg); -static void print_down_msg(struct AutopilotMessageCRCFrame *msg); - - -int main(int argc, char *argv[]) -{ - - uint32_t us_delay; - - if (argc > 1) { - us_delay = atoi(argv[1]); - } else { - us_delay = 1953; - } - - printf("Delay: %dus\n", us_delay); - - if (spi_link_init()) { - TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); - return -1; - } - - uint8_t skip_buf_check = 0; - uint8_t skip_crc_check = 0; - - uint32_t buf_check_errors = 0; - - while (1) { - struct AutopilotMessageCRCFrame crc_msg_out; - struct AutopilotMessageCRCFrame msg_out_prev; - struct AutopilotMessageCRCFrame crc_msg_in; - uint8_t crc_valid; - - /* backup message for later comparison */ - memcpy(&msg_out_prev, &crc_msg_out, sizeof(struct AutopilotMessageCRCFrame)); - /* fill message with data */ - fill_msg(&crc_msg_out); - /* send it over spi */ - spi_link_send(&crc_msg_out, sizeof(struct AutopilotMessageCRCFrame), &crc_msg_in, &crc_valid); - - /* check that received message is identical to the one previously sent */ - if (!skip_buf_check && spi_link.msg_cnt > 1) { - if (memcmp(&crc_msg_in.payload, &msg_out_prev.payload, sizeof(struct OVERO_LINK_MSG_DOWN))) { - printf("Compare failed: (received != expected): \n"); - print_up_msg(&crc_msg_in); - print_down_msg(&msg_out_prev); - buf_check_errors++; - } - } - /* report crc error */ - if (!skip_crc_check & !crc_valid) { - printf("CRC checksum failed: received %04X != computed %04X\n", - crc_msg_in.crc, - crc_calc_block_crc8((uint8_t *)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN))); - } - /* report message count */ - if (!(spi_link.msg_cnt % 1000)) - printf("msg %d, buf err %d, CRC errors: %d\n", spi_link.msg_cnt, - buf_check_errors, spi_link.crc_err_cnt); - - /* give it some rest */ - if (us_delay > 0) { - usleep(us_delay); - } - } - - return 0; -} - - -static void print_up_msg(struct AutopilotMessageCRCFrame *msg) -{ - printf("UP: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", - msg->payload.msg_up.foo, - msg->payload.msg_up.bar, - msg->payload.msg_up.bla, - msg->payload.msg_up.ble, - msg->payload.msg_up.bli, - msg->payload.msg_up.blo, - msg->payload.msg_up.blu, - msg->payload.msg_up.bly, - msg->crc); -} -static void print_down_msg(struct AutopilotMessageCRCFrame *msg) -{ - printf("DW: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", - msg->payload.msg_down.foo, - msg->payload.msg_down.bar, - msg->payload.msg_down.bla, - msg->payload.msg_down.ble, - msg->payload.msg_down.bli, - msg->payload.msg_down.blo, - msg->payload.msg_up.blu, - msg->payload.msg_up.bly, - msg->crc); -} - - -static void fill_msg_counter(struct AutopilotMessageCRCFrame *msg) -{ - static uint32_t foo = 5000; - msg->payload.msg_up.foo = 0x55; - msg->payload.msg_up.bar = 1; - msg->payload.msg_up.bla = 0xff; - msg->payload.msg_up.ble = foo % 255; - msg->payload.msg_up.bli = 1; - msg->payload.msg_up.blo = 0xff; - msg->payload.msg_up.blu = 0; - msg->payload.msg_up.bly = 0; - - foo--; - if (foo == 0) { - foo = 5000; - } -} - -static void fill_msg_cst(struct AutopilotMessageCRCFrame *msg) -{ - msg->payload.msg_up.foo = 0; - msg->payload.msg_up.bar = 0; - msg->payload.msg_up.bla = 0; - msg->payload.msg_up.ble = 0; - msg->payload.msg_up.bli = 0; - msg->payload.msg_up.blo = 0; - msg->payload.msg_up.blu = 0; - msg->payload.msg_up.bly = 0x01; -} - -static void fill_msg_random(struct AutopilotMessageCRCFrame *msg) -{ - msg->payload.msg_up.foo = random(); - msg->payload.msg_up.bar = random(); - msg->payload.msg_up.bla = random(); - msg->payload.msg_up.ble = random(); - msg->payload.msg_up.bli = random(); - msg->payload.msg_up.blo = random(); - msg->payload.msg_up.blu = random(); - msg->payload.msg_up.bly = random(); -} diff --git a/sw/airborne/fms/overo_test_telemetry.c b/sw/airborne/fms/overo_test_telemetry.c deleted file mode 100644 index 5642e5b8904..00000000000 --- a/sw/airborne/fms/overo_test_telemetry.c +++ /dev/null @@ -1,69 +0,0 @@ - -#include - -#include - -#include -#include - -#include "subsystems/datalink/downlink.h" -#include "udp_transport.h" -#include "fms_network.h" - -#define GCS_HOST "10.31.4.7" -#define GCS_PORT 4242 -#define DATALINK_PORT 4243 - -#define TIMEOUT_DT_SEC 0 -//#define TIMEOUT_DT_USEC 500000 -#define TIMEOUT_DT_USEC 50000 - - -#define ADD_TIMEOUT() { \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = TIMEOUT_DT_SEC; \ - tv.tv_usec = TIMEOUT_DT_USEC; \ - event_add(&timeout, &tv); \ - } - -static void timeout_cb(int fd, short event, void *arg); - -static struct event timeout; -static struct FmsNetwork *network; - -void timeout_cb(int fd, short event, void *arg) -{ - - // printf("in timeout_cb\n"); - - DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); - - float foof = 3.14159265358979323846; - double food = 3.14159265358979323846; - DOWNLINK_SEND_TEST_FORMAT(DefaultChannel, &food, &foof); - - UdpTransportPeriodic(); - - ADD_TIMEOUT(); - -} - - -int main(int argc, char **argv) -{ - - network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); - - /* Initalize the event library */ - event_init(); - - /* Add a timeout event */ - evtimer_set(&timeout, timeout_cb, &timeout); - - ADD_TIMEOUT(); - - event_dispatch(); - - return 0; -} diff --git a/sw/airborne/fms/overo_test_telemetry2.c b/sw/airborne/fms/overo_test_telemetry2.c deleted file mode 100644 index eab1e05dfeb..00000000000 --- a/sw/airborne/fms/overo_test_telemetry2.c +++ /dev/null @@ -1,142 +0,0 @@ - -#include - -#include - -#include -#include - -#include -#include - -#include "downlink_transport.h" -#include "messages2.h" -#include "udp_transport2.h" -#include "dl_protocol.h" -#include "fms_network.h" - -#define GCS_HOST "10.31.4.7" -#define GCS_PORT 4242 -#define DATALINK_PORT 4243 - -#define TIMEOUT_DT_SEC 0 -//#define TIMEOUT_DT_USEC 500000 -#define TIMEOUT_DT_USEC 50000 - -#define DL_MSG_SIZE 128 - -#define ADD_TIMEOUT() { \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = TIMEOUT_DT_SEC; \ - tv.tv_usec = TIMEOUT_DT_USEC; \ - event_add(&timeout, &tv); \ - } - -static void timeout_cb(int fd, short event, void *arg); - -static struct event timeout; -static struct event read_event; -static struct FmsNetwork *network; -static struct DownlinkTransport *udp_transport; - -void timeout_cb(int fd, short event, void *arg) -{ - - // printf("in timeout_cb\n"); - - DOWNLINK_SEND_ALIVE(udp_transport, 16, MD5SUM); - - float foof = 3.14159265358979323846; - double food = 3.14159265358979323846; - DOWNLINK_SEND_TEST_FORMAT(udp_transport, &food, &foof); - - if (udp_transport->Periodic) { - udp_transport->Periodic(udp_transport->impl); - } - - ADD_TIMEOUT(); - -} - -static inline int checked_read(int fd, char *buf, size_t len) -{ - int bytes = read(fd, buf, len); - - if (bytes == 0) { - fprintf(stderr, "Connection closed\n"); - } else if (bytes == -1) { - perror("read"); - } - - return bytes; -} - -bool_t my_dl_msg_available; -uint8_t my_dl_buffer[DL_MSG_SIZE] __attribute__((aligned)); - -#define IdOfMsg(x) (x[1]) - -static void dl_handle_msg(struct DownlinkTransport *tp) -{ - uint8_t msg_id = IdOfMsg(my_dl_buffer); - switch (msg_id) { - - case DL_PING: { - DOWNLINK_SEND_PONG(tp); - } - break; - - case DL_SETTING : { - uint8_t i = DL_SETTING_index(my_dl_buffer); - float var = DL_SETTING_value(my_dl_buffer); - // DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(tp, &i, &var); - } - break; - } - -} - -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) -{ - char buf[255]; - int bytes_read; - bytes_read = checked_read(fd, buf, sizeof(buf) - 1); - struct DownlinkTransport *tp = (struct DownlinkTransport *) arg; - struct udp_transport *udp_impl = tp->impl; - - int i = 0; - while (i < bytes_read) { - parse_udp_dl(udp_impl, buf[i]); - i++; - if (udp_impl->udp_dl_msg_received) { - memcpy(my_dl_buffer, udp_impl->udp_dl_payload, udp_impl->udp_dl_payload_len); - dl_handle_msg(tp); - udp_impl->udp_dl_msg_received = FALSE; - } - } -} - -int main(int argc, char **argv) -{ - - network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); - udp_transport = udp_transport_new(network); - - - /* Initalize the event library */ - event_init(); - - event_set(&read_event, network->socket_in, EV_READ | EV_PERSIST, on_datalink_event, udp_transport); - event_add(&read_event, NULL); - - /* Add a timeout event */ - evtimer_set(&timeout, timeout_cb, &timeout); - - ADD_TIMEOUT(); - - event_dispatch(); - - return 0; -} diff --git a/sw/airborne/fms/packet_header.h b/sw/airborne/fms/packet_header.h deleted file mode 100644 index 8b4e7336f40..00000000000 --- a/sw/airborne/fms/packet_header.h +++ /dev/null @@ -1,39 +0,0 @@ -/* Ethernet addresses are 6 bytes */ -#define ETHER_ADDR_LEN 6 - -#define ETHERNET_HEADER_LENGTH 14 - -/* Ethernet header */ -struct ethernet_header { - u_char ether_dhost[ETHER_ADDR_LEN]; /* Destination host address */ - u_char ether_shost[ETHER_ADDR_LEN]; /* Source host address */ - u_short ether_type; /* IP? ARP? RARP? etc */ -} __attribute__((packed));; - -/* IP header */ -struct ip_header { - u_char ip_vhl; /* version << 4 | header length >> 2 */ - u_char ip_tos; /* type of service */ - u_short ip_len; /* total length */ - u_short ip_id; /* identification */ - u_short ip_off; /* fragment offset field */ -#define IP_RF 0x8000 /* reserved fragment flag */ -#define IP_DF 0x4000 /* dont fragment flag */ -#define IP_MF 0x2000 /* more fragments flag */ -#define IP_OFFMASK 0x1fff /* mask for fragmenting bits */ - u_char ip_ttl; /* time to live */ - u_char ip_p; /* protocol */ - u_short ip_sum; /* checksum */ - struct in_addr ip_src, ip_dst; /* source and dest address */ -} __attribute__((packed)); -#define IP_HL_WORDS(ip) (((ip)->ip_vhl) & 0x0f) -#define IP_V(ip) (((ip)->ip_vhl) >> 4) - -/* UDP header */ -struct udp_header { - u_short uh_sport; /* source port */ - u_short uh_dport; /* destination port */ - u_short uh_len; /* window */ - u_short uh_sum; /* checksum */ -} __attribute__((packed)); - diff --git a/sw/airborne/fms/test_telemetry.c b/sw/airborne/fms/test_telemetry.c deleted file mode 100644 index 92a025b7707..00000000000 --- a/sw/airborne/fms/test_telemetry.c +++ /dev/null @@ -1,13 +0,0 @@ - -#include -#include - -int main(int argc, char **argv) -{ - - - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - g_main_loop_run(ml); - - return 0; -} diff --git a/sw/airborne/fms/test_telemetry_2.c b/sw/airborne/fms/test_telemetry_2.c deleted file mode 100644 index e890282756b..00000000000 --- a/sw/airborne/fms/test_telemetry_2.c +++ /dev/null @@ -1,63 +0,0 @@ - -#include - -#include - -#include -#include - - -#include "subsystems/datalink/downlink.h" -#include "udp_transport.h" -#include "fms_network.h" - -#define GCS_HOST "10.31.4.7" -#define GCS_PORT 4242 - -#define TIMEOUT_DT_SEC 0 -#define TIMEOUT_DT_USEC 500000 - -static void timeout_cb(int fd, short event, void *arg); - -static struct event timeout; -static struct FmsNetwork *network; - -void timeout_cb(int fd, short event, void *arg) -{ - - printf("in timeout_cb\n"); - - DOWNLINK_SEND_ALIVE(16, MD5SUM); - - struct timeval tv; - evutil_timerclear(&tv); - tv.tv_sec = TIMEOUT_DT_SEC; - tv.tv_usec = TIMEOUT_DT_USEC; - event_add(&timeout, &tv); - -} - - -int main(int argc, char **argv) -{ - - printf("hello world\n"); - - network = network_new(GCS_HOST, GCS_PORT); - - /* Initalize the event library */ - event_init(); - - /* Add a timeout event */ - evtimer_set(&timeout, timeout_cb, &timeout); - - struct timeval tv; - evutil_timerclear(&tv); - tv.tv_sec = TIMEOUT_DT_SEC; - tv.tv_usec = TIMEOUT_DT_USEC; - event_add(&timeout, &tv); - - event_dispatch(); - - return 0; -} diff --git a/sw/airborne/fms/udp_transport.c b/sw/airborne/fms/udp_transport.c deleted file mode 100644 index bcbf264c803..00000000000 --- a/sw/airborne/fms/udp_transport.c +++ /dev/null @@ -1,16 +0,0 @@ -#include "udp_transport.h" - -/* - * Downlink - */ -char updt_tx_buf[UDPT_TX_BUF_LEN]; -uint16_t udpt_tx_buf_idx; -uint8_t udpt_ck_a, udpt_ck_b; - -/* - * Uplink - */ -uint8_t udp_dl_payload[UDP_DL_PAYLOAD_LEN]; -volatile uint8_t udp_dl_payload_len; -volatile bool_t udp_dl_msg_received; -uint8_t udp_dl_ovrn, udp_dl_nb_err; diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h deleted file mode 100644 index 123e9e546b0..00000000000 --- a/sw/airborne/fms/udp_transport.h +++ /dev/null @@ -1,194 +0,0 @@ -#ifndef UDP_TRANSPORT_H -#define UDP_TRANSPORT_H - -#include "fms_network.h" -#include "fms_debug.h" -#include "std.h" - -#ifdef UDP_TRANSPORT_TIMESTAMP -#define STX_UDP_TX 0x98 -#define STX_UDP_RX 0x99 -#define PPRZ_PROTOCOL_OVERHEAD 8 -#define UdpTransportPutTimestamp(x) UdpTransportPutUint32ByAddr(x) -#else -#define STX_UDP_TX 0x99 -#define STX_UDP_RX 0x99 -#define PPRZ_PROTOCOL_OVERHEAD 4 -#define UdpTransportPutTimestamp(x) {} -#endif - -#ifndef MSG_TIMESTAMP -#define MSG_TIMESTAMP 0 -#endif - -#define UDPT_TX_BUF_LEN 1496 -extern char updt_tx_buf[UDPT_TX_BUF_LEN]; -extern uint16_t udpt_tx_buf_idx; -extern uint8_t udpt_ck_a, udpt_ck_b; -#define UDPT_TX_BUF_WATERMARK 1024 - -#define UdpTransportSizeOf(_payload) (_payload + PPRZ_PROTOCOL_OVERHEAD) - -#define UdpTransportPutSTX() UdpTransportPut1Byte(STX_UDP_TX) - -#define UdpTransportPeriodic() { \ - if (udpt_tx_buf_idx) { \ - int len; \ - len = network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - downlink.nb_bytes += udpt_tx_buf_idx; \ - downlink.nb_msgs++; \ - if (len != udpt_tx_buf_idx) \ - downlink.nb_ovrn++; \ - udpt_tx_buf_idx = 0; \ - } \ - } - -#define UdpTransportCheckFreeSpace(_x) (TRUE) - - -#define UdpTransportHeader(payload_len) { \ - uint32_t msg_timestamp = MSG_TIMESTAMP; \ - /*udpt_tx_buf_idx = 0;*/ \ - UdpTransportPutSTX(); \ - uint8_t msg_len = UdpTransportSizeOf(payload_len); \ - udpt_ck_a = udpt_ck_b = 0; \ - UdpTransportPutUint8(msg_len); \ - UdpTransportPutTimestamp(&msg_timestamp); \ - } - - -#define UdpTransportTrailer() { \ - UdpTransportPut1Byte(udpt_ck_a); \ - UdpTransportPut1Byte(udpt_ck_b); \ - if (udpt_tx_buf_idx > UDPT_TX_BUF_WATERMARK) { \ - network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - udpt_tx_buf_idx = 0; \ - } \ - } - -#define UdpTransportPut1Byte(_x) { \ - updt_tx_buf[udpt_tx_buf_idx] = (_x); \ - udpt_tx_buf_idx++; \ - } - -#define UdpTransportPutUint8(_byte) { \ - udpt_ck_a += _byte; \ - udpt_ck_b += udpt_ck_a; \ - UdpTransportPut1Byte(_byte); \ - } - -#define UdpTransportPutNamedUint8(_name, _byte) UdpTransportPutUint8(_byte) - -#define UdpTransportPut1ByteByAddr(_byte) { \ - uint8_t _x = *(_byte); \ - UdpTransportPutUint8(_x); \ - } - -#define UdpTransportPut2ByteByAddr(_byte) { \ - UdpTransportPut1ByteByAddr(_byte); \ - UdpTransportPut1ByteByAddr((const uint8_t*)_byte+1); \ - } - -#define UdpTransportPut4ByteByAddr(_byte) { \ - UdpTransportPut2ByteByAddr(_byte); \ - UdpTransportPut2ByteByAddr((const uint8_t*)_byte+2); \ - } - -#define UdpTransportPut8ByteByAddr(_byte) { \ - UdpTransportPut4ByteByAddr(_byte); \ - UdpTransportPut4ByteByAddr((const uint8_t*)_byte+4); \ - } - - -/* base types */ -#define UdpTransportPutInt8ByAddr(_x) UdpTransportPut1ByteByAddr(_x) -#define UdpTransportPutUint8ByAddr(_x) UdpTransportPut1ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutInt16ByAddr(_x) UdpTransportPut2ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutUint16ByAddr(_x) UdpTransportPut2ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutInt32ByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutUint32ByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutFloatByAddr(_x) UdpTransportPut4ByteByAddr((const uint8_t*)_x) -#define UdpTransportPutDoubleByAddr(_x) UdpTransportPut8ByteByAddr((const uint8_t*)_x) - -#define UdpTransportPutArray(_put, _n, _x) { \ - uint8_t _i; \ - UdpTransportPutUint8(_n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(&_x[_i]); \ - } \ - } - -#define UdpTransportPutInt16Array(_n, _x) UdpTransportPutArray(UdpTransportPutInt16ByAddr, _n, _x) - -#define UdpTransportPutUint8Array(_n, _x) UdpTransportPutArray(UdpTransportPutUint8ByAddr, _n, _x) - -#define UdpTransportPutUint16Array(_n, _x) UdpTransportPutArray(UdpTransportPutUint16ByAddr, _n, _x) - - -/* - * Parsing of uplink msg - */ -#define UNINIT 0 -#define GOT_STX 1 -#define GOT_LENGTH 2 -#define GOT_PAYLOAD 3 -#define GOT_CRC1 4 - -#define UDP_DL_PAYLOAD_LEN 256 -extern uint8_t udp_dl_payload[UDP_DL_PAYLOAD_LEN]; -extern volatile uint8_t udp_dl_payload_len; -extern volatile bool_t udp_dl_msg_received; -extern uint8_t udp_dl_ovrn, udp_dl_nb_err; - -static inline void parse_udp_dl(uint8_t c) -{ - static uint8_t udp_dl_status = UNINIT; - static uint8_t _ck_a, _ck_b, payload_idx; - - switch (udp_dl_status) { - case UNINIT: - if (c == STX_UDP_RX) { - udp_dl_status++; - } - break; - case GOT_STX: - if (udp_dl_msg_received) { - udp_dl_ovrn++; - goto error; - } - udp_dl_payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - _ck_a = _ck_b = c; - udp_dl_status++; - payload_idx = 0; - break; - case GOT_LENGTH: - udp_dl_payload[payload_idx] = c; - _ck_a += c; _ck_b += _ck_a; - payload_idx++; - if (payload_idx == udp_dl_payload_len) { - udp_dl_status++; - } - break; - case GOT_PAYLOAD: - if (c != _ck_a) { - goto error; - } - udp_dl_status++; - break; - case GOT_CRC1: - if (c != _ck_b) { - goto error; - } - udp_dl_msg_received = TRUE; - goto restart; - } - return; -error: - udp_dl_nb_err++; -restart: - udp_dl_status = UNINIT; - return; -} - -#endif /* UDP_TRANSPORT_H */ - diff --git a/sw/airborne/fms/udp_transport2.c b/sw/airborne/fms/udp_transport2.c deleted file mode 100644 index 1b041341b48..00000000000 --- a/sw/airborne/fms/udp_transport2.c +++ /dev/null @@ -1,113 +0,0 @@ -#include -#include "udp_transport2.h" -#include "fms_network.h" -#include "downlink_transport.h" - - -static void put_1byte(struct udp_transport *udp, const uint8_t x) -{ - udp->updt_tx_buf[udp->udpt_tx_buf_idx] = x; - udp->udpt_tx_buf_idx++; -} - -static void put_uint8_t(struct udp_transport *udp, const uint8_t byte) -{ - udp->udpt_ck_a += byte; - udp->udpt_ck_b += udp->udpt_ck_a; - put_1byte(udp, byte); -} - -static void put_named_uint8_t(struct udp_transport *udp, char *name __attribute__((unused)), const uint8_t byte) -{ - put_uint8_t(udp, byte); -} - -static void put_bytes(void *impl, enum DownlinkDataType data_type __attribute__((unused)), uint8_t len, const void *buf) -{ - struct udp_transport *udp = (struct udp_transport *) impl; - const uint8_t *bytes = (const uint8_t *) buf; - for (int i = 0; i < len; i++) { - put_uint8_t(udp, bytes[i]); - } -} - -static void header(struct udp_transport *udp, uint8_t payload_len) -{ - uint32_t msg_timestamp = MSG_TIMESTAMP; - /*udpt_tx_buf_idx = 0;*/ - put_1byte(udp, STX_UDP_TX); - uint8_t msg_len = payload_len + PPRZ_PROTOCOL_OVERHEAD; - udp->udpt_ck_a = udp->udpt_ck_b = 0; - put_uint8_t(udp, msg_len); - put_bytes(udp, DL_TYPE_UINT32, 4, &msg_timestamp); -} - -static void start_message(void *impl, char *name, uint8_t msg_id, uint8_t payload_len) -{ - struct udp_transport *udp = (struct udp_transport *) impl; - header(udp, 2 + payload_len); - put_uint8_t(udp, AC_ID); - put_named_uint8_t(udp, name, msg_id); -} - -static void end_message(void *impl) -{ - struct udp_transport *udp = (struct udp_transport *) impl; - put_1byte(udp, udp->udpt_ck_a); - put_1byte(udp, udp->udpt_ck_b); - if (udp->udpt_tx_buf_idx > UDPT_TX_BUF_WATERMARK) { - network_write(udp->network, udp->updt_tx_buf, udp->udpt_tx_buf_idx); - udp->udpt_tx_buf_idx = 0; - } -} - -static void overrun(void *impl __attribute__((unused))) -{ - -} - -static void count_bytes(void *udp __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ - -} - -static int check_free_space(void *udp __attribute__((unused)), uint8_t bytes __attribute__((unused))) -{ - return TRUE; -} - -static uint8_t size_of(void *udp __attribute__((unused)), uint8_t len) -{ - return len + 2; -} - -static void periodic(void *impl) -{ - struct udp_transport *udp = (struct udp_transport *) impl; - if (udp->udpt_tx_buf_idx > 0) { - network_write(udp->network, udp->updt_tx_buf, udp->udpt_tx_buf_idx); - udp->udpt_tx_buf_idx = 0; - } -} - -struct DownlinkTransport *udp_transport_new(struct FmsNetwork *network) -{ - struct DownlinkTransport *tp = calloc(1, sizeof(struct DownlinkTransport)); - struct udp_transport *udp = calloc(1, sizeof(struct udp_transport)); - - udp->udp_dl_status = UNINIT; - udp->network = network; - tp->impl = udp; - - tp->StartMessage = start_message; - tp->EndMessage = end_message; - tp->PutBytes = put_bytes; - - tp->Overrun = overrun; - tp->CountBytes = count_bytes; - tp->SizeOf = size_of; - tp->CheckFreeSpace = check_free_space; - tp->Periodic = periodic; - - return tp; -} diff --git a/sw/airborne/fms/udp_transport2.h b/sw/airborne/fms/udp_transport2.h deleted file mode 100644 index 2014a586dbe..00000000000 --- a/sw/airborne/fms/udp_transport2.h +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef UDP_TRANSPORT2_H -#define UDP_TRANSPORT2_H - -#include "fms_network.h" -#include "fms_debug.h" -#include "std.h" -#include "generated/airframe.h" - -#define STX_UDP_TX 0x98 -#define STX_UDP_RX 0x99 -#define PPRZ_PROTOCOL_OVERHEAD 8 - -#ifndef MSG_TIMESTAMP -#define MSG_TIMESTAMP 0 -#endif - -struct DownlinkTransport *udp_transport_new(struct FmsNetwork *network); - -#define UDPT_TX_BUF_LEN 1496 -#define UDPT_TX_BUF_WATERMARK 1024 -#define UDP_DL_PAYLOAD_LEN 256 - -struct udp_transport { - /* - * Downlink - */ - char updt_tx_buf[UDPT_TX_BUF_LEN]; - uint16_t udpt_tx_buf_idx; - uint8_t udpt_ck_a, udpt_ck_b; - - /* - * Uplink - */ - uint8_t udp_dl_payload[UDP_DL_PAYLOAD_LEN]; - volatile uint8_t udp_dl_payload_len; - volatile bool_t udp_dl_msg_received; - uint8_t udp_dl_ovrn, udp_dl_nb_err; - uint8_t udp_dl_status; - uint8_t _ck_a, _ck_b, payload_idx; - - struct FmsNetwork *network; -}; - - -/* - * Parsing of uplink msg - */ -#define UNINIT 0 -#define GOT_STX 1 -#define GOT_LENGTH 2 -#define GOT_PAYLOAD 3 -#define GOT_CRC1 4 - -static inline void parse_udp_dl(struct udp_transport *tp, uint8_t c) -{ - - switch (tp->udp_dl_status) { - case UNINIT: - if (c == STX_UDP_RX) { - tp->udp_dl_status++; - } - break; - case GOT_STX: - if (tp->udp_dl_msg_received) { - tp->udp_dl_ovrn++; - goto error; - } - tp->udp_dl_payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - tp->_ck_a = tp->_ck_b = c; - tp->udp_dl_status++; - tp->payload_idx = 0; - break; - case GOT_LENGTH: - tp->udp_dl_payload[tp->payload_idx] = c; - tp->_ck_a += c; tp->_ck_b += tp->_ck_a; - tp->payload_idx++; - if (tp->payload_idx == tp->udp_dl_payload_len) { - tp->udp_dl_status++; - } - break; - case GOT_PAYLOAD: - if (c != tp->_ck_a) { - goto error; - } - tp->udp_dl_status++; - break; - case GOT_CRC1: - if (c != tp->_ck_b) { - goto error; - } - tp->udp_dl_msg_received = TRUE; - goto restart; - } - return; -error: - tp->udp_dl_nb_err++; -restart: - tp->udp_dl_status = UNINIT; - return; -} - -#endif /* UDP_TRANSPORT2_H */ -