diff --git a/sw/airborne/common_nav.h b/sw/airborne/common_nav.h index 84018a022f..3047e5c111 100644 --- a/sw/airborne/common_nav.h +++ b/sw/airborne/common_nav.h @@ -18,7 +18,7 @@ struct point { #define WaypointY(_wp) (waypoints[_wp].y) #define WaypointAlt(_wp) (waypoints[_wp].a) -extern void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt); +extern void nav_move_waypoint(uint8_t wp_id, float utm_east, float utm_north, float alt); extern const uint8_t nb_waypoint; extern struct point waypoints[]; @@ -65,9 +65,11 @@ void common_nav_periodic_task_4Hz(void); #define Or(x, y) ((x) || (y)) #define Min(x,y) (x < y ? x : y) #define Max(x,y) (x > y ? x : y) -#define NavBlockTime() (block_time) #define LessThan(_x, _y) ((_x) < (_y)) +/** Time in s since the entrance in the current block */ +#define NavBlockTime() (block_time) + #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 555bb0ceef..dfc20a22ce 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -103,6 +103,7 @@ extern void nav_circle_XY(float x, float y, float radius); #define NavCircleWaypoint(wp, radius) \ nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius) +/** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ while (x < 0) x += 360; \ while (x >= 360) x -= 360; \ @@ -110,7 +111,10 @@ extern void nav_circle_XY(float x, float y, float radius); #define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI)) #define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; }) + +/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/ #define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); }) + #define NavCourseCloseTo(x) ({ float _course = x; NormCourse(_course); float deg = DegOfRad(estimator_hspeed_dir); (Min(_course, 350) < deg && deg < _course+10); }) /*********** Navigation along a line *************************************/ diff --git a/sw/airborne/traffic_info.h b/sw/airborne/traffic_info.h index dd3aaecee0..a4cbe031a3 100644 --- a/sw/airborne/traffic_info.h +++ b/sw/airborne/traffic_info.h @@ -34,8 +34,8 @@ struct ac_info_ { uint8_t ac_id; - float east; /* m */ - float north; /* m */ + float east; /* m relative to nav_utm_east0 */ + float north; /* m relative to nav_utm_north0 */ float course; /* rad (CW) */ float alt; /* m */ float gspeed; /* m/s */