diff --git a/.gitignore b/.gitignore
index 1e2145660d..56364c34ca 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,8 @@
*.[oa]
*.out
+*~
+
*.pyc
*.cmo
diff --git a/Makefile b/Makefile
index 1b291aebc9..af0bd3282c 100644
--- a/Makefile
+++ b/Makefile
@@ -58,12 +58,12 @@ MESSAGES_XML = $(CONF)/messages.xml
UBX_XML = $(CONF)/ubx.xml
XSENS_XML = $(CONF)/xsens_MTi-G.xml
TOOLS=$(PAPARAZZI_SRC)/sw/tools
-HAVE_ARM_NONE_EABI_GCC := $(wildcard /usr/bin/arm-none-eabi-gcc)
+HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc)
ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),)
#ARMGCC=/opt/paparazzi/bin/arm-elf-gcc
ARMGCC=/usr/bin/arm-elf-gcc
else
-ARMGCC=/usr/bin/arm-none-eabi-gcc
+ARMGCC=$(HAVE_ARM_NONE_EABI_GCC)
endif
diff --git a/README b/README
index 4239f4a8a6..1fbae924f2 100644
--- a/README
+++ b/README
@@ -16,7 +16,7 @@
# 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.
+# Boston, MA 02111-1307, USA.
Intro
@@ -27,7 +27,7 @@ Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System
Up to date informations are available from the wiki website
- http://paparazzi.enac.fr
+ http://paparazzi.enac.fr
and from the mailing list (http://savannah.nongnu.org/mail/?group=paparazzi)
and the IRC channel (freenode, #paparazzi).
@@ -60,11 +60,11 @@ Main requirements include
For Debian or Ubuntu users, required packages are available at
- http://paparazzi.enac.fr/debian
+ http://paparazzi.enac.fr/debian
- - "paparazzi-dev" will provide everything needed to compile and run the ground segment and the simulator. If something is missing, please report it.
- - "paparazzi-arm7" is required to compile the code for LPC21 based boards ( tiny, twog, booz, etc).
+ - "paparazzi-dev" will provide everything needed to compile and run the ground segment and the simulator. If something is missing, please report it.
+ - "paparazzi-arm7" is required to compile the code for LPC21 based boards ( tiny, twog, booz, etc).
- "paparazzi-stm32" is needed for building code for STM32 based boards (lisa/L, lisa/M)
- "paparazzi-omap" is needed for building code for the optional Gumstix Overo module available on lisa/L
@@ -77,7 +77,7 @@ Compilation and demo simulation
2) "./paparazzi" to run the Paparazzi Center
- 3) Select the "MJ5" or the "TJ1" aircraft in the upper-left A/C
+ 3) Select the "Microjet" aircraft in the upper-left A/C
combo box. Select "sim" from upper-middle "target" combo box. Click
"Build". When the compilation is finished, select "Simulation" from
the upper-right session combo box and click "Execute".
diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21
index 9f29195229..334e243b74 100644
--- a/conf/Makefile.lpc21
+++ b/conf/Makefile.lpc21
@@ -34,6 +34,7 @@ SRC_ARCH = arch/lpc21
# Define programs and commands.
HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc)
ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),)
+$(info Using gcc-arm 3.4.4 packaged by paparazzi.)
CC = arm-elf-gcc
LD = $(CC)
SHELL = sh
@@ -42,6 +43,7 @@ OBJDUMP = arm-elf-objdump
SIZE = arm-elf-size
NM = arm-elf-nm
else
+$(info Using arm-none-eabi-gcc.)
CC = arm-none-eabi-gcc
LD = $(CC)
SHELL = sh
@@ -91,6 +93,7 @@ CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
CFLAGS += -Wa,-adhlns=$(OBJDIR)/$(notdir $(subst $(suffix $<),.lst,$<))
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
CFLAGS += -ffunction-sections -fdata-sections
+CFLAGS += -finline-limit=1200 --param inline-unit-growth=100
# flags only for C
CFLAGS + = -Wstrict-prototypes -Wmissing-declarations
diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile
index 8b2808e155..bc717e9fc4 100644
--- a/conf/autopilot/fixedwing.makefile
+++ b/conf/autopilot/fixedwing.makefile
@@ -10,7 +10,7 @@ CFG_FIXEDWING=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/fixedwing
SRC_FIXEDWING=.
-SRC_ARCH=$(SRC_FIXEDWING)/arch/$(ARCH)
+SRC_ARCH=arch/$(ARCH)
SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/
SRC_FIRMWARE=firmwares/fixedwing
diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile
index 02fa4b7fee..e3c3b31579 100644
--- a/conf/autopilot/setup.makefile
+++ b/conf/autopilot/setup.makefile
@@ -3,8 +3,9 @@
#
#
+SRC_ARCH=arch/$(ARCH)
-CFG_SETUP=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/SETUP
+CFG_SETUP=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/setup
SRC_SETUP=.
@@ -49,9 +50,36 @@ usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+
+ifeq ($(ACTUATORS),)
+ ifeq ($(BOARD),tiny)
+ ifeq ($(BOARD_VERSION),1.1)
+ include $(CFG_SETUP)/actuators_4015.makefile
+ else
+ ifeq ($(BOARD_VERSION),0.99)
+ include $(CFG_SETUP)/actuators_4015.makefile
+ else
+ include $(CFG_SETUP)/actuators_4017.makefile
+ endif
+ endif
+ endif
+ ifeq ($(BOARD),twog)
+ include $(CFG_SETUP)/actuators_4017.makefile
+ endif
+
+ ifeq ($(BOARD),lisa_l)
+ include $(CFG_SETUP)/actuators_direct.makefile
+ endif
+
+else
+ include $(CFG_SETUP)/$(ACTUATORS).makefile
+endif
+
+
# a test program to setup actuators
-setup_actuators.CFLAGS += -DFBW -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
+setup_actuators.CFLAGS += -DFBW -DLED -DTIME_LED=1
setup_actuators.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1
setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
setup_actuators.CFLAGS += -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c downlink.c actuators.c setup_actuators.c $(SRC_ARCH)/uart_hw.c $(SRC_ARCH)/servos_4017_hw.c main.c
+setup_actuators.CFLAGS += $(SETUP_INC) -Ifirmwares/fixedwing
+setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c downlink.c setup_actuators.c $(SRC_ARCH)/uart_hw.c firmwares/fixedwing/main.c
diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile
old mode 100755
new mode 100644
diff --git a/conf/autopilot/subsystems/fixedwing/payload_hum_baro.makefile b/conf/autopilot/subsystems/fixedwing/payload_hum_baro.makefile
deleted file mode 100644
index 0513988cbb..0000000000
--- a/conf/autopilot/subsystems/fixedwing/payload_hum_baro.makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-# Payload: Sensirion humidity/temp, VTI pressure/temp
-
-ap.srcs += humid_sht.c
-ap.CFLAGS += -DUSE_HUMID_SHT -DDAT_PIN=3 -DSCK_PIN=2
-
-ap.srcs += baro_scp.c
-ap.CFLAGS += -DUSE_BARO_SCP
diff --git a/conf/autopilot/subsystems/setup/actuators_4015.makefile b/conf/autopilot/subsystems/setup/actuators_4015.makefile
new file mode 100644
index 0000000000..64f188656f
--- /dev/null
+++ b/conf/autopilot/subsystems/setup/actuators_4015.makefile
@@ -0,0 +1,5 @@
+# for Tiny v1.1
+
+$(TARGET).CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
+$(TARGET).srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
+
diff --git a/conf/autopilot/subsystems/setup/actuators_4017.makefile b/conf/autopilot/subsystems/setup/actuators_4017.makefile
new file mode 100644
index 0000000000..cab5136fc8
--- /dev/null
+++ b/conf/autopilot/subsystems/setup/actuators_4017.makefile
@@ -0,0 +1,5 @@
+# for Tiny v2 or Twog v1
+
+$(TARGET).CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017
+$(TARGET).srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
+
diff --git a/conf/autopilot/subsystems/setup/actuators_direct.makefile b/conf/autopilot/subsystems/setup/actuators_direct.makefile
new file mode 100644
index 0000000000..07865d71d1
--- /dev/null
+++ b/conf/autopilot/subsystems/setup/actuators_direct.makefile
@@ -0,0 +1,13 @@
+# for lisa_l
+
+$(TARGET).CFLAGS += -DACTUATORS=\"servos_direct_hw.h\" -DSERVOS_DIRECT
+$(TARGET).srcs += $(SRC_ARCH)/servos_direct_hw.c actuators.c
+
+
+# TODO TODO UGLY HACK: We re-use the booz actuators: Should become universal actuator code!!
+# Carefull: paths might get broken with this silly rotorcraft/fixedwing mixup of directories
+
+ifeq ($(ARCH), stm32)
+$(TARGET).srcs += firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
+$(TARGET).CFLAGS += -Ifirmwares/rotorcraft/actuators/arch/stm32
+endif
diff --git a/conf/messages.xml b/conf/messages.xml
index 3a69a1fb30..ed2ade3e62 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1615,6 +1615,7 @@
+
diff --git a/conf/modules/digital_cam.xml b/conf/modules/digital_cam.xml
index 8507667bfd..f03907a0b3 100644
--- a/conf/modules/digital_cam.xml
+++ b/conf/modules/digital_cam.xml
@@ -1,21 +1,42 @@
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/digital_cam_i2c.xml b/conf/modules/digital_cam_i2c.xml
new file mode 100644
index 0000000000..17a91367ce
--- /dev/null
+++ b/conf/modules/digital_cam_i2c.xml
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/dc.xml b/conf/settings/dc.xml
index 6cb5d9c9a7..b2e44f1e56 100644
--- a/conf/settings/dc.xml
+++ b/conf/settings/dc.xml
@@ -1,21 +1,24 @@
-
-
-
-
-
-
-
-
-
-
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/lpc21/lpcusb/Makefile b/sw/airborne/arch/lpc21/lpcusb/Makefile
index b52b324be7..505dae4dbc 100644
--- a/sw/airborne/arch/lpc21/lpcusb/Makefile
+++ b/sw/airborne/arch/lpc21/lpcusb/Makefile
@@ -5,7 +5,7 @@ PKG_NAME = target
DATE = $$(date +%Y%m%d)
# Tool definitions
-HAVE_ARM_NONE_EABI_GCC := $(wildcard /usr/bin/arm-none-eabi-gcc)
+HAVE_ARM_NONE_EABI_GCC := $(shell which arm-none-eabi-gcc)
ifeq ($(strip $(HAVE_ARM_NONE_EABI_GCC)),)
CC = arm-elf-gcc
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 247fd9cd34..839a0da20b 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -33,7 +33,7 @@
#include "estimator.h"
#include "subsystems/nav.h"
#include "generated/airframe.h"
-#include "autopilot.h"
+#include "firmwares/fixedwing/autopilot.h"
/* mode */
uint8_t v_ctl_mode;
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 60f214351b..b0954cc92d 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -45,7 +45,7 @@
#include "gyro.h"
#include "ap_downlink.h"
#include "subsystems/nav.h"
-#include "autopilot.h"
+#include "firmwares/fixedwing/autopilot.h"
#include "estimator.h"
#include "generated/settings.h"
#include "link_mcu.h"
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index d3954ce44d..ed29dab3e6 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -50,7 +50,7 @@
#include "firmwares/fixedwing/actuators.h"
#include "subsystems/radio_control.h"
#include "fbw_downlink.h"
-#include "autopilot.h"
+#include "firmwares/fixedwing/autopilot.h"
#include "paparazzi.h"
#include "estimator.h"
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
index 8344339847..4b138cd8b0 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -36,7 +36,7 @@
#include "subsystems/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "autopilot.h"
+#include "firmwares/fixedwing/autopilot.h"
/* outer loop parameters */
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index adca6d09b3..fddcc79b98 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -63,6 +63,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&imu_nb_err, \
&_twi_blmc_nb_err, \
&radio_control.status, \
+ &radio_control.frame_rate, \
&booz_gps_state.fix, \
&autopilot_mode, \
&autopilot_in_flight, \
@@ -82,6 +83,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&imu_nb_err, \
&twi_blmc_nb_err, \
&radio_control.status, \
+ &radio_control.frame_rate, \
&fix, \
&autopilot_mode, \
&autopilot_in_flight, \
diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h
index 01dfa737d5..06ffadced9 100644
--- a/sw/airborne/gps.h
+++ b/sw/airborne/gps.h
@@ -33,6 +33,10 @@
#include "std.h"
#include "gps_hw.h"
+#ifdef GPS
+#error "The flag GPS has been deprecated. Please replace it with USE_GPS."
+#endif
+
#ifdef UBX
#include "gps_ubx.h"
#elif defined USE_GPS_XSENS
diff --git a/sw/airborne/gyro.h b/sw/airborne/gyro.h
index 5fa68ba9bc..2974ba79dc 100644
--- a/sw/airborne/gyro.h
+++ b/sw/airborne/gyro.h
@@ -32,6 +32,10 @@
#include
+#ifdef GYRO
+#error "The flag GYRO has been deprecated. Please replace it with USE_GYRO."
+#endif
+
/** Raw (for debug), taking into accound neutral and temp compensation (if any) */
extern int16_t roll_rate_adc;
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index 584ae04aad..543de22d11 100644
--- a/sw/airborne/infrared.c
+++ b/sw/airborne/infrared.c
@@ -42,6 +42,9 @@
#if defined IR_ESTIMATED_PHI_PI_4 || defined IR_ESTIMATED_PHI_MINUS_PI_4 || defined IR_ESTIMATED_THETA_PI_4
#error "IR_ESTIMATED_PHI_PI_4 correction has been deprecated. Please remove the definition from your airframe config file"
#endif
+#ifdef INFRARED
+#error "The flag INFRARED has been deprecated. Please replace it with USE_INFRARED."
+#endif
int16_t ir_ir1;
diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
new file mode 100644
index 0000000000..645cbe1294
--- /dev/null
+++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
@@ -0,0 +1,101 @@
+/*
+ * 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.
+ *
+ */
+
+
+/** \file atmega_i2c_cam_ctrl.c
+ * \brief Interface with digital camera though AVR AtMega chip
+ *
+ * Send Commands over I2C
+ */
+
+
+#include "atmega_i2c_cam_ctrl.h"
+
+#include "i2c.h"
+#include "led.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include "estimator.h"
+
+
+static struct i2c_transaction atmega_i2c_cam_ctrl_trans;
+
+#ifndef ATMEGA_I2C_DEV
+#define ATMEGA_I2C_DEV i2c0
+#endif
+
+
+#ifndef ATMEGA_SLAVE_ADDR
+#define ATMEGA_SLAVE_ADDR 0x68
+#endif
+
+uint8_t atmega_i2c_cam_ctrl_just_sent_command = 0;
+
+void atmega_i2c_cam_ctrl_init(void)
+{
+ atmega_i2c_cam_ctrl_trans.status = I2CTransDone;
+ dc_init();
+}
+
+void atmega_i2c_cam_ctrl_periodic (void)
+{
+ atmega_i2c_cam_ctrl_just_sent_command = 0;
+ dc_periodic_4Hz();
+
+ // Request Status
+ if (atmega_i2c_cam_ctrl_just_sent_command == 0)
+ {
+ atmega_i2c_cam_ctrl_send(DC_GET_STATUS);
+ }
+}
+
+
+
+void atmega_i2c_cam_ctrl_send(uint8_t cmd)
+{
+ atmega_i2c_cam_ctrl_just_sent_command = 1;
+
+ // Send Command
+ atmega_i2c_cam_ctrl_trans.buf[0] = cmd;
+ I2CTransceive(ATMEGA_I2C_DEV, atmega_i2c_cam_ctrl_trans, ATMEGA_SLAVE_ADDR, 1, 1);
+
+ if (cmd == DC_SHOOT)
+ {
+ dc_send_shot_position();
+ }
+}
+
+void atmega_i2c_cam_ctrl_event( void )
+{
+ if (atmega_i2c_cam_ctrl_trans.status == I2CTransSuccess)
+ {
+ unsigned char cam_ret[1];
+ cam_ret[0] = atmega_i2c_cam_ctrl_trans.buf[0];
+ RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, 1, cam_ret ));
+ atmega_i2c_cam_ctrl_trans.status = I2CTransDone;
+ }
+}
diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h
new file mode 100644
index 0000000000..e825df002b
--- /dev/null
+++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h
@@ -0,0 +1,51 @@
+/*
+ * 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.
+ *
+ */
+
+#ifndef ATMEGA_I2C_CAM_CTRL_H
+#define ATMEGA_I2C_CAM_CTRL_H
+
+// Include Standard Camera Control Interface
+#include "dc.h"
+
+
+void atmega_i2c_cam_ctrl_init(void);
+void atmega_i2c_cam_ctrl_periodic(void);
+void atmega_i2c_cam_ctrl_event(void);
+void atmega_i2c_cam_ctrl_send(uint8_t cmd);
+
+// In I2C mode we can not inline this function:
+static inline void dc_send_command(uint8_t cmd)
+{
+ atmega_i2c_cam_ctrl_send(cmd);
+}
+
+// Allow commands to be set by datalink
+#define ParseCameraCommand() { \
+ { \
+ if ( DL_PAYLOAD_COMMAND_command_length(dl_buffer) == 1){ \
+ dc_send_command(DL_PAYLOAD_COMMAND_command(dl_buffer)[0]); \
+ } \
+ } \
+}
+
+
+#endif
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index 4d0ba12a99..29543af74a 100644
--- a/sw/airborne/modules/digital_cam/dc.c
+++ b/sw/airborne/modules/digital_cam/dc.c
@@ -1,37 +1,91 @@
+/*
+ * 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 "dc.h"
-uint8_t dc_timer;
-uint8_t dc_periodic_shutter;
-uint8_t dc_shutter_timer;
-uint8_t dc_utm_threshold;
+// Variables with boot defaults
+uint8_t dc_autoshoot_meter_grid = 100;
+uint8_t dc_autoshoot_quartersec_period = 2;
+dc_autoshoot_type dc_autoshoot = DC_AUTOSHOOT_STOP;
+
+
+
+
+#ifdef SENSOR_SYNC_SEND
+
uint16_t dc_photo_nr = 0;
-uint8_t dc_shoot = 0;
-
-
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#include "uart.h"
#include "messages.h"
#include "downlink.h"
+#include "estimator.h"
+
+ void dc_send_shot_position(void)
+ {
+ int16_t phi = DegOfRad(estimator_phi*10.0f);
+ int16_t theta = DegOfRad(estimator_theta*10.0f);
+ float gps_z = ((float)gps_alt) / 100.0f;
+ DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow);
+ dc_photo_nr++;
+ }
+
+#endif
-void dc_send_shot_position(void)
-{
- int16_t phi = DegOfRad(estimator_phi*10.0f);
- int16_t theta = DegOfRad(estimator_theta*10.0f);
- float gps_z = ((float)gps_alt) / 100.0f;
- DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow);
- dc_photo_nr++;
-}
-
-uint8_t dc_shutter( void )
-{
- dc_timer = SHUTTER_DELAY;
- DC_PUSH(DC_SHUTTER_LED);
- dc_send_shot_position();
-
- return 0;
+
+/*
+#ifndef DC_GPS_TRIGGER_START
+#define DC_GPS_TRIGGER_START 1
+#endif
+#ifndef DC_GPS_TRIGGER_STOP
+#define DC_GPS_TRIGGER_STOP 3
+#endif
+
+
+static inline void dc_shoot_on_gps( void ) {
+ static uint8_t gps_msg_counter = 0;
+
+ if (dc_shoot > 0)
+ {
+
+ if (gps_msg_counter == 0)
+ {
+ DC_PUSH(DC_SHUTTER_LED);
+
+ dc_send_shot_position();
+ }
+ else if (gps_msg_counter == DC_GPS_TRIGGER_START)
+ {
+ DC_RELEASE(DC_SHUTTER_LED);
+ }
+
+ gps_msg_counter++;
+ if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
+ gps_msg_counter = 0;
+ }
}
+*/
diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h
index 9d5bb6e063..06fc9d1f0e 100644
--- a/sw/airborne/modules/digital_cam/dc.h
+++ b/sw/airborne/modules/digital_cam/dc.h
@@ -1,7 +1,5 @@
/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2003-2008 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -22,29 +20,18 @@
*
*/
+
/** \file dc.h
- * \brief Digital Camera Control
+ * \brief Standard Digital Camera Control Interface
*
- * Provides the control of the shutter and the zoom of a digital camera
- * through standard binary IOs of the board.
+ * -Standard IO
+ * -I2C Control
*
- * Configuration:
- * Since the API of led.h is used, connected pins must be defined as led
- * numbers (usually in the airframe file):
- *
- *
- * Related bank and pin must also be defined:
- *
- *
- * The required initialization (dc_init()) and periodic (4Hz) process
- * (dc_periodic()) are called if the DIGITAL_CAM flag is set:
- * ap.CFLAGS += -DDIGITAL_CAM
- *
- * Usage (from the flight plan, the settings or any airborne code):
- * - dc_Shutter(_) sets the DC_SHUTTER_LED pin output to 1 for 0.5s and sends
- * a DC_SHOT message
- * - dc_Zoom(_) sets the DC_ZOOM_LED pin output to 1 for 0.5s
- * - dc_Periodic(s) activates a periodic call to dc_Shutter() every s seconds
+ * Usage: (from the flight plan, the settings or any airborne code):
+ * - dc_send_command( )
+ * - set the appropriate autoshoot mode (off/time/distance/trigger)
+ * - use the module periodic function to set the autorepeat interval
+ * - define SENSOR_SYNC_SEND to get the DC_SHOT_MESSAGE on every SHOOT command
*/
#ifndef DC_H
@@ -53,124 +40,111 @@
#include "std.h"
#include "led.h"
#include "generated/airframe.h"
-#include "estimator.h"
#include "gps.h"
-extern uint8_t dc_timer;
+/* Generic Set of Digital Camera Commands */
+typedef enum {
+ DC_GET_STATUS = 0,
-extern uint8_t dc_periodic_shutter;
-/* In s. If non zero, period of automatic calls to dc_shutter() */
-extern uint8_t dc_shutter_timer;
-/* In s. Related counter */
+ DC_HOLD = 13,
+ DC_SHOOT = 32,
-extern uint8_t dc_utm_threshold;
-/* In m. If non zero, automatic shots when greater than utm_north % 100 */
+ DC_WIDER = 'w',
+ DC_TALLER = 't',
-/* Picture Number starting from zero */
-extern uint16_t dc_photo_nr;
-extern uint8_t dc_shoot;
+ DC_UP = 'u',
+ DC_DOWN = 'd',
+ DC_CENTER = 'c',
+ DC_LEFT = 'l',
+ DC_RIGHT = 'r',
-#ifndef DC_PUSH
-#define DC_PUSH LED_ON
-#endif
+ DC_MENU = 'm',
+ DC_HOME = 'h',
+ DC_PLAY = 'p',
-#ifndef DC_RELEASE
-#define DC_RELEASE LED_OFF
-#endif
+ DC_ON = 'O',
+ DC_OFF = 'o',
-#define SHUTTER_DELAY 2 /* 4Hz -> 0.5s */
+} dc_command_type;
-uint8_t dc_shutter( void );
+/* Send Command To Camera */
+static inline void dc_send_command(uint8_t cmd);
-static inline uint8_t dc_zoom( void ) {
- dc_timer = SHUTTER_DELAY;
-#ifdef DC_ZOOM_LED
- DC_PUSH(DC_ZOOM_LED);
-#endif
- return 0;
-}
+/* Auotmatic Digital Camera Photo Triggering */
+typedef enum {
+ DC_AUTOSHOOT_STOP = 0,
+ DC_AUTOSHOOT_PERIODIC = 1,
+ DC_AUTOSHOOT_DISTANCE = 2,
+ DC_AUTOSHOOT_EXT_TRIG = 3
+} dc_autoshoot_type;
+extern dc_autoshoot_type dc_autoshoot;
-#define dc_Shutter(_) ({ dc_shutter(); 0; })
-#define dc_Zoom(_) ({ dc_zoom(); 0; })
-#define dc_Periodic(s) ({ dc_periodic_shutter = s; dc_shutter_timer = s; 0; })
+/* AutoShoot photos every X quarter_second */
+extern uint8_t dc_autoshoot_quartersec_period;
-#ifndef DC_PERIODIC_SHUTTER
-#define DC_PERIODIC_SHUTTER 0
-#endif
-
-#define dc_init() { /* initialized as leds */ dc_periodic_shutter = DC_PERIODIC_SHUTTER; DC_PUSH(DC_SHUTTER_LED);} /* Output */
-
-
-#ifndef DC_GPS_TRIGGER_START
-#define DC_GPS_TRIGGER_START 1
-#endif
-#ifndef DC_GPS_TRIGGER_STOP
-#define DC_GPS_TRIGGER_STOP 3
-#endif
+/* AutoShoot photos on a X meter Local Tangent Plane Grid */
+extern uint8_t dc_autoshoot_meter_grid;
+/* Send Down the coordinates of where the photo was taken */
+#ifdef SENSOR_SYNC_SEND
void dc_send_shot_position(void);
-
-static inline void dc_shoot_on_gps( void ) {
- static uint8_t gps_msg_counter = 0;
-
- if (dc_shoot > 0)
- {
-
- if (gps_msg_counter == 0)
- {
- DC_PUSH(DC_SHUTTER_LED);
-
- dc_send_shot_position();
- }
- else if (gps_msg_counter == DC_GPS_TRIGGER_START)
- {
- DC_RELEASE(DC_SHUTTER_LED);
- }
-
- gps_msg_counter++;
- if (gps_msg_counter >= DC_GPS_TRIGGER_STOP)
- gps_msg_counter = 0;
- }
-}
-
-/* 4Hz */
-static inline void dc_periodic( void ) {
- if (dc_timer) {
- dc_timer--;
- } else {
- DC_RELEASE(DC_SHUTTER_LED);
-#ifdef DC_ZOOM_LED
- DC_RELEASE(DC_ZOOM_LED);
+#else
+#define dc_send_shot_position() {}
#endif
- }
- if (dc_shoot > 0)
+/******************************************************************
+ * FUNCTIONS
+ *****************************************************************/
+
+/* get settings */
+static inline void dc_init(void)
+{
+#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
+ dc_autoshoot_quartersec_period = DC_AUTOSHOOT_QUARTERSEC_PERIOD;
+#endif
+#ifdef DC_AUTOSHOOT_METER_GRID
+ dc_autoshoot_meter_grid = DC_AUTOSHOOT_METER_GRID;
+#endif
+}
+
+/* shoot on grid */
+static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
+{
+ uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
+ if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
{
- if (dc_periodic_shutter) {
- RunOnceEvery(2,
- {
- if (dc_shutter_timer) {
- dc_shutter_timer--;
- } else {
- dc_shutter();
- dc_shutter_timer = dc_periodic_shutter;
- }
- });
+ dc_send_command(DC_SHOOT);
+ }
+}
+
+/* periodic 4Hz function */
+static inline void dc_periodic_4Hz( void )
+{
+ static uint8_t dc_shutter_timer = 0;
+
+#ifdef DC_AUTOSHOOT_QUARTERSEC_PERIOD
+ if (dc_autoshoot == DC_AUTOSHOOT_PERIODIC)
+ {
+ if (dc_shutter_timer)
+ {
+ dc_shutter_timer--;
+ } else {
+ dc_send_command(DC_SHOOT);
+ dc_shutter_timer = dc_autoshoot_quartersec_period;
}
}
- else
+#endif
+#ifdef DC_AUTOSHOOT_METER_GRID
+ if (dc_autoshoot == DC_AUTOSHOOT_DISTANCE)
{
- dc_shutter_timer = 0;
+ // Shoot
+ dc_shot_on_utm_north_close_to_100m_grid();
}
+#endif
}
-static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) {
- if (dc_utm_threshold && !dc_timer) {
- uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100;
- if (dist_to_100m_grid < dc_utm_threshold || 100 - dist_to_100m_grid < dc_utm_threshold)
- dc_shutter();
- }
-}
+
+
#endif // DC_H
diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.c b/sw/airborne/modules/digital_cam/led_cam_ctrl.c
new file mode 100644
index 0000000000..af9acb254a
--- /dev/null
+++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.c
@@ -0,0 +1,33 @@
+/*
+ * 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 "led_cam_ctrl.h"
+
+// Include Digital IO
+#include "led.h"
+
+// Button Timer
+uint8_t dc_timer;
+
+
+
+
diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h
new file mode 100644
index 0000000000..fcf6a17b88
--- /dev/null
+++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.h
@@ -0,0 +1,132 @@
+/*
+ * 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.
+ *
+ */
+
+
+/** \file led_cam_ctrl.h
+ * \brief Digital Camera Control
+ *
+ * Provides the control of the shutter and the zoom of a digital camera
+ * through standard binary IOs of the board.
+ *
+ * Configuration:
+ * Since the API of led.h is used, connected pins must be defined as led
+ * numbers (usually in the airframe file):
+ *
+ *
+ *
+ *
+ * Related bank and pin must also be defined:
+ *
+ *
+ * The required initialization (dc_init()) and periodic (4Hz) process
+ *
+ */
+
+#ifndef LED_CAM_CTRL_H
+#define LED_CAM_CTRL_H
+
+// Include Standard Camera Control Interface
+#include "dc.h"
+
+extern uint8_t dc_timer;
+
+static inline void led_cam_ctrl_init(void)
+{
+ // Call common DC init
+ dc_init();
+
+ // Do LED specific DC init
+ dc_timer = 0;
+}
+
+#ifndef DC_PUSH
+#define DC_PUSH LED_ON
+#endif
+
+#ifndef DC_RELEASE
+#define DC_RELEASE LED_OFF
+#endif
+
+#ifndef SHUTTER_DELAY
+#define SHUTTER_DELAY 2 /* 4Hz -> 0.5s */
+#endif
+
+#ifndef DC_SHUTTER_LED
+#error DC: Please specify at least a SHUTTER LED
+#endif
+
+/* Command The Camera */
+static inline void dc_send_command(uint8_t cmd)
+{
+ dc_timer = SHUTTER_DELAY;
+ switch (cmd)
+ {
+ case DC_SHOOT:
+ DC_PUSH(DC_SHUTTER_LED);
+ dc_send_shot_position();
+ break;
+#ifdef DC_ZOOM_IN_LED
+ case DC_ZOOM_IN:
+ DC_PUSH(DC_ZOOM_IN_LED);
+ break;
+#endif
+#ifdef DC_ZOOM_OUT_LED
+ case DC_ZOOM_OUT:
+ DC_PUSH(DC_ZOOM_OUT_LED);
+ break;
+#endif
+#ifdef DC_POWER_LED
+ case DC_POWER:
+ DC_PUSH(DC_POWER_LED);
+ break;
+#endif
+ }
+}
+
+
+/* 4Hz Periodic */
+static inline void led_cam_ctrl_periodic( void )
+{
+ if (dc_timer) {
+ dc_timer--;
+ } else {
+ DC_RELEASE(DC_SHUTTER_LED);
+#ifdef DC_ZOOM_IN_LED
+ DC_RELEASE(DC_ZOOM_IN_LED);
+#endif
+#ifdef DC_ZOOM_OUT_LED
+ DC_RELEASE(DC_ZOOM_OUT_LED);
+#endif
+#ifdef DC_POWER_LED
+ DC_RELEASE(DC_POWER_LED);
+#endif
+ }
+
+ // Common DC Periodic task
+ dc_periodic_4Hz();
+}
+
+
+
+
+
+#endif // DC_H
diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
new file mode 100644
index 0000000000..c682e4ae37
--- /dev/null
+++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
@@ -0,0 +1,92 @@
+/*
+ * 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.
+ *
+ */
+
+
+/** \file sim_i2c_cam_ctrl.c
+ * \brief Simulated Interface with digital camera
+ *
+ */
+
+
+#include "atmega_i2c_cam_ctrl.h"
+
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include "estimator.h"
+
+
+void atmega_i2c_cam_ctrl_init(void)
+{
+ dc_init();
+}
+
+void atmega_i2c_cam_ctrl_periodic (void)
+{
+ dc_periodic_4Hz();
+
+ // Request Status
+ dc_send_command(DC_GET_STATUS);
+}
+
+
+
+void atmega_i2c_cam_ctrl_send(uint8_t cmd)
+{
+ static uint8_t zoom = 0;
+ static uint8_t mode = 0;
+ unsigned char cam_ret[1];
+
+ if (cmd == DC_SHOOT)
+ {
+ dc_send_shot_position();
+ }
+ else if (cmd == DC_TALLER)
+ {
+ zoom = 1;
+ }
+ else if (cmd == DC_WIDER)
+ {
+ zoom = 0;
+ }
+ else if (cmd == DC_GET_STATUS)
+ {
+ mode++;
+ if (mode > 15)
+ mode = 0;
+ }
+
+ cam_ret[0] = mode + zoom * 0x20;
+ RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, 1, cam_ret ));
+
+}
+
+void atmega_i2c_cam_ctrl_event( void )
+{
+}
+
+
+
diff --git a/sw/ground_segment/tmtc/booz_server.ml b/sw/ground_segment/tmtc/booz_server.ml
index 98187d7f75..a076cb9d27 100644
--- a/sw/ground_segment/tmtc/booz_server.ml
+++ b/sw/ground_segment/tmtc/booz_server.ml
@@ -182,6 +182,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
Wind.update ac_name a.gspeed a.course
| "ROTORCRAFT_STATUS" ->
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
+ a.fbw.rc_rate <- ivalue "frame_rate";
a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
a.ap_mode <- check_index (get_pprz_mode (ivalue "ap_mode")) ap_modes "BOOZ_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 1375c9dd28..ce4a4fe2ba 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -103,6 +103,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.last_msg_date <- U.gettimeofday ();
match msg.Pprz.name with
"GPS" ->
+ a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
+ if a.gps_mode = _3D then begin
let p = { LL.utm_x = fvalue "utm_east" /. 100.;
utm_y = fvalue "utm_north" /. 100.;
utm_zone = ivalue "utm_zone" } in
@@ -114,9 +116,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
if !heading_from_course then
a.heading <- a.course;
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
- a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course
+ end
| "GPS_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
@@ -143,9 +145,6 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
Some nav_ref ->
let x = (try fvalue "x" with _ -> fvalue "desired_x")
and y = (try fvalue "y" with _ -> fvalue "desired_y") in
- (*let target_utm = LL.utm_add (LL.utm_of nav_ref) (x, y) in
- let target_geo = LL.of_utm WGS84 target_utm in
- a.desired_pos <- target_geo;*)
a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
| None -> ()
end;
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 33dd4fd378..a66e209bad 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -151,8 +151,7 @@ let send_cam_status = fun a ->
let alpha = -. a.course in
let east = dx *. cos alpha -. dy *. sin alpha
and north = dx *. sin alpha +. dy *. cos alpha in
- let utm = LL.utm_add (LL.utm_of WGS84 a.pos) (east, north) in
- let wgs84 = LL.of_utm WGS84 utm in
+ let wgs84 = Aircraft.add_pos_to_nav_ref (Geo a.pos) (east, north) in
let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in
let values = ["ac_id", Pprz.String a.id;
"cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index 4b6d968488..0255bca0b1 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -8,6 +8,7 @@ let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
+let _3D = 3
let gps_hybrid_modes = [|"OFF";"ON"|]
let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]
diff --git a/sw/in_progress/antenna_track/Makefile b/sw/in_progress/antenna_track/Makefile
new file mode 100644
index 0000000000..87f092656e
--- /dev/null
+++ b/sw/in_progress/antenna_track/Makefile
@@ -0,0 +1,33 @@
+#
+# $Id$
+#
+# 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.
+#
+
+# Quiet compilation
+Q=@
+
+CC = gcc
+CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` $(FPIC)
+LDFLAGS=`pkg-config gtk+-2.0 --libs` -s `pcre-config --libs` -lglibivy
+
+ant_track_pmm : ant_track_pmm.c
+ $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
+
+clean:
+ rm -f *~* *.cm* *.o *.out *.opt .depend
diff --git a/sw/in_progress/antenna_track/ant_track_Pololu.c b/sw/in_progress/antenna_track/ant_track_Pololu.c
deleted file mode 100644
index a7f628f5fb..0000000000
--- a/sw/in_progress/antenna_track/ant_track_Pololu.c
+++ /dev/null
@@ -1,344 +0,0 @@
-/*
- * Paparazzi $Id: ant_track.c 2009-10-30 12:14:20Z griffin $
- *
- * Copyright (C) 2009 - Pascal Brisset, Antoine Drouin
- *
- * Modified by: Mark Griffin and Todd Sandercock
- *
- * 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
-#include /* UNIX standard function definitions */
-#include /* File control definitions */
-#include /* Error number definitions */
-
-#define MANUAL 0
-#define AUTO 1
-
-static double gps_pos_x;
-static double gps_pos_y;
-static double gps_alt;
-static double home_alt;
-static double ant_azim;
-static double ant_elev;
-static int mode;
-static int home_found;
-
-int fd; /* File descriptor for the port */
-
-double hfov = 170., vfov = 170.;
-double hnp = 270., vnp = 30.;
-double hlim1 = 30, hlim2 = 200;
-double vlim1 = 30, vlim2 = 200;
-
-unsigned char startByte = 0x80, deviceId = 0x01, command = 0x03, commandinit =
- 0x00, psiServo = 0x01, thetaServo = 0x02, speed = 0x00, speedCmd = 0x01;
-unsigned char datainit = 0x40;
-unsigned char data1 = 19, data2 = 68;
-
-GtkWidget *azim_scale;
-GtkWidget *elev_scale;
-
-void on_mode_changed(GtkRadioButton *radiobutton, gpointer user_data) {
- mode
- = gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)) ? MANUAL
- : AUTO;
- //IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
- //g_message("Mode changed to %d" , mode);
-}
-
-#define GLADE_HOOKUP_OBJECT(component,widget,name) \
- g_object_set_data_full (G_OBJECT (component), name, \
- gtk_widget_ref (widget), (GDestroyNotify) gtk_widget_unref)
-
-#define GLADE_HOOKUP_OBJECT_NO_REF(component,widget,name) \
- g_object_set_data (G_OBJECT (component), name, widget)
-
-GtkWidget* build_gui(void) {
- GtkWidget *window1;
- GtkWidget *vbox1;
- GtkWidget *vbox2;
- GtkWidget *table1;
- GtkWidget *label1;
- GtkWidget *label2;
- GtkWidget *label3;
- GtkWidget *label4;
- GtkWidget *radiobutton1;
- GSList *radiobutton1_group = NULL;
- GtkWidget *radiobutton2;
- GtkWidget *entry1;
-
- window1 = gtk_window_new(GTK_WINDOW_TOPLEVEL);
- gtk_window_set_title(GTK_WINDOW (window1), "tracking antenna");
-
- vbox1 = gtk_vbox_new(FALSE, 0);
- gtk_widget_show(vbox1);
- gtk_container_add(GTK_CONTAINER (window1), vbox1);
-
- vbox2 = gtk_vbox_new(FALSE, 0);
- gtk_widget_show(vbox2);
- gtk_box_pack_start(GTK_BOX (vbox1), vbox2, TRUE, TRUE, 0);
-
- table1 = gtk_table_new(4, 3, FALSE);
- gtk_widget_show(table1);
- gtk_box_pack_start(GTK_BOX (vbox2), table1, TRUE, TRUE, 0);
- gtk_table_set_col_spacings(GTK_TABLE (table1), 5);
-
- label1 = gtk_label_new("Azimuth");
- gtk_widget_show(label1);
- gtk_table_attach(GTK_TABLE (table1), label1, 0, 1, 1, 2,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_misc_set_alignment(GTK_MISC (label1), 0, 0.5);
-
- label2 = gtk_label_new("Elevation");
- gtk_widget_show(label2);
- gtk_table_attach(GTK_TABLE (table1), label2, 0, 1, 2, 3,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_misc_set_alignment(GTK_MISC (label2), 0, 0.5);
-
- label3 = gtk_label_new("Id");
- gtk_widget_show(label3);
- gtk_table_attach(GTK_TABLE (table1), label3, 0, 1, 3, 4,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_misc_set_alignment(GTK_MISC (label3), 0, 0.5);
-
- label4 = gtk_label_new("mode");
- gtk_widget_show(label4);
- gtk_table_attach(GTK_TABLE (table1), label4, 0, 1, 0, 1,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_misc_set_alignment(GTK_MISC (label4), 0, 0.5);
-
- radiobutton1 = gtk_radio_button_new_with_mnemonic(NULL, "manual");
- gtk_widget_show(radiobutton1);
- gtk_table_attach(GTK_TABLE (table1), radiobutton1, 1, 2, 0, 1,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton1),
- radiobutton1_group);
- radiobutton1_group = gtk_radio_button_get_group(
- GTK_RADIO_BUTTON (radiobutton1));
-
- radiobutton2 = gtk_radio_button_new_with_mnemonic(NULL, "tracking");
- gtk_widget_show(radiobutton2);
- gtk_table_attach(GTK_TABLE (table1), radiobutton2, 2, 3, 0, 1,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
- gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton2),
- radiobutton1_group);
- radiobutton1_group = gtk_radio_button_get_group(
- GTK_RADIO_BUTTON (radiobutton2));
-
- azim_scale = gtk_hscale_new(
- GTK_ADJUSTMENT (gtk_adjustment_new (144.7, 0, 360, 1, 1, 1)));
- gtk_widget_show(azim_scale);
- gtk_table_attach(GTK_TABLE (table1), azim_scale, 1, 3, 1, 2,
- (GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
- (GtkAttachOptions) (GTK_FILL), 0, 0);
- gtk_range_set_update_policy(GTK_RANGE (azim_scale), GTK_UPDATE_DELAYED);
-
- elev_scale = gtk_hscale_new(
- GTK_ADJUSTMENT (gtk_adjustment_new (32.3, 0, 90, 1, 1, 1)));
- gtk_widget_show(elev_scale);
- gtk_table_attach(GTK_TABLE (table1), elev_scale, 1, 3, 2, 3,
- (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (GTK_FILL), 0, 0);
-
- entry1 = gtk_entry_new();
- gtk_widget_show(entry1);
- gtk_table_attach(GTK_TABLE (table1), entry1, 1, 3, 3, 4,
- (GtkAttachOptions) (GTK_EXPAND | GTK_FILL), (GtkAttachOptions) (0),
- 0, 0);
-
- g_signal_connect ((gpointer) radiobutton1, "toggled",
- G_CALLBACK (on_mode_changed),
- NULL);
-
- /* Store pointers to all widgets, for use by lookup_widget(). */
- GLADE_HOOKUP_OBJECT_NO_REF (window1, window1, "window1");
- GLADE_HOOKUP_OBJECT (window1, vbox1, "vbox1");
- GLADE_HOOKUP_OBJECT (window1, vbox2, "vbox2");
- GLADE_HOOKUP_OBJECT (window1, table1, "table1");
- GLADE_HOOKUP_OBJECT (window1, label1, "label1");
- GLADE_HOOKUP_OBJECT (window1, label2, "label2");
- GLADE_HOOKUP_OBJECT (window1, label3, "label3");
- GLADE_HOOKUP_OBJECT (window1, label4, "label4");
- GLADE_HOOKUP_OBJECT (window1, radiobutton1, "radiobutton1");
- GLADE_HOOKUP_OBJECT (window1, radiobutton2, "radiobutton2");
- GLADE_HOOKUP_OBJECT (window1, entry1, "entry1");
-
- return window1;
-}
-
-/* jump here when a GPS message is received */
-void on_GPS_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
- if (home_found == 0) {
- if (atof(argv[0]) == 3) { /* wait until we have a valid GPS fix */
- home_alt = atof(argv[4]) / 100.; /* get the altitude */
- home_found = 1;
- }
- }
- gps_alt = atof(argv[4]) / 100.;
-}
-
-void send_pos(int vert, int hori) {
- // Split the value into two 7 bit numbers
- data1 = vert / 128;
- data2 = vert % 128;
-
- // Command buffer for "Set Position, 8-bit (2 data bytes)" from Pololu servo controller data sheet
- // Send vertical servo
- char buffer1[] = { startByte, deviceId, command, psiServo, data1, data2 };
-
- write(fd, buffer1, 6);
-
- // Split the value into two 7 bit numbers
- data1 = hori / 128;
- data2 = hori % 128;
-
- // Command buffer for "Set Position, 8-bit (2 data bytes)" from Pololu servo controller data sheet
- // Send horizontal servo
- char buffer2[] = { startByte, deviceId, command, thetaServo, data1, data2 };
-
- write(fd, buffer2, 6);
-}
-
-/* jump here when a NAVIGATION message is received */
-void on_NAV_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
- double hpos, vpos;
- double hservo, vservo;
-
- if (mode == AUTO) {
- gps_pos_x = atof(argv[2]);
- gps_pos_y = atof(argv[3]);
- /* calculate azimuth */
- ant_azim = atan2(gps_pos_x, gps_pos_y) * 180. / M_PI;
-
- if (ant_azim < 0)
- ant_azim += 360.;
-
- /* calculate elevation */
- ant_elev = atan2((gps_alt - home_alt), sqrt(atof(argv[5]))) * 180.
- / M_PI;
- if (ant_elev < 0)
- ant_elev = 0.;
-
- gtk_range_set_value(azim_scale, ant_azim);
- gtk_range_set_value(elev_scale, ant_elev);
- }
-
- // The magic is done here
-
- // First take the horizontal angle relative to the neutral point "hnp"
- hpos = ant_azim - hnp;
-
- // Keep the range between (-180,180). this is done so that it consistently swaps sides
- if (hpos < -180) {
- hpos += 360;
- } else if (hpos > 180) {
- hpos -= 360;
- }
-
- // keep the range within the field of view "hfov"
- if (hpos > (hfov / 2)) {
- hpos = hfov / 2;
- } else if (-hpos > (hfov / 2)) {
- hpos = -hfov / 2;
- }
-
- // Take the vertical angle relative to the neutral point "vnp"
- vpos = ant_elev - vnp;
-
- // keep within the field of view "vfov"
- if (vpos > (vfov / 2)) {
- vpos = vfov / 2;
- } else if (-vpos > (vfov / 2)) {
- vpos = -vfov / 2;
- }
-
- // make outputs relative to limits for the Pololu board
-
- vservo = (((vpos + (vfov / 2)) / vfov) * (vlim2 - vlim1)) + vlim1;
- hservo = (((hpos + (hfov / 2)) / hfov) * (hlim2 - hlim1)) + hlim1;
-
- /*g_message("home_alt %f gps_alt %f azim %f elev %f", home_alt, gps_alt, ant_azim, ant_elev); */
-
- // Send servo position.
- send_pos(vservo, hservo);
-
- g_message("vservo %f hservo %f", vservo, hservo);
-}
-
-int open_port(void) {
- struct termios options;
-
- // would probably be good to set the port up as an arg.
- fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
- if (fd == -1) {
- perror("open_port: Unable to open /dev/ttyUSB0 - ");
- } else
- fcntl(fd, F_SETFL, 0);
-
- tcgetattr(fd, &options);
-
- // Set the baud rates to 19200. This can be between 2,000 to 40,000
-
- cfsetispeed(&options, B19200);
- cfsetospeed(&options, B19200);
-
- options.c_cflag |= (CLOCAL | CREAD);
-
- tcsetattr(fd, TCSANOW, &options);
-
- // Send initialisation to the pololu board. By changing "speed" the speed of the servo can be changed
- // if "speed" is nonzero then 1 is the slowest 127 is the fastest. 0 = no speed restriction
- char buffer[] = { startByte, deviceId, speedCmd, psiServo, speed,
- startByte, deviceId, speedCmd, thetaServo, speed};
-
- write(fd, buffer, 10);
-
- return (fd);
-}
-
-int main(int argc, char** argv) {
-
- gtk_init(&argc, &argv);
-
- GtkWidget* window = build_gui();
- gtk_widget_show_all(window);
-
- open_port();
-
- IvyInit("AntennaTracker", "AntennaTracker READY", NULL, NULL, NULL, NULL);
- IvyBindMsg(
- on_GPS_STATUS,
- NULL,
- "^\\S* GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
- IvyBindMsg(on_NAV_STATUS, NULL,
- "^\\S* NAVIGATION (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
- IvyStart("127.255.255.255");
- gtk_main();
- return 0;
-}
-
diff --git a/sw/in_progress/antenna_track/ant_track_pmm.c b/sw/in_progress/antenna_track/ant_track_pmm.c
new file mode 100644
index 0000000000..9d7a70528d
--- /dev/null
+++ b/sw/in_progress/antenna_track/ant_track_pmm.c
@@ -0,0 +1,721 @@
+/*
+ * Paparazzi $Id: ant_track_pmm.c 2010-11-27 18:14:20Z griffin $
+ *
+ * Copyright (C) 2010
+ *
+ * Modified by: Mark Griffin and Todd Sandercock
+ * Modified by: Chris Efstathiou for the Pololu Micro Mestro usb servo controller Jun/2010
+ * Added command line options, a 360 pan option and "MANUAL" control.
+ *
+ * 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.
+ *
+ * The antenna tracker zero azimuth is to the NORTH (NORTH = 0, EAST = 90 WEST = -90, SOUTH = 180/0 degrees).
+ * The elevation zero is totally horizontal, 90 is up and 180 is to the back.
+ * The servo used must be able to do 180 degrees in order to get full 360 degree coverage from the tracker.
+ */
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include /* UNIX standard function definitions */
+#include /* File control definitions */
+#include /* Error number definitions */
+
+#define POLOLU_PROTOCOL_START 0xAA
+#define POLOLU_BOARD_ID 12
+#define SET_SERVO_POSITION_COMMAND 0x04
+#define SET_SERVO_SPEED_COMMAND 0x07
+#define SET_SERVO_ACCELERATION_COMMAND 0x09
+#define SET_SERVO_CENTER_COMMAND 0x22
+
+#define MANUAL 0
+#define AUTO 1
+
+static double gps_pos_x;
+static double gps_pos_y;
+static double gps_alt;
+static double home_alt;
+static double ant_azim = 180.;
+static double ant_elev = 90.;
+static int mode;
+static int home_found;
+static int ant_tracker_pan_mode = 180;
+static double theta_servo_pw_span = 1000.;
+static double psi_servo_pw_span = 1000.;
+static double theta_servo_center_pw = 1500;
+static double psi_servo_center_pw = 1500;
+static char pololu_board_id = 12;
+static char servo_acceleration = 3;
+static char psi_servo_address = 1;
+static char theta_servo_address = 0;
+
+int fd; /* File descriptor for the port */
+volatile int serial_error = 0;
+
+double hfov = 180., vfov = 180.;
+double hnp = 0., vnp = 0.;
+
+unsigned char speed = 0x00;
+
+void set_servos(void);
+
+GtkWidget *azim_scale;
+GtkWidget *elev_scale;
+
+void on_mode_changed(GtkRadioButton *radiobutton, gpointer user_data) {
+
+
+ mode = gtk_toggle_button_get_active(GTK_TOGGLE_BUTTON(radiobutton)) ? MANUAL : AUTO;
+
+
+ //IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
+ //g_message("Mode changed to %d" , mode);
+}
+
+void on_azimuth_changed(GtkAdjustment *hscale, gpointer user_data) {
+
+if (mode == MANUAL) {
+ ant_azim = gtk_range_get_value(GTK_RANGE (azim_scale));
+ ant_elev = gtk_range_get_value(GTK_RANGE (elev_scale));
+ set_servos();
+ }
+ //IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
+ //g_message("Mode changed to %d" , mode);
+}
+
+//void on_elevation_changed(GtkRange *elev_scale, gpointer user_data) {
+void on_elevation_changed(GtkAdjustment *hscale, gpointer user_data) {
+
+if (mode == MANUAL) {
+ ant_azim = gtk_range_get_value(GTK_RANGE (azim_scale));
+ ant_elev = gtk_range_get_value(GTK_RANGE (elev_scale));
+ set_servos();
+ }
+
+
+ //IvySendMsg("1ME RAW_DATALINK 80 SETTING;0;0;%d", mode);
+ //g_message("Mode changed to %d" , mode);
+}
+
+
+#define GLADE_HOOKUP_OBJECT(component,widget,name) \
+ g_object_set_data_full (G_OBJECT (component), name, \
+ gtk_widget_ref (widget), (GDestroyNotify) gtk_widget_unref)
+
+#define GLADE_HOOKUP_OBJECT_NO_REF(component,widget,name) \
+ g_object_set_data (G_OBJECT (component), name, widget)
+
+GtkWidget* build_gui(void) {
+ GtkWidget *window1;
+ GtkWidget *vbox1;
+ GtkWidget *vbox2;
+ GtkWidget *table1;
+ GtkWidget *label1;
+ GtkWidget *label2;
+ GtkWidget *label3;
+ GtkWidget *label4;
+ GtkWidget *radiobutton1;
+ GSList *radiobutton1_group = NULL;
+ GtkWidget *radiobutton2;
+ GtkWidget *entry1;
+
+ window1 = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ gtk_window_set_title(GTK_WINDOW (window1), "tracking antenna");
+
+ vbox1 = gtk_vbox_new(FALSE, 0);
+ gtk_widget_show(vbox1);
+ gtk_container_add(GTK_CONTAINER (window1), vbox1);
+
+ vbox2 = gtk_vbox_new(FALSE, 0);
+ gtk_widget_show(vbox2);
+ gtk_box_pack_start(GTK_BOX (vbox1), vbox2, TRUE, TRUE, 0);
+
+ table1 = gtk_table_new(4, 3, FALSE);
+ gtk_widget_show(table1);
+ gtk_box_pack_start(GTK_BOX (vbox2), table1, TRUE, TRUE, 0);
+ gtk_table_set_col_spacings(GTK_TABLE (table1), 5);
+
+ label1 = gtk_label_new("Azimuth");
+ gtk_widget_show(label1);
+ gtk_table_attach(GTK_TABLE (table1), label1, 0, 1, 1, 2,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label1), 0, 0.5);
+
+ label2 = gtk_label_new("Elevation");
+ gtk_widget_show(label2);
+ gtk_table_attach(GTK_TABLE (table1), label2, 0, 1, 2, 3,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label2), 0, 0.5);
+
+ label3 = gtk_label_new("Id");
+ gtk_widget_show(label3);
+ gtk_table_attach(GTK_TABLE (table1), label3, 0, 1, 3, 4,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label3), 0, 0.5);
+
+ label4 = gtk_label_new("mode");
+ gtk_widget_show(label4);
+ gtk_table_attach(GTK_TABLE (table1), label4, 0, 1, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_misc_set_alignment(GTK_MISC (label4), 0, 0.5);
+
+ radiobutton1 = gtk_radio_button_new_with_mnemonic(NULL, "manual");
+ gtk_widget_show(radiobutton1);
+ gtk_table_attach(GTK_TABLE (table1), radiobutton1, 1, 2, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton1),
+ radiobutton1_group);
+ radiobutton1_group = gtk_radio_button_get_group(
+ GTK_RADIO_BUTTON (radiobutton1));
+
+ radiobutton2 = gtk_radio_button_new_with_mnemonic(NULL, "tracking");
+ gtk_widget_show(radiobutton2);
+ gtk_table_attach(GTK_TABLE (table1), radiobutton2, 2, 3, 0, 1,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (0), 0, 0);
+ gtk_radio_button_set_group(GTK_RADIO_BUTTON (radiobutton2),
+ radiobutton1_group);
+ radiobutton1_group = gtk_radio_button_get_group(
+ GTK_RADIO_BUTTON (radiobutton2));
+
+ azim_scale = gtk_hscale_new(
+ //GTK_ADJUSTMENT (gtk_adjustment_new (180, 0, 360, 1, 1, 1)));
+ GTK_ADJUSTMENT (gtk_adjustment_new (0, 0, 360, 1, 1, 1)));
+ gtk_widget_show(azim_scale);
+ gtk_table_attach(GTK_TABLE (table1), azim_scale, 1, 3, 1, 2,
+ (GtkAttachOptions) (GTK_EXPAND | GTK_FILL),
+ (GtkAttachOptions) (GTK_FILL), 0, 0);
+// gtk_range_set_update_policy(GTK_RANGE (azim_scale), GTK_UPDATE_DELAYED);
+
+ elev_scale = gtk_hscale_new(
+ //GTK_ADJUSTMENT (gtk_adjustment_new (45, 0, 90, 1, 1, 1)));
+ GTK_ADJUSTMENT (gtk_adjustment_new (0, 0, 90, 1, 1, 1)));
+ gtk_widget_show(elev_scale);
+ gtk_table_attach(GTK_TABLE (table1), elev_scale, 1, 3, 2, 3,
+ (GtkAttachOptions) (GTK_FILL), (GtkAttachOptions) (GTK_FILL), 0, 0);
+// gtk_range_set_update_policy(GTK_RANGE (elev_scale), GTK_UPDATE_DELAYED);
+
+ entry1 = gtk_entry_new();
+ gtk_widget_show(entry1);
+ gtk_table_attach(GTK_TABLE (table1), entry1, 1, 3, 3, 4,
+ (GtkAttachOptions) (GTK_EXPAND | GTK_FILL), (GtkAttachOptions) (0),
+ 0, 0);
+
+ g_signal_connect ((gpointer) radiobutton1, "toggled",
+ G_CALLBACK (on_mode_changed),
+ NULL);
+
+ g_signal_connect ((gpointer) azim_scale, "value-changed",
+ G_CALLBACK (on_azimuth_changed),
+ NULL);
+
+ g_signal_connect ((gpointer) elev_scale, "value-changed",
+ G_CALLBACK (on_elevation_changed),
+ NULL);
+
+ /* Store pointers to all widgets, for use by lookup_widget(). */
+ GLADE_HOOKUP_OBJECT_NO_REF (window1, window1, "window1");
+ GLADE_HOOKUP_OBJECT (window1, vbox1, "vbox1");
+ GLADE_HOOKUP_OBJECT (window1, vbox2, "vbox2");
+ GLADE_HOOKUP_OBJECT (window1, table1, "table1");
+ GLADE_HOOKUP_OBJECT (window1, label1, "label1");
+ GLADE_HOOKUP_OBJECT (window1, label2, "label2");
+ GLADE_HOOKUP_OBJECT (window1, label3, "label3");
+ GLADE_HOOKUP_OBJECT (window1, label4, "label4");
+ GLADE_HOOKUP_OBJECT (window1, radiobutton1, "radiobutton1");
+ GLADE_HOOKUP_OBJECT (window1, radiobutton2, "radiobutton2");
+ GLADE_HOOKUP_OBJECT (window1, entry1, "entry1");
+
+ return window1;
+}
+
+void set_servos(void)
+{
+
+double hpos, vpos;
+int hservo = theta_servo_center_pw, vservo = psi_servo_center_pw;
+
+ // The magic is done here
+
+ if (ant_tracker_pan_mode == 180){
+
+ // Take the vertical angle relative to the neutral point "vnp"
+ vpos = ant_elev - vnp;
+
+ // keep within the field of view "vfov"
+ if (vpos > (vfov / 2)) { vpos = vfov / 2; } else if(-vpos > (vfov / 2)){ vpos = -vfov / 2; }
+
+ // First take the horizontal angle relative to the neutral point "hnp"
+ hpos = ant_azim - hnp;
+
+ // Keep the range between (-180,180). this is done so that it consistently swaps sides
+ if (hpos < -180){ hpos += 360; }else if(hpos > 180){ hpos -= 360; }
+
+ // Swap sides to obtain 360 degrees of Azimuth coverage.
+ if(hpos > 90){ hpos = hpos-180; vpos = 180-vpos; }else if(hpos < -90){ hpos = hpos+180; vpos = 180-vpos; }
+
+ // keep the range within the field of view "hfov"
+ if (hpos > (hfov / 2)) { hpos = hfov / 2; } else if (-hpos > (hfov / 2)) { hpos = -hfov / 2; }
+
+ // Convert angles to servo microsecond values suitable for the Pololu micro Maestro servo controller.
+ vpos = (psi_servo_center_pw-(psi_servo_pw_span/2)) + (vpos*(psi_servo_pw_span/vfov));
+ hpos = theta_servo_center_pw + (hpos*(theta_servo_pw_span/(hfov/2)));
+
+ //convert the values to integer.
+ hservo = hpos;
+ vservo = vpos;
+
+
+ }else{
+ vpos = ant_elev;
+
+ // First take the horizontal angle relative to the neutral point "hnp"
+ hpos = ant_azim - hnp;
+
+ // Keep the range between (-180,180).
+ if (hpos < -180){ hpos += 360; }else if(hpos > 180){ hpos -= 360; }
+
+ // Keep the range between 0 to 360.
+ //if (hpos < 0) { hpos += 360; } else if (hpos > 360){ hpos -= 360; }
+
+ // keep the range within the field of view "hfov"
+ if (hpos > (hfov / 2)) { hpos = hfov / 2; } else if (-hpos > (hfov / 2)) { hpos = -hfov / 2; }
+
+ // Convert angles to servo microsecond values suitable for the Pololu micro Maestro servo controller.
+ vpos = (psi_servo_center_pw-(psi_servo_pw_span/2)) + (fabs(vpos)*(psi_servo_pw_span/vfov));
+ hpos = theta_servo_center_pw + (hpos*(theta_servo_pw_span/hfov));
+
+ //convert the values to integer.
+ hservo = hpos;
+ vservo = vpos;
+ }
+
+ // Sanity check.
+ if (vservo < (psi_servo_center_pw-fabs(psi_servo_pw_span/2)) ){
+ vservo = (psi_servo_center_pw-fabs(psi_servo_pw_span/2));
+
+ }else if(vservo > (psi_servo_center_pw+fabs(psi_servo_pw_span/2))){ vservo = (psi_servo_center_pw+fabs(psi_servo_pw_span/2)); }
+
+ if (hservo < (theta_servo_center_pw-fabs(theta_servo_pw_span/2)) ){
+ hservo = (theta_servo_center_pw-fabs(theta_servo_pw_span/2));
+
+ }else if(hservo > (theta_servo_center_pw+fabs(theta_servo_pw_span/2))){ hservo = (theta_servo_center_pw+fabs(theta_servo_pw_span/2)); }
+
+ hservo *= 4; //The pololu Maestro uses 0.25 microsecond increments so we need to multiply microseconds by 4.
+ vservo *= 4; //The pololu Maestro uses 0.25 microsecond increments so we need to multiply microseconds by 4.
+ //g_message("home_alt %f gps_alt %f azim %f elev %f", home_alt, gps_alt, ant_azim, ant_elev);
+
+ // Send servo position.
+ char buffer1[]={ POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_POSITION_COMMAND, psi_servo_address, vservo%128, vservo/128,
+ POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_POSITION_COMMAND, theta_servo_address, hservo%128, hservo/128
+ };
+
+ serial_error = write(fd, buffer1, 12);
+
+ g_message("vservo %i hservo %i", (int)(vservo/4), (int)(hservo/4)); //Divide by 4 so we can have the servo PW with 1 microsecond resolution.
+
+return;
+}
+
+
+
+/* jump here when a GPS message is received */
+void on_GPS_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+ if (home_found == 0) {
+ if (atof(argv[0]) == 3) { /* wait until we have a valid GPS fix */
+ home_alt = atof(argv[4]) / 100.; /* get the altitude */
+ home_found = 1;
+ }
+ }
+ gps_alt = atof(argv[4]) / 100.;
+}
+
+/* jump here when a NAVIGATION message is received */
+void on_NAV_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]) {
+
+
+ if (mode == AUTO) {
+ gps_pos_x = atof(argv[2]);
+ gps_pos_y = atof(argv[3]);
+ /* calculate azimuth */
+ //should be "atan2(gps_pos_y, gps_pos_x)" but it is reversed to give 0 when North.
+ ant_azim = atan2(gps_pos_x, gps_pos_y) * 180. / M_PI;
+
+ if (ant_azim < 0)
+ ant_azim += 360.;
+
+ /* calculate elevation */
+ ant_elev = atan2((gps_alt - home_alt), sqrt(atof(argv[5]))) * 180. / M_PI;
+ // Sanity check
+ if (ant_elev < 0){ ant_elev = 0.; }
+ gtk_range_set_value(GTK_RANGE (azim_scale), ant_azim);
+ gtk_range_set_value(GTK_RANGE (elev_scale), ant_elev);
+
+ set_servos();
+
+ }
+
+
+}
+
+int open_port(char* port ) {
+ struct termios options;
+
+ // would probably be good to set the port up as an arg.
+ // The Pololu micro maestro registers two ports /dev/ttyACM0 and /dev/ttyACM1, /dev/ttyACM0 is the data port.
+ fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+ if (fd == -1) {
+ //perror("open_port: Unable to open /dev/ttyUSB1");
+ printf ("open_port: Unable to open %s \n", port);
+ serial_error = fd;
+
+ } else
+ printf("Success %s %s \n", port, "opened");
+ fcntl(fd, F_SETFL, 0);
+
+ tcgetattr(fd, &options);
+
+ // Set the baud rates to 19200. This can be between 2,000 to 40,000
+
+ cfsetispeed(&options, B19200);
+ cfsetospeed(&options, B19200);
+
+ options.c_cflag |= (CLOCAL | CREAD);
+
+ tcsetattr(fd, TCSANOW, &options);
+
+ // Send initialisation to the pololu micro maestro board.
+ // if "speed" is nonzero then 1 is the slowest 127 is the fastest. 0 = no speed restriction
+ char buffer_0[] = { POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_SPEED_COMMAND, psi_servo_address, 0x00, 0x00,
+ POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_SPEED_COMMAND, theta_servo_address, 0x00, 0x00
+ };
+
+ serial_error = write(fd, buffer_0, 12);
+ // Set servo acceleration to 3 for protecting the servo gears. Fastest = 0, slowest = 255
+ char buffer_1[] = { POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_ACCELERATION_COMMAND, psi_servo_address, (servo_acceleration%128),
+ (servo_acceleration/128),
+ POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_ACCELERATION_COMMAND, theta_servo_address, (servo_acceleration%128),
+ (servo_acceleration/128)
+ };
+
+ serial_error = write(fd, buffer_1, 12);
+ // Set the two servos to their neutral position, Azimuth = 1500us = EAST = 0 degrees & Elevation = 1000 = parallel to ground.
+ char buffer_2[] = { POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_POSITION_COMMAND, theta_servo_address,
+ (((int)theta_servo_center_pw*4) % 128), (((int)theta_servo_center_pw*4) / 128),
+ POLOLU_PROTOCOL_START, pololu_board_id, SET_SERVO_POSITION_COMMAND, psi_servo_address,
+ (((int)psi_servo_center_pw*4) % 128), (((int)psi_servo_center_pw*4) / 128)
+ };
+
+ serial_error = write(fd, buffer_2, 12);
+
+
+ return (fd);
+}
+
+int main(int argc, char** argv) {
+
+ gtk_init(&argc, &argv);
+
+
+ int x = 0, y = 0, z = 0;
+ char buffer[20];
+ char serial_open = 0;
+ printf ("Antenna Tracker for the Paparazzi autopilot, Chris Efstathiou 2010 \n");
+
+ if (argc > 1){
+ char arg_string1[] = "--help";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string1, (sizeof(arg_string1)-1))) == 0 ){
+ printf ("OPTIONS \n");
+ printf ("-------------------------------------------------------------------------------- \n");
+ printf ("'--help' displays this screen \n");
+ printf ("'--port=xxx..x' opens port xxx..x, example --port=/dev/ttyACM0 (Default) \n");
+ printf ("'--pan=xxx' sets pan mode to 180 or 360 degrees. Example --pan=180 (Default) \n");
+ printf ("'--zero_angle=xxx' set the mechanical zero angle. Default is 0 (North)\n");
+ printf ("'--id=xx' sets the Pololu board id. Example --id=12 (Default)\n");
+ printf ("'--servo_acc=xxx' sets the servo acceleration. Example --servo_acc=3 (Default)\n");
+ printf ("'--pan_servo=x' sets the pan (Theta) servo number. Example --pan_servo=0 (Default)\n");
+ printf ("'--tilt_servo=x' sets the tilt (Psi) servo number.Example --tilt_servo=1 (Default) \n");
+ printf ("'--pan_epa=xx..x' sets the Azimuth servo's max travel (Default is 1100us) \n");
+ printf ("'--tilt_epa=xx..x' sets the elevation servo's max travel (Default is 1100us). \n");
+ printf ("HINT a negative value EPA value reverses the servo direction \n");
+ printf ("'--pan_servo_center_pw=xx..x' sets the Azimuth servo's center position (Default is 1500us) \n");
+ printf ("'--tilt_servo_center_pw=xx..x' sets the elevation servo's center position (Default is 1500us) \n");
+ printf ("WARNING: The pololu board limit servo travel to 1000-2000 microseconds. \n");
+ printf ("WARNING: Use the pololu board setup program to change the above limits. \n");
+ printf ("Example --tilt_epa=1100 sets the PW from 950 to 2050 microseconds \n");
+ printf ("Example --pan_epa=-1000 sets the PW from 1000 to 2000 microseconds and reverses the servo direction \n");
+ printf ("An EPA of 1100 sets the servo travel from 1500+(1100/2)=2050us to 1500-(1100/2)=950us. \n");
+ printf ("Use programmable servos like the Hyperion Atlas. \n");
+ printf ("You can also use the proportional 360 degree GWS S125-1T as the Theta (Azimuth) \n");
+ printf ("servo or the mighty but expensive Futaba S5801 \n");
+ printf (" \n");
+ printf ("FOR THE 360 DEGREE PAN MODE: \n");
+ printf ("Mechanical zero (0 degrees or 1500 ms) is to the NORTH, 90 = EAST, +-180 = SOUTH and -90 = WEST. \n");
+ printf ("Elevation center is 45 degrees up (1500ms), 0 degrees = horizontal, 90 degrees is vertical (up) \n");
+ printf ("Of course use this mode if your PAN servo can do a full 360 degrees rotation (GWS S125-1T for example) \n");
+ printf (" \n");
+ printf ("FOR THE 180 DEGREE PAN MODE: \n");
+ printf ("Mechanical zero (0 degrees or 1500 ms) is to the NORTH, 90 = EAST, -90 = WEST. \n");
+ printf ("Elevation center is 90 degrees up (1500ms), 0 degrees = horizontal, 180 degrees is horizontal to the opposite side \n");
+ printf ("When the azimuth is > 90 or < -90 the azimuth and elevation servos swap sides to obtain the full 360 degree coverage. \n");
+ printf ("Of course your PAN and TILT servos must be true 180 degrees servos like the Hyperion ATLAS servos for example. \n");
+ printf (" \n");
+ printf ("-------------------------------------------------------------------------------- \n");
+ printf ("Antenna Tracker V1.2 for the Paparazzi autopilot 28/June/2010 \n");
+ printf ("-------------------------------------------------------------------------------- \n");
+ return 0;
+ }
+ }
+ printf ("Type '--help' for help \n");
+
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string2[] = "--port=";
+ for (x=1; x < argc; x++){
+
+ if ( (strncmp(argv[x], arg_string2, (sizeof(arg_string2)-1))) == 0 ){
+ y=sizeof(arg_string2)-1;
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ printf ("Trying to open %s \n", buffer);
+ open_port(buffer);
+ }
+ }
+ for(z=0; z 1000){
+ printf ("REMEMBER TO SET THE MIN/MAX SERVO LIMITS WITH THE POLOLU SETUP PROGRAM \n");
+ printf ("OTHERWISE THE MAX SERVO MOVEMENT WILL BE RESTRAINED TO 1000 MICROSECONDS \n");
+ }
+ }
+ }
+
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string5[] = "--tilt_epa=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string5, (sizeof(arg_string5)-1))) == 0 ){
+ y=(sizeof(arg_string5)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ psi_servo_pw_span = atoi(buffer);
+ printf ("PSI servo EPA set to %i \n", atoi(buffer));
+ if (abs(psi_servo_pw_span) > 1000){
+ printf ("REMEMBER TO SET THE MIN/MAX SERVO LIMITS WITH THE POLOLU SETUP PROGRAM \n");
+ printf ("OTHERWISE THE MAX SERVO MOVEMENT WILL BE RESTRAINED TO 1000 MICROSECONDS \n");
+ }
+ }
+ }
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string8[] = "--id=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string8, (sizeof(arg_string8)-1))) == 0 ){
+ y=(sizeof(arg_string8)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ pololu_board_id = (char)atoi(buffer);
+ printf ("Pololu Board id set to %i \n", atoi(buffer));
+ }
+ }
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string9[] = "--servo_acc=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string9, (sizeof(arg_string9)-1))) == 0 ){
+ y=(sizeof(arg_string9)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ servo_acceleration = (char)atoi(buffer);
+ printf ("Servo acceleration set to %i \n", atoi(buffer));
+ }
+ }
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string10[] = "--pan_servo=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string10, (sizeof(arg_string10)-1))) == 0 ){
+ y=(sizeof(arg_string10)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ theta_servo_address = (char)atoi(buffer);
+ printf ("Pan (Theta) servo number set to %i \n", atoi(buffer));
+ }
+ }
+
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string11[] = "--tilt_servo=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string11, (sizeof(arg_string11)-1))) == 0 ){
+ y=(sizeof(arg_string11)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ psi_servo_address = (char)atoi(buffer);
+ printf ("Tilt (Psi) servo number set to %i \n", atoi(buffer));
+ }
+ }
+
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string12[] = "--zero_angle=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string12, (sizeof(arg_string12)-1))) == 0 ){
+ y=(sizeof(arg_string12)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ hnp = (double)atoi(buffer);
+ printf ("Zero angle is set to %i %s \n", atoi(buffer), "degrees");
+ }
+ }
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string13[] = "--tilt_servo_center_pw=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string13, (sizeof(arg_string13)-1))) == 0 ){
+ y=(sizeof(arg_string13)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ psi_servo_center_pw = atoi(buffer);
+ printf ("PSI servo center pulse width set to %i \n", atoi(buffer));
+ }
+ }
+ for(z=0; z < sizeof(buffer); z++){ buffer[z] = '\0'; } //Reset the buffer.
+ char arg_string14[] = "--pan_servo_center_pw=";
+ for (x=1; x < argc; x++){
+ if ( (strncmp(argv[x], arg_string14, (sizeof(arg_string14)-1))) == 0 ){
+ y=(sizeof(arg_string14)-1);
+ z=0;
+ while (1){
+ buffer[z] = argv[x][y];
+ if(buffer[z] != '\0'){ y++; z++; }else{ break; }
+ }
+ theta_servo_center_pw = atoi(buffer);
+ printf ("THETA servo center pulse width set to %i \n", atoi(buffer));
+ }
+ }
+ for(z=0; z Xml.attrib c "function", i) rc_channels))
let rcommands = ref [||]
- let adj_bat = GData.adjustment ~value:FM.max_bat_level ~lower:0. ~upper:23. ~step_incr:0.1 ()
+ let adj_bat = GData.adjustment ~value:FM.max_bat_level ~lower:0. ~upper:(FM.max_bat_level+.2.) ~step_incr:0.1 ~page_size:0. ()
external get_commands : Stdlib.pprz_t array -> int = "get_commands"
(** Returns gaz servo value (us) *)