diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.c b/sw/airborne/modules/nav/nav_rotorcraft_base.c index 90b6d23729..949c14cebb 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_base.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_base.c @@ -26,6 +26,7 @@ #include "modules/nav/nav_rotorcraft_base.h" #include "firmwares/rotorcraft/navigation.h" +#include "generated/flight_plan.h" struct NavBase_t nav_rotorcraft_base; @@ -35,7 +36,6 @@ static void nav_stage_init(void) { nav_rotorcraft_base.circle.radians = 0.f; nav_rotorcraft_base.goto_wp.leg_progress = 0.f; - nav_rotorcraft_base.oval.count = 0; } static void nav_goto(struct EnuCoor_f *wp) diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.h b/sw/airborne/modules/nav/nav_rotorcraft_base.h index 85ed7cfbe6..6d44ae8cee 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_base.h +++ b/sw/airborne/modules/nav/nav_rotorcraft_base.h @@ -36,10 +36,11 @@ extern struct NavBase_t nav_rotorcraft_base; extern void nav_rotorcraft_init(void); -/** Macros for circle nav +/** Macros for circle and oval nav */ #define NavCircleCount() nav_circle_get_count(&nav_rotorcraft_base.circle) #define NavCircleQdr() nav_circle_qdr(&nav_rotorcraft_base.circle) +#define NavOvalCount nav_rotorcraft_base.oval.count /** True if x (in degrees) is close to the current QDR (less than 10 degrees) */ diff --git a/sw/airborne/modules/nav/nav_survey_hybrid.c b/sw/airborne/modules/nav/nav_survey_hybrid.c index 58df49cd10..cc9485f4b5 100644 --- a/sw/airborne/modules/nav/nav_survey_hybrid.c +++ b/sw/airborne/modules/nav/nav_survey_hybrid.c @@ -98,7 +98,7 @@ struct SurveyHybridPrivate { struct SurveyHybrid survey_hybrid; static struct SurveyHybridPrivate survey_private; -static void nav_survey_hybrid_setup(float orientation, float sweep, float radius); +static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height); static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans); static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans); @@ -134,7 +134,7 @@ static bool nav_survey_hybrid_mission_local(uint8_t nb, float *params, enum Miss survey_private.corners[i].y = params[5+2*i+1]; survey_private.corners[i].z = height; } - nav_survey_hybrid_setup(orientation, sweep, radius); + nav_survey_hybrid_setup(orientation, sweep, radius, height); return nav_survey_hybrid_run(); } } @@ -164,7 +164,7 @@ static bool nav_survey_hybrid_mission_global(uint8_t nb, float *params, enum Mis enu_of_lla_point_f(&corner, &state.ned_origin_f, &lla); survey_private.corners[i] = corner; } - nav_survey_hybrid_setup(orientation, sweep, radius); + nav_survey_hybrid_setup(orientation, sweep, radius, height); return nav_survey_hybrid_run(); } } @@ -194,7 +194,7 @@ void nav_survey_hybrid_init(void) /** finish preparation of survey based on private structure */ -static void nav_survey_hybrid_setup(float orientation, float sweep, float radius) +static void nav_survey_hybrid_setup(float orientation, float sweep, float radius, float height) { FLOAT_VECT2_ZERO(survey_private.smallest_corner); int i = 0; @@ -347,7 +347,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius // Find the entry point survey_private.entry.x = survey_private.from_wp.x; survey_private.entry.y = survey_private.corners[0].y + entry_distance; - survey_private.entry.z = survey_private.corners[0].z; + survey_private.entry.z = height; // Go into entry state survey_private.status = Entry; @@ -357,7 +357,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius survey_private.valid = true; } -void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius) +void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height) { survey_private.valid = false; if (size < 3 || size > SURVEY_HYBRID_MAX_POLYGON_SIZE) { @@ -372,10 +372,10 @@ void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, ui } survey_private.size = size; - nav_survey_hybrid_setup(orientation, sweep, radius); + nav_survey_hybrid_setup(orientation, sweep, radius, height); } -void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius) +void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height) { survey_private.valid = false; struct EnuCoor_f *start = waypoint_get_enu_f(start_wp); @@ -387,7 +387,7 @@ void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_ float dx = second->x - start->x; float dy = second->y - start->y; float angle = DegOfRad(atan2f(dy, dx)); - nav_survey_hybrid_setup_orientation(start_wp, angle, size, sweep, radius); + nav_survey_hybrid_setup_orientation(start_wp, angle, size, sweep, radius, height); } //========================================================================================================================= diff --git a/sw/airborne/modules/nav/nav_survey_hybrid.h b/sw/airborne/modules/nav/nav_survey_hybrid.h index a11452ede6..de14bc537d 100644 --- a/sw/airborne/modules/nav/nav_survey_hybrid.h +++ b/sw/airborne/modules/nav/nav_survey_hybrid.h @@ -50,8 +50,9 @@ extern void nav_survey_hybrid_init(void); * @param size number of waypoints/corners used to define the polygon * @param sweep distance between scan lines * @param radius turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius) + * @param height starting height in meters */ -extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius); +extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height); /** * Setup "dynamic" polygon survey with sweep orientation towards a waypoint. @@ -60,9 +61,10 @@ extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientat * @param second_wp second waypoint towards which the sweep orientation is computed * @param size number of waypoints/corners used to define the polygon * @param sweep distance between scan lines, if zero uses Poly_Distance - * @param radius turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius) + * @param radius turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius) + * @param height starting height in meters */ -extern void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius); +extern void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height); /** Run polygon hybrid survey */ extern bool nav_survey_hybrid_run(void); diff --git a/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c b/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c index 059b8b484a..2799f1de4a 100644 --- a/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c +++ b/sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c @@ -154,6 +154,7 @@ static bool nav_takeoff_run(void) { break; case NAV_TAKEOFF_CLIMB: // call vertical climb from nav/guidance + autopilot_set_in_flight(true); NavGotoWaypoint(takeoff.climb_id); NavVerticalClimbMode(NAV_TAKEOFF_CLIMB_SPEED); if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > NAV_TAKEOFF_HEIGHT) {