[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5749] made cam a module, see funjet_cam_example.xml
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5749] made cam a module, see funjet_cam_example.xml as an example |
|
Date: |
Mon, 30 Aug 2010 19:25:55 +0000 |
Revision: 5749
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5749
Author: flixr
Date: 2010-08-30 19:25:54 +0000 (Mon, 30 Aug 2010)
Log Message:
-----------
made cam a module, see funjet_cam_example.xml as an example
Modified Paths:
--------------
paparazzi3/trunk/conf/settings/cam.xml
paparazzi3/trunk/conf/settings/cam_pitch.xml
paparazzi3/trunk/sw/airborne/main_ap.c
Added Paths:
-----------
paparazzi3/trunk/conf/airframes/funjet_cam_example.xml
paparazzi3/trunk/conf/modules/cam_point.xml
paparazzi3/trunk/sw/airborne/modules/vision/cam.c
paparazzi3/trunk/sw/airborne/modules/vision/cam.h
paparazzi3/trunk/sw/airborne/modules/vision/point.c
paparazzi3/trunk/sw/airborne/modules/vision/point.h
Added: paparazzi3/trunk/conf/airframes/funjet_cam_example.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/funjet_cam_example.xml
(rev 0)
+++ paparazzi3/trunk/conf/airframes/funjet_cam_example.xml 2010-08-30
19:25:54 UTC (rev 5749)
@@ -0,0 +1,197 @@
+<!DOCTYPE airframe SYSTEM "airframe.dtd">
+
+<!-- Funjet Multiplex (http://www.multiplex-rc.de/), Jeti ECO 25
+ Tiny 2.11 board (http://paparazzi.enac.fr/wiki/Tiny_v2)
+ PerkinElmer TPS334 IR Sensors
+ Tilted infrared sensor
(http://paparazzi.enac.fr/wiki/Image:Tiny_v2_1_Funjet.jpg)
+ XBee modem with AT firmware
+ LEA 5H GPS
+-->
+
+<airframe name="Funjet Tiny 2.11">
+
+ <firmware name="fixedwing">
+ <target name="sim" board="pc"/>
+ <target name="ap" board="tiny_2.11">
+ <param name="FLASH_MODE" value="IAP"/>
+ <define name="AGR_CLIMB"/>
+ <define name="LOITER_TRIM"/>
+ <define name="ALT_KALMAN"/>
+ </target>
+
+ <subsystem name="radio_control" type="ppm"/>
+ <subsystem name="joystick"/>
+
+ <!-- Communication -->
+ <subsystem name="telemetry" type="transparent"/>
+
+ <!-- Actuators are automatically chosen according to board-->
+ <subsystem name="control"/>
+ <!-- Sensors -->
+ <subsystem name="attitude" type="infrared"/>
+ <subsystem name="gps" type="ublox_lea5h"/>
+
+ <subsystem name="navigation"/>
+ </firmware>
+
+
+ <firmware name="setup">
+ <target name="tunnel" board="tiny_2.11"/>
+ <target name="setup_actuators" board="tiny_2.11"/>
+ </firmware>
+
+ <modules main_freq="60">
+ <load name="cam_point.xml">
+ <param name="POINT_CAM_YAW_PITCH" value="1"/>
+ <param name="SHOW_CAM_COORDINATES" value="1"/>
+ </load>
+ <load name="sys_mon.xml"/>
+ </modules>
+
+<!-- commands section -->
+ <servos>
+ <servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
+ <servo name="AILEVON_LEFT" no="2" min="1900" neutral="1521" max="1100"/>
+ <servo name="AILEVON_RIGHT" no="6" min="1100" neutral="1510" max="1900"/>
+ </servos>
+
+ <commands>
+ <axis name="THROTTLE" failsafe_value="0"/>
+ <axis name="ROLL" failsafe_value="0"/>
+ <axis name="PITCH" failsafe_value="0"/>
+ </commands>
+
+ <rc_commands>
+ <set command="THROTTLE" value="@THROTTLE"/>
+ <set command="ROLL" value="@ROLL"/>
+ <set command="PITCH" value="@PITCH"/>
+ </rc_commands>
+
+ <section name="MIXER">
+ <define name="AILEVON_AILERON_RATE" value="0.45"/>
+ <define name="AILEVON_ELEVATOR_RATE" value="0.8"/>
+ </section>
+
+ <command_laws>
+ <let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
+ <let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
+ <set servo="MOTOR" value="@THROTTLE"/>
+ <set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
+ <set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
+ </command_laws>
+
+ <section name="AUTO1" prefix="AUTO1_">
+ <define name="MAX_ROLL" value="0.85"/>
+ <define name="MAX_PITCH" value="0.6"/>
+ </section>
+
+ <section name="INFRARED" prefix="IR_">
+ <define name="ADC_IR1_NEUTRAL" value="512"/>
+ <define name="ADC_IR2_NEUTRAL" value="512"/>
+ <define name="ADC_TOP_NEUTRAL" value="512"/>
+
+ <define name="CORRECTION_UP" value="1."/>
+ <define name="CORRECTION_DOWN" value="1."/>
+ <define name="CORRECTION_LEFT" value="1."/>
+ <define name="CORRECTION_RIGHT" value="1."/>
+
+ <define name="LATERAL_CORRECTION" value="-1"/>
+ <define name="LONGITUDINAL_CORRECTION" value="1"/>
+ <define name="VERTICAL_CORRECTION" value="1.5"/>
+
+ <define name="HORIZ_SENSOR_TILTED" value="1"/>
+ <define name="IR1_SIGN" value="1"/>
+ <define name="IR2_SIGN" value="-1"/>
+ <define name="TOP_SIGN" value="-1"/>
+
+ <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+ <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+ </section>
+
+ <section name="BAT">
+ <define name="MILLIAMP_AT_FULL_THROTTLE" value="10000."/>
+ <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+ </section>
+
+ <section name="MISC">
+ <define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
+ <define name="CARROT" value="5." unit="s"/>
+ <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+ <define name="CONTROL_RATE" value="60" unit="Hz"/>
+ <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
+
+ <define name="TRIGGER_DELAY" value="1."/>
+ <define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
+ <define name="MIN_CIRCLE_RADIUS" value="50."/>
+ </section>
+
+ <section name="VERTICAL CONTROL" prefix="V_CTL_">
+ <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+ <!-- outer loop proportional gain -->
+ <define name="ALTITUDE_PGAIN" value="-0.06"/> <!-- -0.024 -->
+ <!-- outer loop saturation -->
+ <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+ <!-- auto throttle inner loop -->
+ <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.45"/>
+ <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.35"/>
+ <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
+ <define name="AUTO_THROTTLE_LOITER_TRIM" value="1000"/>
+ <define name="AUTO_THROTTLE_DASH_TRIM" value="-1200"/>
+ <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.2"
unit="%/(m/s)"/>
+ <define name="AUTO_THROTTLE_PGAIN" value="-0.023"/> <!-- -0.012 -->
+ <define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
+ <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+ <!-- auto pitch inner loop -->
+ <define name="AUTO_PITCH_PGAIN" value="-0.06"/> <!-- -0.03 -->
+ <define name="AUTO_PITCH_IGAIN" value="0.0"/>
+ <define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
+ <define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
+
+ <define name="THROTTLE_SLEW" value="0.1"/>
+ </section>
+
+ <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+ <define name="COURSE_PGAIN" value="-0.9"/>
+ <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
+ <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+ <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+ <define name="ROLL_PGAIN" value="6600."/>
+ <define name="AILERON_OF_THROTTLE" value="0.0"/>
+ <define name="PITCH_PGAIN" value="-5500."/>
+ <define name="PITCH_DGAIN" value="0.4"/>
+
+ <define name="ELEVATOR_OF_ROLL" value="2400"/>
+ </section>
+
+ <section name="NAV">
+ <define name="NAV_PITCH" value="0."/>
+ <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
+ </section>
+
+ <section name="AGGRESSIVE" prefix="AGR_">
+ <define name="BLEND_START" value="50"/><!-- Altitude Error to Initiate
Aggressive Climb CANNOT BE ZERO!!-->
+ <define name="BLEND_END" value="15"/><!-- Altitude Error to Blend
Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
+ <define name="CLIMB_THROTTLE" value="0.9"/><!-- Gaz for Aggressive Climb
-->
+ <define name="CLIMB_PITCH" value="0.35"/><!-- Pitch for Aggressive Climb
-->
+ <define name="DESCENT_THROTTLE" value="0.05"/><!-- Gaz for Aggressive
Decent -->
+ <define name="DESCENT_PITCH" value="-0.35"/><!-- Pitch for Aggressive
Decent -->
+ <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for
Altitude Error Equal to Start Altitude -->
+ <define name="DESCENT_NAV_RATIO" value="1.0"/>
+ </section>
+
+ <section name="FAILSAFE" prefix="FAILSAFE_">
+ <define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
+ <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
+ <define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
+ <define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
+ <define name="HOME_RADIUS" value="100" unit="m"/>
+ </section>
+
+ <section name="SIMU">
+ <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
+ </section>
+
+</airframe>
Added: paparazzi3/trunk/conf/modules/cam_point.xml
===================================================================
--- paparazzi3/trunk/conf/modules/cam_point.xml (rev 0)
+++ paparazzi3/trunk/conf/modules/cam_point.xml 2010-08-30 19:25:54 UTC (rev
5749)
@@ -0,0 +1,18 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="cam_point" dir="vision">
+ <!-- depend require="booz_pwm|led" -->
+ <header>
+ <file name="cam.h"/>
+ <file name="point.h"/>
+ </header>
+ <init fun="cam_init()"/>
+ <periodic fun="cam_periodic()" freq="10."/>
+ <makefile>
+ <flag name="CAM"/>
+ <flag name="MOBILE_CAM"/>
+ <flag name="POINT_CAM"/>
+ <file name="cam.c"/>
+ <file name="point.c"/>
+ </makefile>
+</module>
Modified: paparazzi3/trunk/conf/settings/cam.xml
===================================================================
--- paparazzi3/trunk/conf/settings/cam.xml 2010-08-30 16:34:27 UTC (rev
5748)
+++ paparazzi3/trunk/conf/settings/cam.xml 2010-08-30 19:25:54 UTC (rev
5749)
@@ -3,7 +3,7 @@
<settings>
<dl_settings NAME="control">
<dl_settings name="cam">
- <dl_setting MAX="5" MIN="0" STEP="1" module="cam" VAR="cam_mode">
+ <dl_setting MAX="5" MIN="0" STEP="1" module="vision/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="cam"
VAR="cam_tilt_c"/>
- <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam"
VAR="cam_pan_c"/>
+ <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_settings>
<dl_settings name="target">
- <dl_setting min="1" max="27" step="1" module="cam" var="cam_target_wp"
shortname="wp"/>
+ <dl_setting min="1" max="27" step="1" module="vision/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 16:34:27 UTC
(rev 5748)
+++ paparazzi3/trunk/conf/settings/cam_pitch.xml 2010-08-30 19:25:54 UTC
(rev 5749)
@@ -3,7 +3,7 @@
<settings>
<dl_settings NAME="control">
<dl_settings name="cam">
- <dl_setting MAX="5" MIN="0" STEP="1" module="cam" VAR="cam_mode">
+ <dl_setting MAX="5" MIN="0" STEP="1" module="vison/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="cam"
VAR="cam_tilt_c">
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vison/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="cam"
VAR="cam_pan_c"/>
+ <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="vison/cam"
VAR="cam_pan_c"/>
</dl_settings>
<!--
<dl_settings name="target">
-<dl_setting min="1" max="10" step="1" module="cam" var="cam_target_wp"
shortname="wp"/>
+<dl_setting min="1" max="10" step="1" module="vison/cam" var="cam_target_wp"
shortname="wp"/>
</dl_settings>
-->
</dl_settings>
Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c 2010-08-30 16:34:27 UTC (rev
5748)
+++ paparazzi3/trunk/sw/airborne/main_ap.c 2010-08-30 19:25:54 UTC (rev
5749)
@@ -683,9 +683,6 @@
inter_mcu_received_ap = TRUE;
#endif
-#ifdef MOBILE_CAM
- cam_periodic();
-#endif
}
modules_periodic_task();
Added: paparazzi3/trunk/sw/airborne/modules/vision/cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/cam.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/cam.c 2010-08-30 19:25:54 UTC
(rev 5749)
@@ -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
+}
Added: paparazzi3/trunk/sw/airborne/modules/vision/cam.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/cam.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/cam.h 2010-08-30 19:25:54 UTC
(rev 5749)
@@ -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/vision/point.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/point.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/point.c 2010-08-30 19:25:54 UTC
(rev 5749)
@@ -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
+}
Added: paparazzi3/trunk/sw/airborne/modules/vision/point.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/point.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/point.h 2010-08-30 19:25:54 UTC
(rev 5749)
@@ -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] [5749] made cam a module, see funjet_cam_example.xml as an example,
Felix Ruess <=