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

MAV07

Browse Source
This commit is contained in:
Pascal Brisset
2007-09-22 13:03:29 +00:00
parent cd47f9d10f
commit 95f7dffd85
2 changed files with 30 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/bomb.c
+26 -6
View File
@@ -22,7 +22,7 @@ float bomb_trigger_delay = TRIGGER_DELAY;
float airspeed = 14.;
float bomb_start_qdr;
#define CLIMB_TIME 5 /* s */
#define CLIMB_TIME 3 /* s */
#define SAFE_CLIMB 20 /* m */
static float bomb_x, bomb_y, bomb_z;
@@ -85,7 +85,7 @@ unit_t bomb_update_release( uint8_t wp_target ) {
/** Compute a first approximation for the RELEASE waypoint from wind and
expected airspeed and altitude */
unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start ) {
unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float bomb_radius ) {
waypoints[WP_RELEASE].a = waypoints[wp_start].a;
bomb_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a;
bomb_x = 0.;
@@ -101,10 +101,12 @@ unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start ) {
float x1 = x_0 / d;
float y_1 = y_0 / d;
waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * BOMB_RADIUS;
waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * BOMB_RADIUS;
waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * bomb_radius;
waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * bomb_radius;
waypoints[WP_BASELEG].a = waypoints[wp_start].a;
bomb_start_qdr = M_PI - atan2(-y_1, -x1);
if (bomb_radius < 0)
bomb_start_qdr += M_PI;
bomb_vx = x1 * airspeed + wind_east;
bomb_vy = y_1 * airspeed + wind_north;
@@ -115,8 +117,8 @@ unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start ) {
integrate(wp_target);
waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + CLIMB_TIME * vx0;
waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + CLIMB_TIME * vy0;
waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0;
waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0;
waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB;
return 0;
@@ -129,4 +131,22 @@ unit_t bomb_shoot( void ) {
return 0;
}
/* Compute start and end waypoints to be aligned on w1-w2 */
bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) {
float x_0 = waypoints[w2].x - waypoints[w1].x;
float y_0 = waypoints[w2].y - waypoints[w1].y;
/* Unit vector from W1 to W2 */
float d = sqrt(x_0*x_0+y_0*y_0);
x_0 /= d;
y_0 /= d;
waypoints[wp_before].x = waypoints[w1].x - d_before * x_0;
waypoints[wp_before].y = waypoints[w1].y - d_before * y_0;
waypoints[wp_after].x = waypoints[w2].x + d_after * x_0;
waypoints[wp_after].y = waypoints[w2].y + d_after * y_0;
return FALSE;
}
#endif /* WP_RELEASE */
sw/airborne/bomb.h
+4 -3
View File
@@ -1,15 +1,16 @@
#ifndef BOMB_H
#define BOMB_H
#define BOMB_RADIUS 100
#define MY_BOMB_RADIUS DEFAULT_CIRCLE_RADIUS
extern unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start );
extern unit_t bomb_compute_approach( uint8_t wp_target, uint8_t wp_start, float radius );
extern unit_t bomb_update_release( uint8_t wp_target );
extern unit_t bomb_shoot( void );
extern float bomb_trigger_delay, bomb_start_qdr;
extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after);
#define BombComputeApproach(_target, _start) bomb_compute_approach(_target, _start)
#define BombComputeApproach(_target, _start, _radius) bomb_compute_approach(_target, _start, _radius)
#define BombUpdateRelease(_wp) bomb_update_release(_wp)
#define BombReadyToShoot() bomb_ready_to_shoot()
#define BombShoot() bomb_shoot()