diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml index 54a5ea580a..b88dbba506 100644 --- a/conf/airframes/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/UofAdelaide/A1000_LISA.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/flight_plans/basic_booz.xml b/conf/flight_plans/basic_booz.xml index a1d396802f..38ccff991c 100644 --- a/conf/flight_plans/basic_booz.xml +++ b/conf/flight_plans/basic_booz.xml @@ -63,11 +63,15 @@ + + + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index f3f5612ddd..f59925738b 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -19,6 +19,7 @@ + diff --git a/sw/airborne/booz/booz2_autopilot.c b/sw/airborne/booz/booz2_autopilot.c index 7e84edfac1..713c54dcac 100644 --- a/sw/airborne/booz/booz2_autopilot.c +++ b/sw/airborne/booz/booz2_autopilot.c @@ -71,7 +71,7 @@ void booz2_autopilot_init(void) { void booz2_autopilot_periodic(void) { - RunOnceEvery(BOOZ2_AP_NAV_RATE, nav_periodic_task()); + RunOnceEvery(BOOZ2_NAV_PRESCALER, nav_periodic_task()); #ifdef BOOZ_FAILSAFE_GROUND_DETECT if (booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE && booz2_autopilot_detect_ground) { booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL); diff --git a/sw/airborne/booz/booz2_autopilot.h b/sw/airborne/booz/booz2_autopilot.h index 39e621770a..2123d3e858 100644 --- a/sw/airborne/booz/booz2_autopilot.h +++ b/sw/airborne/booz/booz2_autopilot.h @@ -46,7 +46,6 @@ #define BOOZ2_AP_MODE_HOVER_Z_HOLD 11 #define BOOZ2_AP_MODE_NAV 12 -#define BOOZ2_AP_NAV_RATE 32 extern uint8_t booz2_autopilot_mode; extern uint8_t booz2_autopilot_mode_auto2; diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c index c8a7d95499..cd89935b8f 100644 --- a/sw/airborne/booz/booz2_navigation.c +++ b/sw/airborne/booz/booz2_navigation.c @@ -25,6 +25,7 @@ #include "booz2_navigation.h" +#include "booz/booz2_debug.h" #include "booz_gps.h" #include "booz2_ins.h" @@ -32,10 +33,6 @@ #include "modules.h" #include "flight_plan.h" -#ifdef USE_FMS -#include "booz_fms.h" -#endif - #include "math/pprz_algebra_int.h" const uint8_t nb_waypoint = NB_WAYPOINT; @@ -317,28 +314,20 @@ void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) { } } -#ifdef USE_FMS -void nav_update_wp_from_fms(uint8_t _wp) { - if (fms.enabled && _wp < nb_waypoint) { - int32_t s_heading, c_heading; - PPRZ_ITRIG_SIN(s_heading, nav_heading); - PPRZ_ITRIG_COS(c_heading, nav_heading); - struct Int32Vect3 dpos; - dpos.x = (NavFmsMaxHSpeed * fms.input.h_sp.speed.x >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128); - dpos.y = (NavFmsMaxHSpeed * fms.input.h_sp.speed.y >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128); - dpos.z = (NavFmsMaxVSpeed * fms.input.v_sp.climb >> (INT32_SPEED_FRAC - INT32_POS_FRAC)) / (BOOZ2_AP_NAV_RATE * 128); - waypoints[_wp].x += (s_heading * dpos.x + c_heading * dpos.y) >> INT32_TRIG_FRAC; - waypoints[_wp].y += (c_heading * dpos.x - s_heading * dpos.y) >> INT32_TRIG_FRAC; - waypoints[_wp].z += dpos.z; - int32_t dheading = (NavFmsMaxHeadingRate * fms.input.h_sp.speed.z >> (INT32_RATE_FRAC - INT32_ANGLE_FRAC)) / (BOOZ2_AP_NAV_RATE * 128); - if (dheading != 0) { - nav_heading += dheading; - INT32_COURSE_NORMALIZE(nav_heading); - } - RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, &(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z))); - } +void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) { + MY_ASSERT(_wp < nb_waypoint); + int32_t s_heading, c_heading; + PPRZ_ITRIG_SIN(s_heading, nav_heading); + PPRZ_ITRIG_COS(c_heading, nav_heading); + struct Int32Vect3 delta_pos; + VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the division is really a >> */ + waypoints[_wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[_wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[_wp].z += delta_pos.z; + nav_heading += heading_rate_sp / BOOZ2_NAV_FREQ; + INT32_COURSE_NORMALIZE(nav_heading); + RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &_wp, &(waypoints[_wp].x), &(waypoints[_wp].y), &(waypoints[_wp].z))); } -#endif /* USE_FMS */ bool_t nav_detect_ground(void) { if (!booz2_autopilot_detect_ground) return FALSE; diff --git a/sw/airborne/booz/booz2_navigation.h b/sw/airborne/booz/booz2_navigation.h index eb60519349..44b66042ec 100644 --- a/sw/airborne/booz/booz2_navigation.h +++ b/sw/airborne/booz/booz2_navigation.h @@ -28,6 +28,9 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_geodetic_float.h" +#define BOOZ2_NAV_FREQ 16 +#define BOOZ2_NAV_PRESCALER (PERIODIC_FREQ/BOOZ2_NAV_FREQ) + extern struct EnuCoor_i booz2_navigation_target; extern struct EnuCoor_i booz2_navigation_carrot; @@ -212,15 +215,7 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx); #define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y) #define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt)) -#ifdef USE_FMS -#define NavFmsMaxHSpeed ((int32_t)SPEED_BFP_OF_REAL(4.)) -#define NavFmsMaxVSpeed ((int32_t)SPEED_BFP_OF_REAL(2.)) -#define NavFmsMaxHeadingRate ((int32_t)RATE_BFP_OF_REAL(RadOfDeg(60.))) -extern void nav_update_wp_from_fms(uint8_t _wp); -#define NavUpdateWPFromFms(_wp) { \ - nav_update_wp_from_fms(_wp); \ -} -#endif /* USE_FMS */ +extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ); #endif /* BOOZ2_NAVIGATION_H */