[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5752] renamed vision to cam_control
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5752] renamed vision to cam_control |
|
Date: |
Mon, 30 Aug 2010 19:54:26 +0000 |
Revision: 5752
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5752
Author: flixr
Date: 2010-08-30 19:54:26 +0000 (Mon, 30 Aug 2010)
Log Message:
-----------
renamed vision to cam_control
Modified Paths:
--------------
paparazzi3/trunk/conf/modules/booz_cam.xml
paparazzi3/trunk/conf/modules/cam_point.xml
paparazzi3/trunk/conf/modules/cam_roll.xml
paparazzi3/trunk/conf/settings/booz_cam.xml
paparazzi3/trunk/conf/settings/cam.xml
paparazzi3/trunk/conf/settings/cam_pitch.xml
paparazzi3/trunk/conf/settings/cam_roll.xml
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/modules/cam_control/
paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.h
paparazzi3/trunk/sw/airborne/modules/cam_control/cam.c
paparazzi3/trunk/sw/airborne/modules/cam_control/cam.h
paparazzi3/trunk/sw/airborne/modules/cam_control/cam_roll.c
paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.h
paparazzi3/trunk/sw/airborne/modules/cam_control/point.c
paparazzi3/trunk/sw/airborne/modules/cam_control/point.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/modules/vision/
Modified: paparazzi3/trunk/conf/modules/booz_cam.xml
===================================================================
--- paparazzi3/trunk/conf/modules/booz_cam.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/modules/booz_cam.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -1,6 +1,6 @@
<!DOCTYPE module SYSTEM "module.dtd">
-<module name="vision">
+<module name="booz_cam" dir="cam_control">
<!-- depend require="booz_pwm|led" -->
<header>
<file name="booz_cam.h"/>
Modified: paparazzi3/trunk/conf/modules/cam_point.xml
===================================================================
--- paparazzi3/trunk/conf/modules/cam_point.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/modules/cam_point.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -1,6 +1,6 @@
<!DOCTYPE module SYSTEM "module.dtd">
-<module name="cam_point" dir="vision">
+<module name="cam_point" dir="cam_control">
<header>
<file name="cam.h"/>
<file name="point.h"/>
Modified: paparazzi3/trunk/conf/modules/cam_roll.xml
===================================================================
--- paparazzi3/trunk/conf/modules/cam_roll.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/modules/cam_roll.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -1,6 +1,6 @@
<!DOCTYPE module SYSTEM "module.dtd">
-<module name="cam_roll" dir="vision">
+<module name="cam_roll" dir="cam_control">
<header>
<file name="cam.h"/>
</header>
Modified: paparazzi3/trunk/conf/settings/booz_cam.xml
===================================================================
--- paparazzi3/trunk/conf/settings/booz_cam.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/settings/booz_cam.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -2,7 +2,7 @@
<dl_settings>
<dl_settings NAME="CAM">
- <dl_setting var="booz_cam_mode" MIN="0" STEP="1" MAX="3"
module="vision/booz_cam" shortname="mode" values="NONE|MAN|HEADING|WP"
handler="SetCamMode">
+ <dl_setting var="booz_cam_mode" MIN="0" STEP="1" MAX="3"
module="cam_control/booz_cam" shortname="mode" values="NONE|MAN|HEADING|WP"
handler="SetCamMode">
<strip_button name="CN" value="0"/>
<strip_button name="CM" value="1"/>
<strip_button name="CH" value="2"/>
Modified: paparazzi3/trunk/conf/settings/cam.xml
===================================================================
--- paparazzi3/trunk/conf/settings/cam.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/settings/cam.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -3,7 +3,7 @@
<settings>
<dl_settings NAME="control">
<dl_settings name="cam">
- <dl_setting MAX="5" MIN="0" STEP="1" module="vision/cam" VAR="cam_mode">
+ <dl_setting MAX="5" MIN="0" STEP="1" module="cam_control/cam"
VAR="cam_mode">
<strip_button name="AC_TARGET" value="5"/>
<strip_button name="WP_TARGET" value="4"/>
<strip_button name="XY_TARGET" value="3"/>
@@ -14,12 +14,12 @@
</dl_settings>
<dl_settings name="angles">
- <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vision/cam"
VAR="cam_tilt_c"/>
- <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vision/cam"
VAR="cam_pan_c"/>
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam"
VAR="cam_tilt_c"/>
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam"
VAR="cam_pan_c"/>
</dl_settings>
<dl_settings name="target">
- <dl_setting min="1" max="27" step="1" module="vision/cam"
var="cam_target_wp" shortname="wp"/>
+ <dl_setting min="1" max="27" step="1" module="cam_control/cam"
var="cam_target_wp" shortname="wp"/>
</dl_settings>
</dl_settings>
Modified: paparazzi3/trunk/conf/settings/cam_pitch.xml
===================================================================
--- paparazzi3/trunk/conf/settings/cam_pitch.xml 2010-08-30 19:54:18 UTC
(rev 5751)
+++ paparazzi3/trunk/conf/settings/cam_pitch.xml 2010-08-30 19:54:26 UTC
(rev 5752)
@@ -3,7 +3,7 @@
<settings>
<dl_settings NAME="control">
<dl_settings name="cam">
- <dl_setting MAX="5" MIN="0" STEP="1" module="vison/cam" VAR="cam_mode">
+ <dl_setting MAX="5" MIN="0" STEP="1" module="cam_control/cam"
VAR="cam_mode">
<!-- <strip_button name="TARGET" value="4"/>
<strip_button name="NADIR" value="2"/>
--> <strip_button name="ANGLES" value="1"/>
@@ -11,15 +11,15 @@
</dl_settings>
<dl_settings name="angles">
- <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vison/cam"
VAR="cam_tilt_c">
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam"
VAR="cam_tilt_c">
<strip_button name="Look Foreward" icon="lookfore.png" value="-0.5"/>
<strip_button name="Look Down" icon="lookdown.png" value="0.5"/>
</dl_setting>
- <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vison/cam"
VAR="cam_pan_c"/>
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam"
VAR="cam_pan_c"/>
</dl_settings>
<!--
<dl_settings name="target">
-<dl_setting min="1" max="10" step="1" module="vison/cam" var="cam_target_wp"
shortname="wp"/>
+<dl_setting min="1" max="10" step="1" module="cam_control/cam"
var="cam_target_wp" shortname="wp"/>
</dl_settings>
-->
</dl_settings>
Modified: paparazzi3/trunk/conf/settings/cam_roll.xml
===================================================================
--- paparazzi3/trunk/conf/settings/cam_roll.xml 2010-08-30 19:54:18 UTC (rev
5751)
+++ paparazzi3/trunk/conf/settings/cam_roll.xml 2010-08-30 19:54:26 UTC (rev
5752)
@@ -3,11 +3,11 @@
<settings>
<dl_settings>
<dl_settings NAME="Cam">
- <dl_setting MAX="45" MIN="-45" STEP="1" VAR="phi_c"
module="vision/cam_roll" handler="SetPhiDeg" shortname="phi deg" auto="true">
+ <dl_setting MAX="45" MIN="-45" STEP="1" VAR="phi_c"
module="cam_control/cam_roll" handler="SetPhiDeg" shortname="phi deg"
auto="true">
</dl_setting>
- <dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_mode"
module="vision/cam_roll" shortname="manual - stablzd">
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_mode"
module="cam_control/cam_roll" shortname="manual - stablzd">
</dl_setting>
- <dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_switch"
module="vision/cam_roll" shortname="still - mobile" handler="Switch">
+ <dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_switch"
module="cam_control/cam_roll" shortname="still - mobile" handler="Switch">
</dl_setting>
</dl_settings>
</dl_settings>
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,140 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2009 Gautier Hattenberger <address@hidden>,
+ * Antoine Drouin <address@hidden>
+ *
+ * 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 "booz_cam.h"
+#include "booz2_pwm_hw.h"
+#include "booz_ahrs.h"
+#include "booz2_navigation.h"
+#include "booz2_ins.h"
+#include "flight_plan.h"
+
+uint8_t booz_cam_mode;
+
+// Tilt definition
+#ifdef BOOZ_CAM_TILT_NEUTRAL
+int16_t booz_cam_tilt_pwm;
+int16_t booz_cam_tilt;
+#ifndef BOOZ_CAM_TILT_MIN
+#define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL
+#endif
+#ifndef BOOZ_CAM_TILT_MAX
+#define BOOZ_CAM_TILT_MAX BOOZ_CAM_TILT_NEUTRAL
+#endif
+#define BOOZ_CAM_USE_TILT 1
+#endif
+
+// Pan definition
+#ifdef BOOZ_CAM_PAN_NEUTRAL
+int16_t booz_cam_pan;
+#ifndef BOOZ_CAM_PAN_MIN
+#define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL
+#endif
+#ifndef BOOZ_CAM_PAN_MAX
+#define BOOZ_CAM_PAN_MAX BOOZ_CAM_PAN_NEUTRAL
+#endif
+#define BOOZ_CAM_USE_PAN 1
+#endif
+
+#if defined BOOZ_CAM_TILT_ANGLE_MIN && defined BOOZ_CAM_TILT_ANGLE_MAX &&
defined BOOZ_CAM_USE_TILT
+#define CAM_TA_MIN ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MIN)
+#define CAM_TA_MAX ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MAX)
+#define BOOZ_CAM_USE_TILT_ANGLES 1
+#endif
+
+// PWM definition
+#ifndef BOOZ_CAM_SetPwm
+#define BOOZ_CAM_SetPwm(_v) Booz2SetPwmValue(_v)
+#endif
+
+#ifndef BOOZ_CAM_DEFAULT_MODE
+#define BOOZ_CAM_DEFAULT_MODE BOOZ_CAM_MODE_NONE
+#endif
+
+void booz_cam_init(void) {
+ booz_cam_mode = BOOZ_CAM_DEFAULT_MODE;
+#ifdef BOOZ_CAM_USE_TILT
+ booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
+ BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
+ booz_cam_tilt = 0;
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+ booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL;
+#endif
+ if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { LED_ON(CAM_SWITCH_LED); } // CAM
OFF
+ else { LED_OFF(CAM_SWITCH_LED); } // CAM ON
+}
+
+void booz_cam_periodic(void) {
+
+ switch (booz_cam_mode) {
+ case BOOZ_CAM_MODE_NONE:
+#ifdef BOOZ_CAM_USE_TILT
+ booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+ booz_cam_pan = booz_ahrs.ltp_to_body_euler.psi;
+#endif
+ break;
+ case BOOZ_CAM_MODE_MANUAL:
+#ifdef BOOZ_CAM_USE_TILT
+ Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX);
+#endif
+ break;
+ case BOOZ_CAM_MODE_HEADING:
+#ifdef BOOZ_CAM_USE_TILT_ANGLES
+ Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
+ booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX -
BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+ Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX);
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+ Bound(booz_cam_pan,BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX);
+ nav_heading = booz_cam_pan;
+#endif
+ break;
+ case BOOZ_CAM_MODE_WP:
+#ifdef WP_CAM
+ {
+ struct Int32Vect2 diff;
+ VECT2_DIFF(diff, waypoints[WP_CAM], booz_ins_enu_pos);
+ INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
+ INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
+ nav_heading = booz_cam_pan;
+#ifdef BOOZ_CAM_USE_TILT_ANGLES
+ int32_t dist, height;
+ INT32_VECT2_NORM(dist, diff);
+ height = (waypoints[WP_CAM].z - booz_ins_enu_pos.z) >> INT32_POS_FRAC;
+ INT32_ATAN2(booz_cam_tilt, height, dist);
+ Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
+ booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX -
BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+ Bound(booz_cam_tilt_pwm, BOOZ_CAM_TILT_MIN, BOOZ_CAM_TILT_MAX);
+#endif
+ }
+#endif
+ break;
+ }
+#ifdef BOOZ_CAM_USE_TILT
+ BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
+#endif
+}
+
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.h (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.h 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,57 @@
+/*
+ * $Id: $
+ *
+ * Copyright (C) 2009 Gautier Hattenberger <address@hidden>,
+ * Antoine Drouin <address@hidden>
+ *
+ * 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 BOOZ_CAM_H
+#define BOOZ_CAM_H
+
+#include "airframe.h"
+#include "std.h"
+#include "led.h"
+
+#define BOOZ_CAM_MODE_NONE 0
+#define BOOZ_CAM_MODE_MANUAL 1
+#define BOOZ_CAM_MODE_HEADING 2
+#define BOOZ_CAM_MODE_WP 3
+
+extern uint8_t booz_cam_mode;
+
+#ifdef BOOZ_CAM_TILT_NEUTRAL
+extern int16_t booz_cam_tilt_pwm;
+extern int16_t booz_cam_tilt;
+#endif
+#ifdef BOOZ_CAM_PAN_NEUTRAL
+extern int16_t booz_cam_pan;
+#endif
+
+extern void booz_cam_init(void);
+extern void booz_cam_periodic(void);
+
+#define booz_cam_SetCamMode(_v) { \
+ booz_cam_mode = _v; \
+ if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { LED_ON(CAM_SWITCH_LED); } \
+ else { LED_OFF(CAM_SWITCH_LED); } \
+}
+
+#endif /* BOOZ2_CAM_H */
+
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/cam.c (from rev 5751,
paparazzi3/trunk/sw/airborne/modules/vision/cam.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam.c 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,208 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file cam.c
+ * \brief Pan/Tilt camera library
+ *
+ */
+
+#include <math.h>
+#include "cam.h"
+#include "common_nav.h"
+#include "autopilot.h"
+#include "flight_plan.h"
+#include "estimator.h"
+#include "traffic_info.h"
+#ifdef POINT_CAM
+#include "point.h"
+#endif // POINT_CAM
+
+#ifdef TEST_CAM
+float test_cam_estimator_x;
+float test_cam_estimator_y;
+float test_cam_estimator_z;
+float test_cam_estimator_phi;
+float test_cam_estimator_theta;
+float test_cam_estimator_hspeed_dir;
+#endif // TEST_CAM
+
+#ifdef CAM_PAN_NEUTRAL
+#if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
+#error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
+#endif
+#if (CAM_PAN_NEUTRAL == CAM_PAN_MIN)
+#error CAM_PAN_MIN has to be different from CAM_PAN_NEUTRAL
+#endif
+#endif
+
+#ifdef CAM_TILT_NEUTRAL
+#if (CAM_TILT_MAX == CAM_TILT_NEUTRAL)
+#error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
+#endif
+#if (CAM_TILT_NEUTRAL == CAM_TILT_MIN)
+#error CAM_TILT_MIN has to be different from CAM_TILT_NEUTRAL
+#endif
+#endif
+
+#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
+#define DELTA_ALPHA 0.2
+
+#ifdef CAM_PAN0
+float cam_pan_c = RadOfDeg(CAM_PAN0);
+#else
+float cam_pan_c;
+#endif
+
+#ifdef CAM_TILT0
+float cam_tilt_c = RadOfDeg(CAM_TILT0);
+#else
+float cam_tilt_c;
+#endif
+
+float cam_phi_c;
+float cam_theta_c;
+
+float cam_target_x, cam_target_y, cam_target_alt;
+uint8_t cam_target_wp;
+uint8_t cam_target_ac;
+
+uint8_t cam_mode;
+
+int16_t cam_pan_command;
+int16_t cam_tilt_command;
+
+void cam_nadir(void);
+void cam_angles(void);
+void cam_target(void);
+void cam_waypoint_target(void);
+void cam_ac_target(void);
+
+void cam_init( void ) {
+ cam_mode = CAM_MODE_OFF;
+}
+
+void cam_periodic( void ) {
+ switch (cam_mode) {
+ case CAM_MODE_OFF:
+ break;
+ case CAM_MODE_ANGLES:
+ cam_angles();
+ break;
+ case CAM_MODE_NADIR:
+ cam_nadir();
+ break;
+ case CAM_MODE_XY_TARGET:
+ cam_target();
+ break;
+ case CAM_MODE_WP_TARGET:
+ cam_waypoint_target();
+ break;
+ case CAM_MODE_AC_TARGET:
+ cam_ac_target();
+ break;
+ }
+}
+
+/** Computes the servo values from cam_pan_c and cam_tilt_c */
+void cam_angles( void ) {
+ float cam_pan = 0;
+ float cam_tilt = 0;
+
+#ifdef CAM_PAN_NEUTRAL
+ float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
+ if (pan_diff > 0)
+ cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX -
CAM_PAN_NEUTRAL)));
+ else
+ cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN -
CAM_PAN_NEUTRAL)));
+#endif
+
+#ifdef CAM_TILT_NEUTRAL
+ float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
+ if (tilt_diff > 0)
+ cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX -
CAM_TILT_NEUTRAL)));
+ else
+ cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN -
CAM_TILT_NEUTRAL)));
+#endif
+
+ cam_pan = TRIM_PPRZ(cam_pan);
+ cam_tilt = TRIM_PPRZ(cam_tilt);
+
+ cam_phi_c = cam_pan_c;
+ cam_theta_c = cam_tilt_c;
+
+#ifdef COMMAND_CAM_PAN
+ ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
+#endif
+#ifdef COMMAND_CAM_TILT
+ ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
+#endif
+}
+
+/** Computes the right angles from target_x, target_y, target_alt */
+void cam_target( void ) {
+#ifdef TEST_CAM
+ vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
+ test_cam_estimator_phi, test_cam_estimator_theta,
test_cam_estimator_hspeed_dir,
+ cam_target_x, cam_target_y, cam_target_alt,
+ &cam_pan_c, &cam_tilt_c);
+#else
+ vPoint(estimator_x, estimator_y, estimator_z,
+ estimator_phi, estimator_theta, estimator_hspeed_dir,
+ cam_target_x, cam_target_y, cam_target_alt,
+ &cam_pan_c, &cam_tilt_c);
+#endif
+ cam_angles();
+}
+
+/** Point straight down */
+void cam_nadir( void ) {
+#ifdef TEST_CAM
+ cam_target_x = test_cam_estimator_x;
+ cam_target_y = test_cam_estimator_y;
+#else
+ cam_target_x = estimator_x;
+ cam_target_y = estimator_y;
+#endif
+ cam_target_alt = 0;
+ cam_target();
+}
+
+
+void cam_waypoint_target( void ) {
+ if (cam_target_wp < nb_waypoint) {
+ cam_target_x = waypoints[cam_target_wp].x;
+ cam_target_y = waypoints[cam_target_wp].y;
+ }
+ cam_target_alt = ground_alt;
+ cam_target();
+}
+
+void cam_ac_target( void ) {
+#ifdef TRAFFIC_INFO
+ struct ac_info_ * ac = get_ac_info(cam_target_ac);
+ cam_target_x = ac->east;
+ cam_target_y = ac->north;
+ cam_target_alt = ac->alt;
+ cam_target();
+#endif // TRAFFIC_INFO
+}
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/cam.h (from rev 5751,
paparazzi3/trunk/sw/airborne/modules/vision/cam.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam.h 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,75 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005- Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file cam.h
+ * \brief Pan/Tilt camera API
+ *
+ */
+
+#ifndef CAM_H
+#define CAM_H
+
+#include <inttypes.h>
+#include "inter_mcu.h"
+
+#define CAM_MODE_OFF 0 /* Do nothing */
+#define CAM_MODE_ANGLES 1 /* Input: servo angles */
+#define CAM_MODE_NADIR 2 /* Input: () */
+#define CAM_MODE_XY_TARGET 3 /* Input: target_x, target_y */
+#define CAM_MODE_WP_TARGET 4 /* Input: waypoint no */
+#define CAM_MODE_AC_TARGET 5 /* Input: ac id */
+
+extern uint8_t cam_mode;
+
+extern float cam_phi_c, cam_theta_c;
+
+extern float cam_pan_c, cam_tilt_c;
+/* pan (move left and right), tilt (move up and down) */
+/** Radians, for CAM_MODE_ANGLES mode */
+
+extern float cam_target_x, cam_target_y;
+/** For CAM_MODE_XY_TARGET mode */
+
+extern uint8_t cam_target_wp;
+/** For CAM_MODE_WP_TARGET mode */
+
+extern uint8_t cam_target_ac;
+/** For CAM_MODE_AC_TARGET mode */
+
+void cam_periodic( void );
+void cam_init( void );
+
+extern int16_t cam_pan_command;
+#define cam_SetPanCommand(x) { ap_state->commands[COMMAND_CAM_PAN] =
cam_pan_command = x;}
+extern int16_t cam_tilt_command;
+#define cam_SetTiltCommand(x) { ap_state->commands[COMMAND_CAM_TILT] =
cam_tilt_command = x;}
+
+#ifdef TEST_CAM
+extern float test_cam_estimator_x;
+extern float test_cam_estimator_y;
+extern float test_cam_estimator_z;
+extern float test_cam_estimator_phi;
+extern float test_cam_estimator_theta;
+extern float test_cam_estimator_hspeed_dir;
+#endif // TEST_CAM
+#endif // CAM_H
Added: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_roll.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_roll.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_roll.c 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,79 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file cam.c
+ * \brief Pan/Tilt camera library
+ *
+ */
+
+#include <math.h>
+#include "cam.h"
+#include "nav.h"
+#include "autopilot.h"
+#include "flight_plan.h"
+#include "estimator.h"
+#include "inter_mcu.h"
+#include "nav.h"
+
+#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
+#define DELTA_ALPHA 0.2
+
+#define MAX_CAM_ROLL M_PI/2
+
+float cam_roll_phi; /* radian */
+float phi_c; /* radian */
+float theta_c; /* have to be defined for telemetry message */
+
+float target_x, target_y, target_alt;
+
+#ifdef MOBILE_CAM
+
+#define MODE_MANUAL 0
+#define MODE_STABILIZED 1
+
+uint8_t cam_roll_mode;
+bool_t cam_roll_switch;
+
+void cam_init( void ) {
+ cam_roll_switch = 0;
+#if defined VIDEO_SWITCH_PIN && !(defined SITL)
+ IO0DIR |= _BV(VIDEO_SWITCH_PIN);
+ IO0CLR = _BV(VIDEO_SWITCH_PIN);
+#endif
+}
+
+void cam_periodic( void ) {
+ switch (cam_roll_mode) {
+ case MODE_STABILIZED:
+ phi_c = cam_roll_phi + estimator_phi;
+ break;
+ case MODE_MANUAL:
+ phi_c = cam_roll_phi;
+ break;
+ default:
+ phi_c = 0;
+ }
+ ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ /
RadOfDeg(CAM_PHI_MAX_DEG));
+}
+
+#endif // MOBILE_CAM
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-08-30 19:54:26 UTC (rev 5752)
@@ -0,0 +1,235 @@
+/*
+ * $Id: demo_module.c 3079 2009-03-11 16:55:42Z gautier $
+ *
+ * Copyright (C) 2010 Gautier Hattenberger
+ *
+ * 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 "cam_track.h"
+
+#include "booz2_ins.h"
+#include "booz_ahrs.h"
+
+#ifdef USE_HFF
+#include "ins/booz2_hf_float.h"
+#endif
+
+struct FloatVect3 target_pos_ned;
+struct FloatVect3 target_speed_ned;
+struct FloatVect3 target_accel_ned;
+
+struct FloatVect3 last_pos_ned;
+
+#define CAM_DATA_LEN (3*4)
+#define CAM_START_1 0xFF
+#define CAM_START_2 0xFE
+#define CAM_END 0xF0
+
+#define UNINIT 0
+#define GOT_START_1 1
+#define GOT_START_2 2
+#define GOT_LEN 3
+#define GOT_DATA 4
+#define GOT_END 5
+
+#include "messages.h"
+#include "downlink.h"
+
+volatile uint8_t cam_msg_received;
+uint8_t cam_status;
+uint8_t cam_data_len;
+
+void track_init(void) {
+ booz_ins_ltp_initialised = TRUE; // ltp is initialized and centered on the
target
+ booz_ins_update_on_agl = TRUE; // use sonar to update agl (assume flat
ground)
+
+ cam_status = UNINIT;
+ cam_data_len = CAM_DATA_LEN;
+
+}
+
+#include <stdio.h>
+void track_periodic_task(void) {
+ char cmd_msg[256];
+ uint8_t c = 0;
+
+ cmd_msg[c++] = 'A';
+ cmd_msg[c++] = ' ';
+ float phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi);
+ if (phi > 0) cmd_msg[c++] = ' ';
+ else { cmd_msg[c++] = '-'; phi = -phi; }
+ cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10*phi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (100*phi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
+ cmd_msg[c++] = ' ';
+ float theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta);
+ if (theta > 0) cmd_msg[c++] = ' ';
+ else { cmd_msg[c++] = '-'; theta = -theta; }
+ cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10*theta) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (100*theta) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
+ cmd_msg[c++] = ' ';
+ float psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi);
+ if (psi > 0) cmd_msg[c++] = ' ';
+ else { cmd_msg[c++] = '-'; psi = -psi; }
+ cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10*psi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (100*psi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
+ cmd_msg[c++] = ' ';
+ float alt = -POS_FLOAT_OF_BFP(booz_ins_ltp_pos.z);
+ //alt = 0.40;
+ if (alt > 0) cmd_msg[c++] = ' ';
+ else { cmd_msg[c++] = '-'; alt = -alt; }
+ cmd_msg[c++] = '0' + ((unsigned int) (alt/10) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) alt % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (10*alt) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (100*alt) % 10);
+ cmd_msg[c++] = '0' + ((unsigned int) (1000*alt) % 10);
+ cmd_msg[c++] = ' ';
+ cmd_msg[c++] = '\n';;
+
+ int i;
+ for (i = 0; i < c; i++) {
+ CamUartSend1(cmd_msg[i]);
+ }
+ //DOWNLINK_SEND_DEBUG(DefaultChannel,c,cmd_msg);
+
+}
+
+void track_event(void) {
+ if (!booz_ins_ltp_initialised) {
+ booz_ins_ltp_initialised = TRUE;
+ booz_ins_hf_realign = TRUE;
+ }
+
+#ifdef USE_HFF
+ if (booz_ins_hf_realign) {
+ booz_ins_hf_realign = FALSE;
+ struct FloatVect2 pos, zero;
+ pos.x = -target_pos_ned.x;
+ pos.y = -target_pos_ned.y;
+ booz_ins_realign_h(pos, zero);
+ }
+ const stuct FlotVect2 measuremet_noise = { 10.0, 10.0);
+ b2_hff_update_pos(-target_pos_ned, measurement_noise);
+ booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+#else
+ // store pos in ins
+ booz_ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
+ booz_ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y));
+ // compute speed from last pos
+ // TODO get delta T
+ // store last pos
+ VECT3_COPY(last_pos_ned, target_pos_ned);
+#endif
+
+ b2_hff_lost_counter = 0;
+}
+
+#define CAM_MAX_PAYLOAD 254
+uint8_t cam_data_buf[CAM_MAX_PAYLOAD];
+uint8_t cam_data_idx;
+
+void parse_cam_msg( void ) {
+ uint8_t* ptr;
+ // pos x
+ ptr = (uint8_t*)(&(target_pos_ned.x));
+ *ptr = cam_data_buf[0];
+ ptr++;
+ *ptr = cam_data_buf[1];
+ ptr++;
+ *ptr = cam_data_buf[2];
+ ptr++;
+ *ptr = cam_data_buf[3];
+ // pos y
+ ptr = (uint8_t*)(&(target_pos_ned.y));
+ *ptr = cam_data_buf[4];
+ ptr++;
+ *ptr = cam_data_buf[5];
+ ptr++;
+ *ptr = cam_data_buf[6];
+ ptr++;
+ *ptr = cam_data_buf[7];
+ // pos z
+ ptr = (uint8_t*)(&(target_pos_ned.z));
+ *ptr = cam_data_buf[8];
+ ptr++;
+ *ptr = cam_data_buf[9];
+ ptr++;
+ *ptr = cam_data_buf[10];
+ ptr++;
+ *ptr = cam_data_buf[11];
+
+ //DOWNLINK_SEND_DEBUG(DefaultChannel,12,cam_data_buf);
+}
+
+void parse_cam_buffer( uint8_t c ) {
+ char bla[1];
+ bla[1] = c;
+ //DOWNLINK_SEND_DEBUG(DefaultChannel,1,bla);
+ switch (cam_status) {
+ case UNINIT:
+ if (c != CAM_START_1)
+ goto error;
+ cam_status++;
+ break;
+ case GOT_START_1:
+ if (c != CAM_START_2)
+ goto error;
+ cam_status++;
+ break;
+ case GOT_START_2:
+ cam_data_len = c;
+ if (cam_data_len > CAM_MAX_PAYLOAD)
+ goto error;
+ cam_data_idx = 0;
+ cam_status++;
+ break;
+ case GOT_LEN:
+ cam_data_buf[cam_data_idx] = c;
+ cam_data_idx++;
+ if (cam_data_idx >= cam_data_len)
+ cam_status++;
+ break;
+ case GOT_DATA:
+ if (c != CAM_END)
+ goto error;
+ cam_msg_received = TRUE;
+ goto restart;
+ break;
+ }
+ return;
+ error:
+ restart:
+ cam_status = UNINIT;
+ return;
+}
+
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.h (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/cam_track.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.h
2010-08-30 19:54:26 UTC (rev 5752)
@@ -0,0 +1,72 @@
+/*
+ * $Id: demo_module.h 3079 2009-03-11 16:55:42Z gautier $
+ *
+ * Copyright (C) 2010 Gautier Hattenberger
+ *
+ * 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 cam_track.h
+ *
+ * blob tracking with cmucam
+ */
+
+#ifndef CAM_TRACK_H
+#define CAM_TRACK_H
+
+#include <inttypes.h>
+#include "math/pprz_algebra_float.h"
+
+extern struct FloatVect3 target_pos_ned;
+extern struct FloatVect3 target_speed_ned;
+extern struct FloatVect3 target_accel_ned;
+
+extern void track_init(void);
+extern void track_periodic_task(void);
+extern void track_event(void);
+
+extern volatile uint8_t cam_msg_received;
+extern void parse_cam_msg( void );
+extern void parse_cam_buffer( uint8_t );
+
+#include "uart.h"
+
+#define __CamLink(dev, _x) dev##_x
+#define _CamLink(dev, _x) __CamLink(dev, _x)
+#define CamLink(_x) _CamLink(CAM_LINK, _x)
+
+#define CamBuffer() CamLink(ChAvailable())
+#define ReadCamBuffer() { while (CamLink(ChAvailable())&&!cam_msg_received)
parse_cam_buffer(CamLink(Getch())); }
+#define CamUartSend1(c) CamLink(Transmit(c))
+#define CamUartInitParam(_a,_b,_c) CamLink(InitParam(_a,_b,_c))
+#define CamUartRunning CamLink(TxRunning)
+
+#define CamEventCheckAndHandle() { \
+ if (CamBuffer()) { \
+ ReadCamBuffer(); \
+ } \
+ if (cam_msg_received) { \
+ parse_cam_msg(); \
+ track_event(); \
+ cam_msg_received = FALSE; \
+ } \
+}
+
+
+#endif
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/point.c (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/point.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/point.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/point.c 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,379 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-2008 Arnold Schroeter
+ *
+ * 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 point.c
+ * \brief Determines camera pan and tilt angles.
+ *
+ * project: Paparazzi
+ * description: Determines camera pan and tilt angles from
+ * plane's and object's positions and plane's
+ * pitch and roll angles. Software might be optimized
+ * by removing multiplications with 0, it is left this
+ * way for better understandabilty and changeability.
+ *
+ * author: Arnold Schroeter, Martin Mueller
+ *
+ * hardware:
+ *
+ * The camera control is made of normal servos. Usually servos have a
+ * turn angle of about 90\xB0. This is changed electrically so that they
+ * can do a 180\xB0. It is achieved by adding two serial resistors at both
+ * sides of the potentiometer (P1), one for increasing the usable angle
+ * (R1) and the other for moving the middle position to a useful angle
+ * (R2). Therefore a servo with a 270\xB0 potentiometer is needed. Very
+ * small and light servos have 180\xB0 potentiometers, these do not allow
+ * a 180\xB0 degrees sweep. Cut the outer two connections between the
+ * potentiometer and the board to insert the resistors. The values for
+ * R1 and R2 should be found out by testing as there might be serial
+ * resistors on the servo board that affect the values. Start with
+ * about 1/2 the value of P1 for R1 and change R1 until you get a
+ * little more than 180\xB0 sweep. Then insert and modify R2 to set
+ * neutral back to the middle position of the potentiometer.
+ *
+ *
+ * ^
+ * /
+ * ----------
+ * *-------I / I-------*
+ * I ---------- I
+ * I / P1 I
+ * I I I
+ * I I I
+ * --- I ---
+ * I I I I I
+ * I I I I I
+ * I I I I I
+ * --- R1 I --- R2
+ * I I I
+ * I I I
+ *
+ *
+ */
+
+#include <math.h>
+#include "point.h"
+
+typedef struct {
+ float fx;
+ float fy;
+ float fz;} VECTOR;
+
+typedef struct {
+ float fx1; float fx2; float fx3;
+ float fy1; float fy2; float fy3;
+ float fz1; float fz2; float fz3;} MATRIX;
+
+void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC);
+void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC);
+
+/*******************************************************************
+; function name: vSubtractVectors
+; description: subtracts two vectors a = b - c
+; parameters:
+;*******************************************************************/
+void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC)
+{
+ svA->fx = svB.fx - svC.fx;
+ svA->fy = svB.fy - svC.fy;
+ svA->fz = svB.fz - svC.fz;
+}
+
+/*******************************************************************
+; function name: vMultiplyMatrixByVector
+; description: multiplies matrix by vector svA = smB * svC
+; parameters:
+;*******************************************************************/
+void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC)
+{
+ svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
+ svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
+ svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
+}
+
+/*******************************************************************
+; function name: vPoint
+; description: Transforms ground coordinate system into
+; plane's coordinate system via three rotations
+; and determines positions of camera servos.
+; parameters: fPlaneNorth, fPlaneEast, fPlaneAltitude plane's
+; position with respect to ground
+; in m (actually the units do not matter as
+; long as they are the same as for the object's
+; position)
+; fRollAngle level=0; right wing down = positive values
+; fPitchAngle level=0; nose up = positive values
+; plane's pitch and roll angles
+; with respect to ground in radians
+; fYawAngle north=0; right= positive values in radians
+; plane's yaw angle with respect to north
+; fObjectNorth, fObjectEast, fAltitude object's
+; position with respect to ground
+; in m (actually the units do not matter as
+; long as they are the same for the plane's
+; position)
+; fPan, fTilt angles for camera servos in radians,
+; pan is turn/left-right and tilt is down-up
+; in reference to the camera picture
+; camera mount: The way the camera is mounted is given through a
+; define POINT_CAM_a_[_b] where a gives the mount
+; angle within the aircraft and b the angle when
+; viewing the direction of the first servo.
+;*******************************************************************/
+void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
+ float fRollAngle, float fPitchAngle, float fYawAngle,
+ float fObjectEast, float fObjectNorth, float fAltitude,
+ float *fPan, float *fTilt)
+{
+ static VECTOR svPlanePosition,
+ svObjectPosition,
+ svObjectPositionForPlane,
+ svObjectPositionForPlane2;
+
+ static MATRIX smRotation;
+
+ svPlanePosition.fx = fPlaneNorth;
+ svPlanePosition.fy = fPlaneEast;
+ svPlanePosition.fz = fPlaneAltitude;
+
+ svObjectPosition.fx = fObjectNorth;
+ svObjectPosition.fy = fObjectEast;
+ svObjectPosition.fz = fAltitude;
+
+ /* distance between plane and object */
+ vSubtractVectors(&svObjectPositionForPlane, svObjectPosition,
svPlanePosition);
+
+ /* yaw */
+ smRotation.fx1 = (float)(cos(fYawAngle));
+ smRotation.fx2 = (float)(sin(fYawAngle));
+ smRotation.fx3 = 0.;
+ smRotation.fy1 = -smRotation.fx2;
+ smRotation.fy2 = smRotation.fx1;
+ smRotation.fy3 = 0.;
+ smRotation.fz1 = 0.;
+ smRotation.fz2 = 0.;
+ smRotation.fz3 = 1.;
+
+ vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation,
svObjectPositionForPlane);
+
+ /* pitch */
+ smRotation.fx1 = (float)(cos(fPitchAngle));
+ smRotation.fx2 = 0.;
+ smRotation.fx3 = (float)(sin(fPitchAngle));
+ smRotation.fy1 = 0.;
+ smRotation.fy2 = 1.;
+ smRotation.fy3 = 0.;
+ smRotation.fz1 = -smRotation.fx3;
+ smRotation.fz2 = 0.;
+ smRotation.fz3 = smRotation.fx1;
+
+ vMultiplyMatrixByVector(&svObjectPositionForPlane, smRotation,
svObjectPositionForPlane2);
+
+ /* roll */
+ smRotation.fx1 = 1.;
+ smRotation.fx2 = 0.;
+ smRotation.fx3 = 0.;
+ smRotation.fy1 = 0.;
+ smRotation.fy2 = (float)(cos(fRollAngle));
+ smRotation.fy3 = (float)(-sin(fRollAngle));
+ smRotation.fz1 = 0.;
+ smRotation.fz2 = -smRotation.fy3;
+ smRotation.fz3 = smRotation.fy2;
+
+ vMultiplyMatrixByVector(&svObjectPositionForPlane2, smRotation,
svObjectPositionForPlane);
+
+#ifdef POINT_CAM_PITCH
+
+ /*
+ * This is for one axis pitch camera mechanisms. The pitch servo neutral
+ * makes the camera look down, 90\xB0 is to the front and -90\xB0 is to the
+ * back. The pitch value is given through the tilt parameter.
+ * The camera picture is upright when looking in flight direction.
+ *
+ * tilt servo, looking from left:
+ *
+ * plane front <-------------- plane back
+ * / I \
+ * / I \
+ * 45\xB0 I -45\xB0
+ * 0\xB0
+ *
+ * (should be hyperbolic, we use lines to make it better, the plane rolls
+ * away from the object while flying towards it!)
+ *
+ */
+
+ /* fTilt = 0 -> camera looks down
+ 90 -> camera looks forward
+ -90 -> camera looks backward
+ */
+#if 0 //we roll away anyways
+ *fTilt = (float)(atan2( svObjectPositionForPlane2.fx,
+ sqrt( svObjectPositionForPlane2.fy *
svObjectPositionForPlane2.fy
+ + svObjectPositionForPlane2.fz *
svObjectPositionForPlane2.fz )
+ ));
+#else
+ *fTilt = (float)(atan2( svObjectPositionForPlane2.fx,
-svObjectPositionForPlane2.fz ));
+#endif
+
+ /* fPan is deactivated
+ */
+ *fPan = 0;
+#else
+#ifdef POINT_CAM_ROLL
+
+ /*
+ * This is for single axis roll camera mechanisms. The tilt servo neutral
+ * makes the camera look down, -90\xB0 is to the right and 90\xB0 is to the
+ * left.
+ * The camera picture is upright when looking to the right.
+ *
+ *
+ * tilt servo, looking from behind:
+ *
+ * plane left --------------- plane right
+ * / I \
+ * / I \
+ * 45\xB0 I -45\xB0
+ * 0\xB0
+ *
+ */
+#if 1 // have to check if it helps
+ *fTilt = (float)(atan2( svObjectPositionForPlane2.fy,
+ sqrt( svObjectPositionForPlane2.fx *
svObjectPositionForPlane2.fx
+ + svObjectPositionForPlane2.fz *
svObjectPositionForPlane2.fz )
+ ));
+#else
+ *fTilt = (float)(atan2( svObjectPositionForPlane2.fy,
-svObjectPositionForPlane2.fz));
+#endif
+
+ /* fPan is deactivated
+ */
+ *fPan = 0;
+#else
+#ifdef POINT_CAM_YAW_PITCH
+
+/*
+ * This is for two axes pan/tilt camera mechanisms. The default is to
+ * circle clockwise so view is right. The pan servo neutral makes
+ * the camera look to the right with 0\xB0 given, 90\xB0 is to the back and
+ * -90\xB0 is to the front. The tilt servo neutral makes the camera look
+ * down with 0\xB0 given, 90\xB0 is to the right and -90\xB0 is to the left
(all
+ * values are used in radian in the software). If the camera looks to
+ * the right side of the plane, the picture is upright. It is upside
+ * down when looking to the left. That is corrected with the MPEG
+ * decoding software on the laptop by mirroring. The pan servo is fixed
+ * in the plane and the tilt servo is moved by the pan servo and moves
+ * the camera.
+ *
+ *
+ * pan servo, tilt set to 90\xB0, looking from top:
+ *
+ * plane front
+ *
+ * ^
+ * I
+ * I 45\xB0
+ * I /
+ * I/
+ * I------- 0\xB0
+ * I\
+ * I \
+ * I -45\xB0
+ * I
+ *
+ * plane back
+ *
+ *
+ * tilt servo, pan set to 0\xB0, looking from back:
+ *
+ * plane left --------------- plane right
+ * / I \
+ * / I \
+ * -45\xB0 I 45\xB0
+ * 0\xB0
+ *
+ */
+
+ /* fPan = 0 -> camera looks along the wing
+ 90 -> camera looks in flight direction
+ -90 -> camera looks backwards
+ */
+ /* fixed to the plane*/
+ *fPan = (float)(atan2(svObjectPositionForPlane2.fx,
fabs(svObjectPositionForPlane2.fy)));
+
+ /* fTilt = 0 -> camera looks down
+ 90 -> camera looks into right hemisphere
+ -90 -> camera looks into left hemispere
+ actually the camera always looks more or less downwards, but never upwards
+ */
+ *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx *
svObjectPositionForPlane2.fx
+ + svObjectPositionForPlane2.fy *
svObjectPositionForPlane2.fy ),
+ -svObjectPositionForPlane2.fz
+ ));
+ if (svObjectPositionForPlane2.fy < 0)
+ {
+ *fPan = -*fPan;
+ *fTilt = -*fTilt;
+ }
+
+#else
+#ifdef POINT_CAM_PITCH_ROLL
+
+/*
+ * This is for another two axes camera mechanisms. The tilt servo is fixed to
+ * the fuselage and moves the pan servo.
+ *
+ * tilt servo, looking from left:
+ *
+ * plane front <--------------- plane back
+ * / I \
+ * / I \
+ * 45\xB0 I -45\xB0
+ * 0\xB0
+ *
+ *
+ * pan servo, looking from back:
+ *
+ * plane left --------------- plane right
+ * / I \
+ * / I \
+ * 45\xB0 I -45\xB0
+ * 0\xB0
+ *
+ */
+
+ *fTilt = (float)(atan2( svObjectPositionForPlane2.fx,
-svObjectPositionForPlane2.fz));
+
+ *fPan = (float)(atan2(-svObjectPositionForPlane2.fy,
+ sqrt( svObjectPositionForPlane2.fx *
svObjectPositionForPlane2.fx
+ + svObjectPositionForPlane2.fz *
svObjectPositionForPlane2.fz )
+ ));
+
+#else
+#error at least one POINT_CAM_* camera mount has to be defined!
+#endif
+#endif
+#endif
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/modules/cam_control/point.h (from rev
5751, paparazzi3/trunk/sw/airborne/modules/vision/point.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/point.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/point.h 2010-08-30
19:54:26 UTC (rev 5752)
@@ -0,0 +1,34 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2005-2008 Arnold Schroeter
+ *
+ * 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 POINT_H
+#define POINT_H
+
+void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
+ float fRollAngle, float fPitchAngle, float fYawAngle,
+ float fObjectEast, float fObjectNorth, float fAltitude,
+ float *fPan, float *fTilt);
+
+#endif /* POINT_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5752] renamed vision to cam_control,
Felix Ruess <=