diff --git a/conf/conf.xml b/conf/conf.xml index 8f6c226fa4..b32c956da1 100644 --- a/conf/conf.xml +++ b/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" /> diff --git a/conf/flight_plans/muret_cam.xml b/conf/flight_plans/muret_cam.xml new file mode 100644 index 0000000000..da21ec62f3 --- /dev/null +++ b/conf/flight_plans/muret_cam.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile index 3a87b10629..56e8f9f486 100644 --- a/sw/airborne/autopilot/Makefile +++ b/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 = \ - main.c \ - modem.c \ - link_fbw.c \ - spi.c \ - adc.c \ - $(GPS) \ - infrared.c \ - pid.c \ - nav.c \ - uart.c \ - estimator.c \ - if_calib.c \ - mainloop.c +$(TARGET).srcs = \ + main.c \ + modem.c \ + link_fbw.c \ + spi.c \ + adc.c \ + $(GPS) \ + infrared.c \ + pid.c \ + nav.c \ + uart.c \ + estimator.c \ + if_calib.c \ + mainloop.c \ + cam.c include ../../../conf/Makefile.local include ../../../conf/Makefile.avr diff --git a/sw/airborne/autopilot/cam.c b/sw/airborne/autopilot/cam.c new file mode 100755 index 0000000000..0f2607e313 --- /dev/null +++ b/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 +#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; +} diff --git a/sw/airborne/autopilot/cam.h b/sw/airborne/autopilot/cam.h new file mode 100755 index 0000000000..f5c4fcc3c7 --- /dev/null +++ b/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 + + +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 diff --git a/sw/airborne/autopilot/main.c b/sw/airborne/autopilot/main.c index ad735466f9..541f3a8f1d 100644 --- a/sw/airborne/autopilot/main.c +++ b/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) diff --git a/sw/airborne/autopilot/nav.c b/sw/airborne/autopilot/nav.c index 33f7b5e135..03a67c43ce 100644 --- a/sw/airborne/autopilot/nav.c +++ b/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" diff --git a/sw/airborne/autopilot/nav.h b/sw/airborne/autopilot/nav.h index 1b314f66d9..59c191da7f 100644 --- a/sw/airborne/autopilot/nav.h +++ b/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); diff --git a/sw/include/std.h b/sw/include/std.h index 05d910df7e..f5d373230c 100644 --- a/sw/include/std.h +++ b/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 */ diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 3c1ac3f40e..e086341128 100644 --- a/sw/simulator/Makefile +++ b/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 diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index dce707293e..7492690885 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/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 ();