mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
Merge branch 'master' of github.com:paparazzi/paparazzi into nav
This commit is contained in:
Felix Ruess
@@ -2,6 +2,8 @@
|
||||
*.[oa]
|
||||
*.out
|
||||
|
||||
*~
|
||||
|
||||
*.pyc
|
||||
|
||||
*.cmo
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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".
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Executable → Regular
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -1615,6 +1615,7 @@
|
||||
<field name="link_imu_nb_err" type="uint32"/>
|
||||
<field name="blmc_nb_err" type="uint8"/>
|
||||
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
|
||||
<field name="frame_rate" type="uint8" unit="Hz"/>
|
||||
<field name="gps_status" type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
|
||||
<field name="ap_mode" type="uint8" values="FAILSAFE|KILL|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV"/>
|
||||
<field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
|
||||
|
||||
@@ -1,21 +1,42 @@
|
||||
<!DOCTYPE module SYSTEM "./module.dtd">
|
||||
|
||||
<module name="digital_cam">
|
||||
<header>
|
||||
<file name="dc.h"/>
|
||||
</header>
|
||||
<init fun="dc_init()"/>
|
||||
<periodic fun="dc_periodic()" freq="4" autorun="TRUE"/>
|
||||
<makefile>
|
||||
<raw>
|
||||
<!--
|
||||
|
||||
// Use (parts of) the following section in airframe file to change
|
||||
|
||||
<section name="DIGITAL_CAMERA" prefix="DC_">
|
||||
|
||||
<define name="PUSH" value"LED_ON" />
|
||||
<define name="RELEASE" value"LED_OFF" />
|
||||
|
||||
<define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
|
||||
<define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
|
||||
</section>
|
||||
|
||||
# ap.CFLAGS += -DGPS_TRIGGERED_FUNCTION="dc_shoot_on_gps"
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_START=1
|
||||
# ap.CFLAGS += -DDC_GPS_TRIGGER_STOP=3
|
||||
|
||||
</raw>
|
||||
-->
|
||||
|
||||
<module name="digital_cam">
|
||||
<header>
|
||||
<file name="led_cam_ctrl.h"/>
|
||||
</header>
|
||||
|
||||
<init fun="led_cam_ctrl_init()"/>
|
||||
|
||||
<periodic fun="led_cam_ctrl_periodic()" freq="4" autorun="TRUE"/>
|
||||
|
||||
<makefile >
|
||||
|
||||
<flag name="DIGITAL_CAM" />
|
||||
<file name="led_cam_ctrl.c"/>
|
||||
<file name="dc.c"/>
|
||||
<flag name="SENSOR_SYNC_SEND" value="1" />
|
||||
|
||||
</makefile>
|
||||
|
||||
|
||||
</module>
|
||||
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="atmega_i2c_cam_ctrl" dir="digital_cam">
|
||||
<header>
|
||||
<file name="atmega_i2c_cam_ctrl.h"/>
|
||||
</header>
|
||||
|
||||
<init fun="atmega_i2c_cam_ctrl_init()"/>
|
||||
<periodic fun="atmega_i2c_cam_ctrl_periodic()" autorun="TRUE" freq="4" />
|
||||
<event fun="atmega_i2c_cam_ctrl_event()"/>
|
||||
|
||||
<datalink message="PAYLOAD_COMMAND" fun="ParseCameraCommand()" />
|
||||
|
||||
<makefile target="ap">
|
||||
<file name="atmega_i2c_cam_ctrl.c"/>
|
||||
<file name="dc.c"/>
|
||||
<flag name="ATMEGA_I2C_DEVICE" value="i2c0"/>
|
||||
<flag name="USE_I2C0" value="1"/>
|
||||
<flag name="SENSOR_SYNC_SEND" value="1" />
|
||||
</makefile>
|
||||
|
||||
<makefile target="sim">
|
||||
<file name="sim_i2c_cam_ctrl.c"/>
|
||||
<file name="dc.c"/>
|
||||
<flag name="SENSOR_SYNC_SEND" value="1" />
|
||||
</makefile>
|
||||
|
||||
</module>
|
||||
|
||||
+16
-13
@@ -1,21 +1,24 @@
|
||||
<!DOCTYPE settings SYSTEM "settings.dtd">
|
||||
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="DC">
|
||||
<dl_setting MAX="1" MIN="1" STEP="1" VAR="dc_timer" module="digital_cam/dc" handler="Shutter" shortname="Shutter">
|
||||
<strip_button name="Photo" icon="digital-camera.png" value="1"/>
|
||||
</dl_setting>
|
||||
<dl_setting MAX="1" MIN="1" STEP="1" VAR="dc_timer" handler="Zoom" shortname="Zoom">
|
||||
<strip_button name="Zoom" icon="zoom.png" value="1"/>
|
||||
</dl_setting>
|
||||
<dl_setting MAX="60" MIN="0" STEP="1" VAR="dc_periodic_shutter" handler="Periodic" shortname="Periodic"/>
|
||||
<dl_setting MAX="5" MIN="0" STEP="1" VAR="dc_utm_threshold" shortname="UTM%"/>
|
||||
<dl_settings name="control">
|
||||
<dl_settings name="dc">
|
||||
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="dc_shoot" >
|
||||
<strip_button name="Start Autoshoot" icon="on.png" value="1"/>
|
||||
<strip_button name="Stop Autoshoot" icon="off.png" value="0"/>
|
||||
<dl_setting max="255" min="0" step="1" module="digital_cam/dc" var="0" handler="send_command" shortname="Shutter">
|
||||
<strip_button name="Photo" icon="digital-camera.png" value="32" group="maindc"/>
|
||||
<strip_button name="Power" icon="off.png" value="111" group="maindc"/>
|
||||
<strip_button name="ZoomIn" icon="zoom.png" value="116" group="dczoom"/>
|
||||
<strip_button name="ZoomOut" icon="zoom.png" value="119" group="dczoom"/>
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting max="3" min="0" step="1" var="dc_autoshoot" >
|
||||
<strip_button name="Start Autoshoot" icon="on.png" value="1" group="dcauto"/>
|
||||
<strip_button name="Stop Autoshoot" icon="off.png" value="0" group="dcauto"/>
|
||||
</dl_setting>
|
||||
|
||||
<dl_setting max="255" min="1" step="1" var="dc_autoshoot_quartersec_period" handler="Periodic" shortname="Periodic" param="DC_AUTOSHOOT_QUARTERSEC_PERIOD" unit="quarter-sec"/>
|
||||
<dl_setting max="5" min="0" step="1" var="dc_autoshoot_meter_grid" shortname="UTM%" param="DC_AUTOSHOOT_METER_GRID" unit="meter"/>
|
||||
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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"
|
||||
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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, \
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -32,6 +32,10 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
@@ -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):
|
||||
* <define name="DC_SHUTTER_LED" value="6"/>
|
||||
* <define name="DC_ZOOM_LED" value="7"/>
|
||||
* Related bank and pin must also be defined:
|
||||
* <define name="LED_6_BANK" value="0"/>
|
||||
* <define name="LED_6_PIN" value="2"/>
|
||||
* 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
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -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):
|
||||
* <define name="DC_SHUTTER_LED" value="6"/>
|
||||
* <define name="DC_ZOOM_IN_LED" value="7"/>
|
||||
* <define name="DC_ZOOM_OUT_LED" value="8"/>
|
||||
* <define name="DC_POWER_LED" value="9"/>
|
||||
* Related bank and pin must also be defined:
|
||||
* <define name="LED_6_BANK" value="0"/>
|
||||
* <define name="LED_6_PIN" value="2"/>
|
||||
* 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
|
||||
@@ -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 )
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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"|]
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -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 <gtk/gtk.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#include <termios.h>
|
||||
#include <unistd.h> /* UNIX standard function definitions */
|
||||
#include <fcntl.h> /* File control definitions */
|
||||
#include <errno.h> /* 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;
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -56,7 +56,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
|
||||
List.assoc x (Array.to_list (Array.mapi (fun i c -> 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) *)
|
||||
|
||||
Reference in New Issue
Block a user