camera control

conf/conf.xml

@@ -22,7 +22,7 @@
     ac_id="2"
     airframe="airframes/twinstar2.xml" 
     radio="radios/mc3030.xml" 
-    flight_plan="flight_plans/muret2.xml"
+    flight_plan="flight_plans/muret_cam.xml"
   />
   
   <map

conf/flight_plans/flight_plan.dtd

@@ -83,12 +83,16 @@ from_qdr CDATA #IMPLIED
 from_dist CDATA #IMPLIED
 hmode CDATA #IMPLIED
 vmode CDATA #IMPLIED
+cam_mode (null|fix|manual|nadir|target) #IMPLIED
+target CDATA #IMPLIED
 pitch CDATA #IMPLIED
 alt CDATA #IMPLIED
 until CDATA #IMPLIED
 gaz CDATA #IMPLIED>
 
 <!ATTLIST xyz
+cam_mode (null|fix|manual|nadir|target) #IMPLIED
+target CDATA #IMPLIED
 radius CDATA #IMPLIED>
 
 <!ATTLIST circle
@@ -96,6 +100,8 @@ wp CDATA #REQUIRED
 radius CDATA #REQUIRED
 alt CDATA #IMPLIED
 vmode CDATA #IMPLIED
+cam_mode (null|fix|manual|nadir|target) #IMPLIED
+target CDATA #IMPLIED
 climb CDATA #IMPLIED
 until CDATA #IMPLIED>
 

conf/flight_plans/muret_cam.xml

@@ -0,0 +1,30 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan NAME="example - Muret for camera control" LON0="1.27289" MAX_DIST_FROM_HOME="1000" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
+  <rc_control>
+    <mode NAME="AUTO1">
+      <setting VAR="ir_pitch_neutral" RANGE="60." RC="gain_1_up" TYPE="int16"/>
+      <setting VAR="ir_roll_neutral" RANGE="-60." RC="gain_1_down" TYPE="int16"/>
+    </mode>
+    <mode NAME="AUTO2">
+      <setting VAR="course_pgain" RANGE="0.1" RC="gain_1_up" TYPE="float"/>
+      <setting VAR="pitch_of_roll" RANGE=".2" RC="gain_1_down" TYPE="float"/>
+    </mode>
+  </rc_control>
+  <waypoints utm_x0="360284.8" utm_y0="4813595.5">
+    <waypoint name="HOME" x="0.0" y="30.0" lat="43.4625000078" lon="1.27288231142" alt="250."/>
+     </waypoints>
+
+  <blocks>
+    <block NAME="init">
+      <exception COND="(RcEvent1())" DEROUTE="init"/>
+
+      <circle wp="HOME" alt="GROUND_ALT+50" radius="75" cam_mode="nadir" until="stage_time > 100"/>
+      <circle wp="HOME" alt="GROUND_ALT+50" radius="75" cam_mode="manual" until="stage_time > 100"/>
+      <circle wp="HOME" alt="GROUND_ALT+50" radius="75" cam_mode="target" target="HOME" until="stage_time > 100"/>
+      <circle wp="HOME" alt="GROUND_ALT+50" radius="75" cam_mode="target" until="stage_time > 100"/>
+      <xyz cam_mode="target"/>
+    </block>
+  </blocks>
+</flight_plan>
+

sw/airborne/autopilot/Makefile

@@ -52,20 +52,21 @@ INCLUDES = -I $(FBW) -I ../../include -I $(VARINCLUDE) -I $(ACINCLUDE)
 GPS = gps_ubx.c
 GPS_FLAGS=-DUBX
 
-$(TARGET).srcs	=							\
+$(TARGET).srcs	= \
-	main.c								\
+	main.c \
-	modem.c								\
+	modem.c \
-	link_fbw.c							\
+	link_fbw.c \
-	spi.c								\
+	spi.c \
-	adc.c								\
+	adc.c \
-	$(GPS)								\
+	$(GPS) \
-	infrared.c							\
+	infrared.c \
-	pid.c								\
+	pid.c \
-	nav.c								\
+	nav.c \
-	uart.c                                                          \
+	uart.c \
-        estimator.c                                                     \
+        estimator.c \
-	if_calib.c							\
+	if_calib.c \
-	mainloop.c
+	mainloop.c \
+	cam.c
 
 include ../../../conf/Makefile.local
 include ../../../conf/Makefile.avr

sw/airborne/autopilot/cam.c

@@ -0,0 +1,102 @@
+/*
+ * $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 Regroup functions to control the camera
+ *
+ */
+
+#include <math.h>
+#include "cam.h"
+#include "nav.h"
+#include "autopilot.h"
+#include "flight_plan.h"
+#include "estimator.h"
+#include "link_fbw.h"
+
+#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
+#define DELTA_ALPHA 0.2
+
+#define MAX_CAM_ROLL M_PI/2
+#define MAX_CAM_PITCH M_PI/3
+
+#define target_alt GROUND_ALT
+
+float phi_c, theta_c;
+float target_x, target_y;
+
+void cam_manual() {
+  int16_t yaw = from_fbw.channels[RADIO_YAW];
+  if (yaw > MIN_PPRZ_CAM || yaw < -MIN_PPRZ_CAM) {
+    phi_c += FLOAT_OF_PPRZ(yaw, 0, DELTA_ALPHA);
+    phi_c = Min(phi_c, MAX_CAM_ROLL);
+    phi_c = Max(phi_c, -MAX_CAM_ROLL);
+  }
+  int16_t pitch = from_fbw.channels[RADIO_PITCH];
+  if (pitch > MIN_PPRZ_CAM || pitch < -MIN_PPRZ_CAM) {
+    theta_c += FLOAT_OF_PPRZ(pitch, 0, DELTA_ALPHA); 
+    theta_c = Min(theta_c, MAX_CAM_PITCH);
+    theta_c = Max(theta_c, -MAX_CAM_PITCH);
+  }
+}
+
+void cam_nadir() {
+  phi_c = -estimator_phi;
+  theta_c = -estimator_theta;
+}
+
+void cam_target() {
+  float h = estimator_z - target_alt;
+  float c_psi = cos(estimator_psi);
+  float s_psi = sin(estimator_psi);
+  phi_c = atan((target_x*c_psi - target_y*s_psi - estimator_x) / h) - estimator_phi;
+  theta_c = atan((target_x*s_psi + target_y*c_psi - estimator_y) / h)- estimator_theta;
+}
+
+#define MAX_DIST_TARGET 500.
+
+void cam_manual_target() {
+  int16_t yaw = from_fbw.channels[RADIO_YAW];
+  if (yaw > MIN_PPRZ || yaw < -MIN_PPRZ) {
+    target_x += FLOAT_OF_PPRZ(yaw, 0, -20.);
+    target_x = Min(target_x, MAX_DIST_TARGET + estimator_x);
+    target_x = Max(target_x, -MAX_DIST_TARGET + estimator_x);
+  }
+  int16_t pitch = from_fbw.channels[RADIO_PITCH];
+  if (pitch > MIN_PPRZ || pitch < -MIN_PPRZ) {
+    target_y += FLOAT_OF_PPRZ(pitch, 0, -20.);
+    target_y = Min(target_y, MAX_DIST_TARGET + estimator_y);
+    target_y = Max(target_y, -MAX_DIST_TARGET + estimator_y);
+  }
+  cam_target();
+}
+
+void cam_waypoint_target(uint8_t wp) {
+  target_x = waypoints[wp].x;
+  target_y = waypoints[wp].y;
+}
+
+void cam_carrot() {
+  target_x = carrot_x;
+  target_y = carrot_y;
+}

sw/airborne/autopilot/cam.h

@@ -0,0 +1,47 @@
+/*
+ * $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 Regroup functions to control the camera
+ *
+ */
+
+#ifndef CAM_H
+#define CAM_H
+
+#include <inttypes.h>
+
+
+extern float phi_c, theta_c;
+extern float target_x, target_y;
+
+#define CamNull() { phi_c = 0; theta_c = 0; }
+#define CamFix() {}
+
+void cam_nadir();
+void cam_manual();
+void cam_manual_target();
+void cam_waypoint_target(uint8_t wp);
+void cam_carrot();
+
+#endif

sw/airborne/autopilot/main.c

@@ -42,6 +42,7 @@
 #include "autopilot.h"
 #include "estimator.h"
 #include "if_calib.h"
+#include "cam.h"
 
 //
 //
@@ -377,6 +378,13 @@ void navigation_task( void ) {
     nav_update();
   
   DOWNLINK_SEND_NAVIGATION(&nav_block, &nav_stage, &estimator_x, &estimator_y, &desired_course, &dist2_to_wp, &course_pgain, &dist2_to_home);
+
+  int16_t x = target_x;
+  int16_t y = target_y;
+  int8_t phi = DegOfRad(phi_c);
+  int8_t theta = DegOfRad(theta_c);
+  DOWNLINK_SEND_CAM(&phi, &theta, &x, &y);
+  
   
   if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
     if (lateral_mode >= LATERAL_MODE_COURSE)

sw/airborne/autopilot/nav.c

@@ -36,6 +36,7 @@
 #include "autopilot.h"
 #include "link_fbw.h"
 #include "airframe.h"
+#include "cam.h"
 
 uint8_t nav_stage, nav_block;
 /** To save the current stage when an exception is raised */
@@ -45,6 +46,7 @@ static uint8_t last_wp;
 float rc_pitch;
 uint16_t stage_time, block_time;
 float stage_time_ds;
+float carrot_x, carrot_y;
 static float sum_alpha, last_alpha;
 /** represents in percent the stage where the uav is on a route
  *  or the qdr when uav circles.
@@ -83,8 +85,6 @@ static void glide_to(uint8_t last_wp, uint8_t wp);
 
 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
 
-#define DegOfRad(x) ((x) / M_PI * 180.)
-#define RadOfDeg(x) ((x)/180. * M_PI)
 #define NormCourse(x) { \
   while (x < 0) x += 360; \
   while (x >= 360) x -= 360; \
@@ -121,7 +121,6 @@ static float qdr;
 #define MAX_HEIGHT_CARROT 150.
 
 #define Goto3D(radius) { \
-  static float carrot_x, carrot_y; \
   if (pprz_mode == PPRZ_MODE_AUTO2) { \
     int16_t yaw = from_fbw.channels[RADIO_YAW]; \
     if (yaw > MIN_DX || yaw < -MIN_DX) { \
@@ -150,8 +149,6 @@ static float qdr;
 
 #define And(x, y) ((x) && (y))
 #define Or(x, y) ((x) || (y))
-#define Min(x,y) (x < y ? x : y)
-#define Max(x,y) (x > y ? x : y)
 #define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)
 
 #include "flight_plan.h"

sw/airborne/autopilot/nav.h

@@ -58,6 +58,7 @@ extern float stage_time_ds;
 /** in number of circle */
 extern float circle_count;
 extern float nav_desired_roll;
+extern float carrot_x, carrot_y;
 
 void nav_update(void);
 void nav_home(void);

sw/include/std.h

@@ -42,4 +42,11 @@ typedef uint8_t bool_t;
 #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
 #endif
 
+
+#define DegOfRad(x) ((x) / M_PI * 180.)
+#define RadOfDeg(x) ((x)/180. * M_PI)
+
+#define Min(x,y) (x < y ? x : y)
+#define Max(x,y) (x > y ? x : y)
+
 #endif /* STD_H */

sw/simulator/Makefile

@@ -32,7 +32,7 @@ SIMHCMO=$(SIMHML:%.ml=%.cmo)
 SIMSML = stdlib.ml data.ml flightModel.ml gps.ml sitl.ml sim.ml
 SIMSCMO=$(SIMSML:%.ml=%.cmo)
 SIMSCMX=$(SIMSML:%.ml=%.cmx)
-SIMSC = sim_ir.c sim_gps.c sim_ap.c estimator.c pid.c nav.c main.c
+SIMSC = sim_ir.c sim_gps.c sim_ap.c estimator.c pid.c nav.c main.c cam.c
 SIMSO=$(SIMSC:%.c=$(OBJDIR)/%.o)
 SIMSA=sims.cma
 

sw/tools/gen_flight_plan.ml

@@ -125,6 +125,32 @@ let stage = ref 0
 
 let output_label l = lprintf "Label(%s)\n" l
 
+let get_index_waypoint = fun x l ->
+  try
+    string_of_int (List.assoc x l)
+  with
+    Not_found -> failwith (sprintf "Unknown waypoint: %s\n" x)
+
+let output_cam_mode = fun x index_of_waypoints ->
+  let m =  try Xml.attrib x "cam_mode" with _ -> "fix" in
+  match m with
+    "null" -> lprintf "CamNull()";
+  | "fix" -> lprintf "CamFix()";
+  | "manual" -> lprintf "cam_manual();\n"
+  | "nadir" -> lprintf "cam_nadir();\n"
+  | "target" ->
+      if Xml.tag x = "xyz" then
+	lprintf "cam_carrot();\n"
+      else begin
+	try 
+	  let wp = Xml.attrib x "target" in
+	  let i = get_index_waypoint wp index_of_waypoints in
+	  lprintf "cam_waypoint_target(%s);\n" i
+	with _ ->
+	  lprintf "cam_manual_target();\n"
+      end
+  | _ -> failwith (sprintf "Error: unknown '%s' cam mode" m)
+
 let output_vmode x wp last_wp =
   let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in
   if pitch = "auto"
@@ -189,11 +215,6 @@ let output_hmode x wp last_wp =
     ExtXml.Error _ -> lprintf "fly_to(%s);\n" wp; "direct" (* Default behaviour *)
 	  
 
-let get_index_waypoint = fun x l ->
-  try
-    string_of_int (List.assoc x l)
-  with
-    Not_found -> failwith (sprintf "Unknown waypoint: %s\n" x)
 
 	
 let rec compile_stage = fun block x ->
@@ -261,6 +282,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	right (); 
 	lprintf "desired_course = RadOfDeg(%s);\n" (parsed_attrib x "course");
 	ignore (output_vmode x "" "");
+	output_cam_mode x index_of_waypoints;
 	left (); lprintf "}\n";
 	lprintf "return;\n"
     | "follow" ->
@@ -268,6 +290,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	let id = ExtXml.attrib x "ac_id" in
 	let d = ExtXml.attrib x "distance" in
 	lprintf "Follow(%s, %s);\n" id d;
+	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
     | "attitude" ->
 	stage ();
@@ -277,6 +300,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	lprintf "lateral_mode = LATERAL_MODE_ROLL;\n";
 	lprintf "nav_desired_roll = RadOfDeg(%s);\n" (parsed_attrib x "roll");
 	ignore (output_vmode x "" "");
+	output_cam_mode x index_of_waypoints;
 	left (); lprintf "}\n";
 	lprintf "return;\n"
     | "go" ->
@@ -300,6 +324,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	let vmode = output_vmode x wp last_wp in
 	if vmode = "glide" && hmode <> "route" then
 	  failwith "glide vmode requires route hmode";
+	output_cam_mode x index_of_waypoints;
 	left (); lprintf "}\n";
 	lprintf "return;\n"
     | "stay" ->
@@ -319,6 +344,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	stage ();
 	let r = try parsed_attrib  x "radius" with _ -> "100" in
 	lprintf "Goto3D(%s)\n" r;
+	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
     | "circle" ->
 	stage ();
@@ -333,6 +359,7 @@ let rec print_stage = fun index_of_waypoints x ->
 	  with
 	    ExtXml.Error _ -> ()
 	end;
+	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
     | "set" ->
 	stage ();