From a58a758da3543ddb86f4ecd5ef1d26d2a7d97cb8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 4 Aug 2014 21:13:51 +0200 Subject: [PATCH] [rotorcraft] add dist_wp --- conf/messages.xml | 1 + sw/airborne/firmwares/rotorcraft/navigation.c | 33 +++++++++++++++---- sw/airborne/firmwares/rotorcraft/navigation.h | 5 +++ sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/tmtc/rotorcraft_server.ml | 2 +- 5 files changed, 35 insertions(+), 8 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 0d00f86e745..985f34c7eb5 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1436,6 +1436,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index a1b58a7edaa..d2f28c28d5d 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -69,6 +69,8 @@ float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE; float dist2_to_home; bool_t too_far_from_home; +float dist2_to_wp; + uint8_t horizontal_mode; struct EnuCoor_i nav_segment_start, nav_segment_end; struct EnuCoor_i nav_circle_center; @@ -106,9 +108,10 @@ static inline void nav_set_altitude( void ); static void send_nav_status(void) { float dist_home = sqrtf(dist2_to_home); + float dist_wp = sqrtf(dist2_to_wp); DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice, &block_time, &stage_time, - &dist_home, + &dist_home, &dist_wp, &nav_block, &nav_stage, &horizontal_mode); if (horizontal_mode == HORIZONTAL_MODE_ROUTE) { @@ -169,6 +172,7 @@ void nav_init(void) { too_far_from_home = FALSE; dist2_to_home = 0; + dist2_to_wp = 0; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status); @@ -214,6 +218,7 @@ void nav_run(void) { void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { if (radius == 0) { VECT2_COPY(navigation_target, *wp_center); + dist2_to_wp = get_dist2_to_point(wp_center); } else { struct Int32Vect2 pos_diff; @@ -279,6 +284,8 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) { nav_segment_start = *wp_start; nav_segment_end = *wp_end; horizontal_mode = HORIZONTAL_MODE_ROUTE; + + dist2_to_wp = get_dist2_to_point(wp_end); } bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time) { @@ -389,6 +396,8 @@ void nav_init_stage( void ) { void nav_periodic_task(void) { RunOnceEvery(16, { stage_time++; block_time++; }); + dist2_to_wp = 0; + /* from flight_plan.h */ auto_nav(); @@ -454,19 +463,31 @@ void nav_home(void) { nav_altitude = waypoints[WP_HOME].z; nav_flight_altitude = nav_altitude; + dist2_to_wp = dist2_to_home; + /* run carrot loop */ nav_run(); } +/** Returns squared horizontal distance to given point */ +float get_dist2_to_point(struct EnuCoor_i *p) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); + struct FloatVect2 pos_diff; + pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x; + pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y; + return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y; +} + +/** Returns squared horizontal distance to given waypoint */ +float get_dist2_to_waypoint(uint8_t wp_id) { + return get_dist2_to_point(&waypoints[wp_id]); +} + /** Computes squared distance to the HOME waypoint potentially sets * #too_far_from_home */ void compute_dist2_to_home(void) { - struct EnuCoor_i* pos = stateGetPositionEnu_i(); - struct Int32Vect2 home_d; - VECT2_DIFF(home_d, waypoints[WP_HOME], *pos); - INT32_VECT2_RSHIFT(home_d, home_d, INT32_POS_FRAC); - dist2_to_home = (float)(home_d.x * home_d.x + home_d.y * home_d.y); + dist2_to_home = get_dist2_to_waypoint(WP_HOME); too_far_from_home = dist2_to_home > max_dist2_from_home; } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index c37ff1d4c5f..44e55232596 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -74,6 +74,10 @@ extern float dist2_to_home; ///< squared distance to home waypoint extern bool_t too_far_from_home; extern float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode +extern float dist2_to_wp; ///< squared distance to next waypoint + +extern float get_dist2_to_waypoint(uint8_t wp_id); +extern float get_dist2_to_point(struct EnuCoor_i *p); extern void compute_dist2_to_home(void); extern void nav_home(void); @@ -121,6 +125,7 @@ extern bool_t nav_set_heading_current(void); #define NavGotoWaypoint(_wp) { \ horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \ + dist2_to_wp = get_dist2_to_waypoint(_wp); \ } /*********** Navigation on a circle **************************************/ diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index e43e8ba78c0..13c68986ebd 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1167,7 +1167,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> (* Estimated Time Arrival to next waypoint *) let d = Pprz.float_assoc "dist_to_wp" vs in let label = - if d = 0. || ac.speed = 0. then + if d < 0.5 || ac.speed < 0.5 then "N/A" else sprintf "%.0fs" (d /. ac.speed) in diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index 07c1a904dd4..1b5966bf142 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -205,7 +205,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> a.cur_block <- ivalue "cur_block"; a.cur_stage <- ivalue "cur_stage"; a.horizontal_mode <- check_index (ivalue "horizontal_mode") horiz_modes "AP_HORIZ"; - (*a.dist_to_wp <- sqrt (fvalue "dist2_wp")*) + a.dist_to_wp <- fvalue "dist_wp"; | "WP_MOVED_ENU" -> begin match a.nav_ref with