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

[rotorcraft] nav approaching_time implemented

Browse Source 
see #715
This commit is contained in:
kadir4172
2014-05-12 16:19:11 +03:00
committed by Felix Ruess
parent 4aa70f18f2
commit 7a3a55ab1d
2 changed files with 35 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+27 -8
View File
@@ -290,23 +290,42 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
horizontal_mode = HORIZONTAL_MODE_ROUTE;
}
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time) {
int32_t dist_to_point;
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
struct EnuCoor_i* pos = stateGetPositionEnu_i();
VECT2_DIFF(diff, waypoints[wp_idx], *pos);
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
INT32_VECT2_NORM(dist_to_point, diff);
//printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
//fflush(stdout);
//if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
if (from_idx > 0 && from_idx < NB_WAYPOINT) {
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
if (approaching_time >= 0) {
struct Int32Vect2 estimated_pos;
struct Int32Vect2 estimated_progress;
struct Int32Vect2 estimated_diff;
int32_t estimated_dist;
struct EnuCoor_i* speed = stateGetSpeedEnu_i();
VECT2_SMUL(estimated_progress, *speed, approaching_time);
INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
VECT2_SUM(estimated_pos, *pos, estimated_progress);
VECT2_DIFF(estimated_diff, waypoints[wp_idx], estimated_pos);
INT32_VECT2_RSHIFT(estimated_diff, estimated_diff, INT32_POS_FRAC);
INT32_VECT2_NORM(estimated_dist, estimated_diff);
if (estimated_dist < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
if (from_idx > 0 && from_idx < NB_WAYPOINT) {
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC);
return (estimated_diff.x * from_diff.x + estimated_diff.y * from_diff.y < 0);
}
}
else return FALSE;
return FALSE;
}
bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time) {
sw/airborne/firmwares/rotorcraft/navigation.h
+8 -3
View File
@@ -93,6 +93,10 @@ extern bool_t nav_set_heading_deg(float deg);
extern bool_t nav_set_heading_towards(float x, float y);
extern bool_t nav_set_heading_towards_waypoint(uint8_t wp);
/** default approaching_time for a wp */
#ifndef CARROT
#define CARROT 0
#endif
#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; })
#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; })
@@ -151,9 +155,9 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
}
/** Proximity tests on approaching a wp */
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
#define NavApproaching(wp, time) nav_approaching_from(wp, 0)
#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx, int16_t approaching_time);
#define NavApproaching(wp, time) nav_approaching_from(wp, 0, time)
#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from, time)
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
@@ -202,6 +206,7 @@ bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
#define nav_SetNavRadius(x) {}
#define navigation_SetFlightAltitude(x) { \
flight_altitude = x; \
nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \