Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 12:57:27 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

cam pointing handling

Browse Source
This commit is contained in:
Pascal Brisset
2005-11-22 07:10:43 +00:00
parent 519a9b4c6f
commit 42b19bb7f3
8 changed files with 47 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/twinstar1.xml
+11 -11
View File
@@ -67,21 +67,21 @@
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
</section>
<section name="SIMU">
<define name="ROLL_RESPONSE_FACTOR" value="2."/>
<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
<define name="WEIGHT" value="1.3"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
<section name="SIMU">
<define name="ROLL_RESPONSE_FACTOR" value="2."/>
<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
<define name="WEIGHT" value="1.3"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
<define name="DEFAULT_GAZ" value="CLIMB_LEVEL_GAZ+0.05" unit="%"/>
<define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
<define name="HOME_RADIUS" value="50" unit="m"/>
</section>
<define name="HOME_RADIUS" value="50" unit="m"/>
</section>
<section name="CAM" prefix="CAM_">
<define name="THETA0" value="45" unit="deg"/>
<define name="PHI0" value="-45" unit="deg"/>
<define name="THETA0" value="0" unit="deg"/>
<define name="PHI0" value="30" unit="deg"/>
</section>
<makefile>
LOCAL_CFLAGS += -DWAVECARD
conf/airframes/twinstar2.xml
+2 -2
View File
@@ -80,7 +80,7 @@
<define name="HOME_RADIUS" value="50" unit="m"/>
</section>
<section name="CAM" prefix="CAM_">
<define name="THETA0" value="45" unit="deg"/>
<define name="PHI0" value="45" unit="deg"/>
<define name="THETA0" value="15" unit="deg"/>
<define name="PHI0" value="-30" unit="deg"/>
</section>
</airframe>
conf/flight_plans/flight_plan.dtd
+11 -4
View File
@@ -96,7 +96,8 @@ from_qdr CDATA #IMPLIED
from_dist CDATA #IMPLIED
hmode CDATA #IMPLIED
vmode CDATA #IMPLIED
cam_mode (null|fix|manual|nadir|target) #IMPLIED
cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
cam_ac_target CDATA #IMPLIED
target CDATA #IMPLIED
pitch CDATA #IMPLIED
alt CDATA #IMPLIED
@@ -112,10 +113,15 @@ until CDATA #IMPLIED>
<!ATTLIST follow
ac_id CDATA #REQUIRED
distance CDATA #REQUIRED>
distance CDATA #REQUIRED
height CDATA #REQUIRED
cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
cam_ac_target CDATA #IMPLIED
>
<!ATTLIST xyz
cam_mode (null|fix|manual|nadir|target) #IMPLIED
cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
cam_ac_target CDATA #IMPLIED
target CDATA #IMPLIED
radius CDATA #IMPLIED>
@@ -126,8 +132,9 @@ wp_dist CDATA #IMPLIED
radius CDATA #REQUIRED
alt CDATA #IMPLIED
vmode CDATA #IMPLIED
cam_mode (null|fix|manual|nadir|target) #IMPLIED
cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
target CDATA #IMPLIED
cam_ac_target CDATA #IMPLIED
climb CDATA #IMPLIED
until CDATA #IMPLIED>
conf/flight_plans/mav05_ccw.xml
+1 -1
View File
@@ -22,7 +22,7 @@
<deroute BLOCK="zz.takeoff"/>
</block>
<block NAME="follow">
<follow ac_id="1" distance="20"/>
<follow ac_id="1" distance="25" height="25" cam_mode="target" target="zz.TARGET"/>
</block>
</blocks>
</flight_plan>
sw/airborne/autopilot/cam.c
+12 -3
View File
@@ -33,6 +33,7 @@
#include "flight_plan.h"
#include "estimator.h"
#include "link_fbw.h"
#include "traffic_info.h"
#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
#define DELTA_ALPHA 0.2
@@ -40,8 +41,6 @@
#define MAX_CAM_ROLL M_PI/2
#define MAX_CAM_PITCH M_PI/3
#define target_alt GROUND_ALT
#ifdef CAM_PHI0
float phi_c = RadOfDeg(CAM_PHI0);
#else
@@ -54,7 +53,7 @@ float theta_c = RadOfDeg(CAM_THETA0);
float theta_c;
#endif
float target_x, target_y;
float target_x, target_y, target_alt;
void cam_manual( void ) {
if (pprz_mode == PPRZ_MODE_AUTO2) {
@@ -102,12 +101,22 @@ void cam_manual_target( void ) {
target_y = Min(target_y, MAX_DIST_TARGET + estimator_y);
target_y = Max(target_y, -MAX_DIST_TARGET + estimator_y);
}
target_alt = GROUND_ALT;
cam_target();
}
void cam_waypoint_target( uint8_t wp ) {
target_x = waypoints[wp].x;
target_y = waypoints[wp].y;
target_alt = GROUND_ALT;
cam_target();
}
void cam_ac_target( uint8_t ac_id ) {
struct ac_info_ * ac = get_ac_info(ac_id);
target_x = ac->east;
target_y = ac->north;
target_alt = ac->alt;
cam_target();
}
sw/airborne/autopilot/nav.c
+2 -2
View File
@@ -166,10 +166,10 @@ static float qdr;
#define Max(x,y) (x > y ? x : y)
#define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)
#define Follow(_ac_id, _distance) { \
#define Follow(_ac_id, _distance, _height) { \
struct ac_info_ * ac = get_ac_info(_ac_id); \
vertical_mode = VERTICAL_MODE_AUTO_ALT; \
desired_altitude = ac->alt; \
desired_altitude = Max(ac->alt + _height, SECURITY_ALT); \
float alpha = M_PI/2 - ac->course; \
fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
}
sw/simulator/Makefile
+1 -1
View File
@@ -92,7 +92,7 @@ $(OBJDIR)/%.o : $(AP)/%.c
$(OBJDIR)/main.o : $(OBJDIR)/main.c
$(OCAMLCC) -c -o $@ -I $(SIMDIR) -I $(FBW) -I $(AP) -I ../include -I $(VARINCLUDE) $<
$(OBJDIR)/sim_gps.o $(OBJDIR)/nav.o $(OBJDIR)/main.o $(OBJDIR)/sim_ir.o $(OBJDIR)/sim_ap.o $(OBJDIR)/pid.o $(OBJDIR)/estimator.o : $(ACINCLUDE)/flight_plan.h $(ACINCLUDE)/airframe.h
$(OBJDIR)/sim_gps.o $(OBJDIR)/nav.o $(OBJDIR)/main.o $(OBJDIR)/sim_ir.o $(OBJDIR)/sim_ap.o $(OBJDIR)/pid.o $(OBJDIR)/estimator.o $(OBJDIR)/cam.o : $(ACINCLUDE)/flight_plan.h $(ACINCLUDE)/airframe.h
$(OBJDIR)/sim_ap.o : $(AP)/traffic_info.h $(AP)/nav.h
sw/tools/gen_flight_plan.ml
+7 -3
View File
@@ -153,6 +153,9 @@ let output_cam_mode = fun x index_of_waypoints ->
with _ ->
lprintf "cam_manual_target();\n"
end
| "follow" ->
let i = ExtXml.attrib x "cam_ac_target" in
lprintf "cam_ac_target(%s);\n" i
| _ -> failwith (sprintf "Error: unknown '%s' cam mode" m)
let output_vmode x wp last_wp =
@@ -292,9 +295,10 @@ let rec print_stage = fun index_of_waypoints x ->
lprintf "return;\n"
| "follow" ->
stage ();
let id = ExtXml.attrib x "ac_id" in
let d = ExtXml.attrib x "distance" in
lprintf "Follow(%s, %s);\n" id d;
let id = ExtXml.attrib x "ac_id"
and d = ExtXml.attrib x "distance"
and h = ExtXml.attrib x "height" in
lprintf "Follow(%s, %s, %s);\n" id d h;
output_cam_mode x index_of_waypoints;
lprintf "return;\n"
| "attitude" ->