diff --git a/sw/airborne/bomb.c b/sw/airborne/bomb.c index 36c1d83787..d067880fb9 100644 --- a/sw/airborne/bomb.c +++ b/sw/airborne/bomb.c @@ -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 */ diff --git a/sw/airborne/bomb.h b/sw/airborne/bomb.h index 131a830ec4..a7c23435ed 100644 --- a/sw/airborne/bomb.h +++ b/sw/airborne/bomb.h @@ -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()