Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2008-01-28 22:50:43 +00:00
parent de171a55b6
commit b54df78eee
10 changed files with 79 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/microjet6.xml
+2 -2
View File
@@ -245,8 +245,8 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c
ap.CFLAGS += -DGYRO -DADXRS150
ap.srcs += gyro.c
#ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
#ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
ap.srcs += nav_line.c
conf/airframes/tl.xml
+5
View File
@@ -39,6 +39,11 @@
<set command="CAM" value="@GAIN1"/>
</rc_commands>
<section name="horizontal_control">
<define name="NAV_GOTO_H_PGAIN" value="RadOfDeg(10)" unit="rad/m"/>
<define name="NAV_GOTO_H_DGAIN" value="0"/>
</section>
<makefile>
ARCHI=arm7
conf/airframes/twinjet1.xml
+1 -1
View File
@@ -224,7 +224,7 @@ ap.srcs += infrared.c estimator.c
ap.CFLAGS += -DGYRO -DADXRS150
ap.srcs += gyro.c
ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -DWIND_INFO
ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
ap.srcs += nav_line.c
conf/flight_plans/tl.xml
+2
View File
@@ -20,6 +20,8 @@
<stay wp="HOME"/>
</block>
<block name="target">
<call fun="NavSetWaypointHere(WP_TARGET)"/>
<set value="0" var="kill_throttle"/>
<stay wp="TARGET"/>
</block>
</blocks>
sw/airborne/common_nav.h
+6
View File
@@ -69,4 +69,10 @@ void common_nav_periodic_task_4Hz(void);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
#define NavSetWaypointHere(_wp) ({ \
waypoints[_wp].x = estimator_x; \
waypoints[_wp].y = estimator_y; \
FALSE; \
})
#endif /* COMMON_NAV_H */
sw/airborne/nav.h
-6
View File
@@ -101,12 +101,6 @@ void nav_home(void);
void nav_init(void);
void nav_without_gps(void);
#define NavSetWaypointHere(_wp) ({ \
waypoints[_wp].x = estimator_x; \
waypoints[_wp].y = estimator_y; \
FALSE; \
})
extern float nav_circle_trigo_qdr; /** Angle from center to mobile */
extern void nav_circle_XY(float x, float y, float radius);
sw/airborne/tl_control.c
+3 -3
View File
@@ -25,9 +25,9 @@ pprz_t tl_control_commands[COMMANDS_NB];
#define TL_CONTROL_RATE_R_MAX_SP 100.
/* float tl_control_attitude_phi_sp; */
/* float tl_control_attitude_theta_sp; */
/* float tl_control_attitude_psi_sp; */
float tl_control_attitude_phi_sp;
float tl_control_attitude_theta_sp;
float tl_control_attitude_psi_sp;
/* float tl_control_attitude_phi_theta_pgain; */
/* float tl_control_attitude_phi_theta_dgain; */
/* float tl_control_attitude_psi_pgain; */
sw/airborne/tl_estimator.c
+16
View File
@@ -5,6 +5,9 @@
bool_t estimator_in_flight;
uint16_t estimator_flight_time;
float tl_estimator_u;
float tl_estimator_v;
float estimator_x; /* m */
float estimator_y; /* m */
float estimator_z; /* altitude in m */
@@ -13,7 +16,11 @@ float estimator_speed; /* m/s */
float estimator_climb; /* m/s */
float estimator_course; /* rad, CCW */
float estimator_psi; /* rad, CCW */
void tl_estimator_init(void) {
tl_estimator_u = 0.;
tl_estimator_v = 0.;
}
void tl_estimator_use_gps(void) {
@@ -29,3 +36,12 @@ void tl_estimator_use_gps(void) {
estimator_climb = gps_climb / 100.;
estimator_course = RadOfDeg(gps_course / 10.);
}
void tl_estimator_to_body_frame(float east, float north,
float *front, float *right) {
float c = cos(estimator_psi);
float s = sin(estimator_psi);
*front = c * north + s * east;
*right = - s * north + c * east;
}
sw/airborne/tl_estimator.h
+7
View File
@@ -6,6 +6,9 @@
extern bool_t estimator_in_flight;
extern uint16_t estimator_flight_time;
extern float tl_estimator_u;
extern float tl_estimator_v;
extern float estimator_x; /* m */
extern float estimator_y; /* m */
extern float estimator_z; /* altitude in m */
@@ -14,9 +17,13 @@ extern float estimator_speed; /* m/s */
extern float estimator_climb; /* m/s */
extern float estimator_course; /* rad, CCW */
extern float estimator_psi; /* rad, CCW */
void tl_estimator_init(void);
void tl_estimator_use_gps(void);
void tl_estimator_to_body_frame(float east, float north, float *front, float *right);
#endif /* TL_ESTIMATOR_H */
sw/airborne/tl_nav.c
+37 -1
View File
@@ -5,11 +5,20 @@
#include "tl_control.h"
#include "gps.h"
#define NavGotoWaypoint(_wp) {}
float tl_nav_goto_h_pgain;
float tl_nav_goto_h_dgain;
#define TL_NAV_GOTO_MAX_PHI_COMMAND RadOfDeg(30.)
#define TL_NAV_GOTO_MAX_THETA_COMMAND RadOfDeg(30.)
static void nav_goto_waypoint(uint8_t wp);
#define NavGotoWaypoint(_wp) { nav_goto_waypoint(_wp); }
#define NavVerticalAutoThrottleMode(_foo) {}
#define NavVerticalAltitudeMode(_foo, _bar) {}
static void nav_home(void) {
kill_throttle = TRUE;
}
#define NAV_C
@@ -18,6 +27,8 @@ static void nav_home(void) {
void tl_nav_init(void) {
nav_block = 0;
nav_stage = 0;
tl_nav_goto_h_pgain = NAV_GOTO_H_PGAIN;
tl_nav_goto_h_dgain = NAV_GOTO_H_DGAIN;
}
void tl_nav_periodic_task(void) {
@@ -29,6 +40,31 @@ void nav_init_stage( void ) {
stage_time = 0;
}
static void fly_to_xy(float x_sp, float y_sp) {
/* get a position error vector */
float x_error = estimator_x - x_sp;
float y_error = estimator_y - y_sp;
/* Normalize the error */
float norm_error = sqrt(x_error*x_error+ y_error*y_error);
x_error /= norm_error;
y_error /= norm_error;
/* convert to body frame */
float y_unit_err_body, x_unit_err_body;
tl_estimator_to_body_frame(x_error, y_error, &x_unit_err_body, &y_unit_err_body);
/* Compute command */
float tl_nav_goto_phi_command = - tl_nav_goto_h_pgain * y_unit_err_body * norm_error + - tl_nav_goto_h_dgain * tl_estimator_v;
float tl_nav_goto_theta_command = tl_nav_goto_h_pgain * x_unit_err_body * norm_error + tl_nav_goto_h_dgain * tl_estimator_u;
Bound(tl_nav_goto_phi_command, -TL_NAV_GOTO_MAX_PHI_COMMAND, TL_NAV_GOTO_MAX_PHI_COMMAND);
Bound(tl_nav_goto_theta_command, -TL_NAV_GOTO_MAX_THETA_COMMAND, TL_NAV_GOTO_MAX_THETA_COMMAND);
tl_control_attitude_phi_sp = tl_nav_goto_phi_command;
tl_control_attitude_theta_sp = tl_nav_goto_theta_command;
}
static void nav_goto_waypoint(uint8_t wp) {
fly_to_xy(waypoints[wp].x, waypoints[wp].y);
}