diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index a4fd061a316..1f591dce319 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -26,7 +26,7 @@ */ #include "firmwares/fixedwing/guidance/guidance_v.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -207,7 +207,7 @@ void v_ctl_altitude_loop( void ) { } #endif - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction; BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb); @@ -253,7 +253,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { static float last_err; float f_throttle = 0; - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; float controlled_throttle = v_ctl_auto_throttle_cruise_throttle @@ -326,14 +326,14 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; // Pitch control (input: rate of climb error, output: pitch setpoint) - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain * (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); // Ground speed control loop (input: groundspeed error, output: airspeed controlled) - float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod); + float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f()); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; @@ -345,7 +345,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { } // Airspeed control loop (input: airspeed controlled, output: throttle controlled) - float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed); + float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f()); v_ctl_auto_airspeed_sum_err += err_airspeed; BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR); controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain; @@ -367,7 +367,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { */ #ifdef V_CTL_AUTO_PITCH_PGAIN inline static void v_ctl_climb_auto_pitch_loop(void) { - float err = estimator_z_dot - v_ctl_climb_setpoint; + float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_throttle_setpoint = nav_throttle_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index d0469b53e42..5edff3ad84c 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -27,7 +27,7 @@ #include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/guidance/guidance_v_n.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -185,7 +185,7 @@ void v_ctl_altitude_loop( void ) { //static float last_lead_input = 0.; // Altitude error - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb; // Lead controller @@ -209,7 +209,7 @@ static inline void v_ctl_set_pitch ( void ) { v_ctl_auto_pitch_sum_err = 0; // Compute errors - float err = v_ctl_climb_setpoint - estimator_z_dot; + float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err = err - last_err; last_err = err; @@ -234,7 +234,7 @@ static inline void v_ctl_set_throttle( void ) { v_ctl_auto_throttle_sum_err = 0; // Compute errors - float err = v_ctl_climb_setpoint - estimator_z_dot; + float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err = err - last_err; last_err = err; @@ -264,7 +264,7 @@ static inline void v_ctl_set_airspeed( void ) { Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX); // Compute errors - float err_vz = v_ctl_climb_setpoint - estimator_z_dot; + float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD; last_err_vz = err_vz; if (v_ctl_auto_throttle_igain > 0.) { @@ -276,7 +276,7 @@ static inline void v_ctl_set_airspeed( void ) { BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); } - float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed; + float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f(); float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD; last_err_as = err_airspeed; if (v_ctl_auto_airspeed_throttle_igain > 0.) { @@ -321,7 +321,7 @@ static inline void v_ctl_set_airspeed( void ) { static inline void v_ctl_set_groundspeed( void ) { // Ground speed control loop (input: groundspeed error, output: airspeed controlled) - float err_groundspeed = v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod; + float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f(); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index cf9a09ff906..502d51ccd94 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -32,7 +32,7 @@ #include "led.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/guidance/guidance_v.h" @@ -205,14 +205,14 @@ void h_ctl_course_loop ( void ) { static float last_err; // Ground path error - float err = h_ctl_course_setpoint - estimator_hspeed_dir; + float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(err); float d_err = err - last_err; last_err = err; NormRadAngle(d_err); - float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank @@ -227,7 +227,7 @@ static inline void compute_airspeed_ratio( void ) { if (use_airspeed_ratio) { // low pass airspeed static float airspeed = 0.; - airspeed = ( 15*airspeed + estimator_airspeed ) / 16; + airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16; // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); @@ -289,13 +289,14 @@ inline static void h_ctl_roll_loop( void ) { #endif // Compute errors - float err = h_ctl_ref_roll_angle - estimator_phi; + float err = h_ctl_ref_roll_angle - stateGetNedToBodyEulers_f()->phi; + struct FloatRates* body_rate = stateGetBodyRates_f(); #ifdef SITL static float last_err = 0; - estimator_p = (err - last_err)/(1/60.); + body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim last_err = err; #endif - float d_err = h_ctl_ref_roll_rate - estimator_p; + float d_err = h_ctl_ref_roll_rate - body_rate->p; if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_roll_sum_err = 0.; @@ -329,7 +330,7 @@ inline static void loiter(void) { float pitch_trim; #if USE_AIRSPEED - if (estimator_airspeed > NOMINAL_AIRSPEED) { + if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) { pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); } else { pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); @@ -359,7 +360,7 @@ inline static void h_ctl_pitch_loop( void ) { if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); + h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi); #if USE_PITCH_TRIM loiter(); #endif @@ -386,9 +387,9 @@ inline static void h_ctl_pitch_loop( void ) { #endif // Compute errors - float err = h_ctl_ref_pitch_angle - estimator_theta; + float err = h_ctl_ref_pitch_angle - stateGetNedToBodyEulers_f()->theta; #if USE_GYRO_PITCH_RATE - float d_err = h_ctl_ref_pitch_rate - estimator_q; + float d_err = h_ctl_ref_pitch_rate - stateGetBodyRates_f()->q; #else // soft derivation float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 313ed6cc953..d8739b8f946 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -31,7 +31,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "std.h" #include "led.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/guidance/guidance_v.h" @@ -181,17 +181,17 @@ void h_ctl_course_loop ( void ) { static float last_err; // Ground path error - float err = estimator_hspeed_dir - h_ctl_course_setpoint; + float err = *stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint; NormRadAngle(err); #ifdef STRONG_WIND // Usefull path speed const float reference_advance = (NOMINAL_AIRSPEED / 2.); - float advance = cos(err) * estimator_hspeed_mod / reference_advance; + float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance; if ( (advance < 1.) && // Path speed is small - (estimator_hspeed_mod < reference_advance) // Small path speed is due to wind (small groundspeed) + ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed) ) { /* @@ -210,7 +210,7 @@ void h_ctl_course_loop ( void ) { */ // Heading error - float herr = estimator_psi - h_ctl_course_setpoint; //+crab); + float herr = *stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab); NormRadAngle(herr); if (advance < -0.5) // 90 degree turn @@ -268,7 +268,7 @@ void h_ctl_course_loop ( void ) { } #endif - float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain); @@ -316,14 +316,15 @@ void h_ctl_attitude_loop ( void ) { #ifdef H_CTL_ROLL_ATTITUDE_GAIN inline static void h_ctl_roll_loop( void ) { - float err = estimator_phi - h_ctl_roll_setpoint; + float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; + struct FloatRates* body_rate = stateGetBodyRates_f(); #ifdef SITL static float last_err = 0; - estimator_p = (err - last_err)/(1/60.); + body_rate->p = (err - last_err)/(1/60.); last_err = err; #endif float cmd = h_ctl_roll_attitude_gain * err - + h_ctl_roll_rate_gain * estimator_p + + h_ctl_roll_rate_gain * body_rate->p + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -333,7 +334,7 @@ inline static void h_ctl_roll_loop( void ) { /** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */ inline static void h_ctl_roll_loop( void ) { - float err = estimator_phi - h_ctl_roll_setpoint; + float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; float cmd = h_ctl_roll_pgain * err + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); @@ -357,7 +358,7 @@ inline static void h_ctl_roll_loop( void ) { #ifdef H_CTL_RATE_LOOP static inline void h_ctl_roll_rate_loop() { - float err = estimator_p - h_ctl_roll_rate_setpoint; + float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint; /* I term calculation */ static float roll_rate_sum_err = 0.; @@ -419,28 +420,29 @@ inline static float loiter(void) { inline static void h_ctl_pitch_loop( void ) { static float last_err; + struct FloatEulers* att = stateGetNedToBodyEulers_f(); /* sanity check */ if (h_ctl_elevator_of_roll <0.) h_ctl_elevator_of_roll = 0.; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); + h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi); float err = 0; #ifdef USE_AOA switch (h_ctl_pitch_mode){ case H_CTL_PITCH_MODE_THETA: - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; break; case H_CTL_PITCH_MODE_AOA: - err = estimator_AOA - h_ctl_pitch_loop_setpoint; + err = estimator_AOA - h_ctl_pitch_loop_setpoint; //TODO AOA into state interface break; default: - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; break; } #else //NO_AOA - err = estimator_theta - h_ctl_pitch_loop_setpoint; + err = att->theta - h_ctl_pitch_loop_setpoint; #endif diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 9ac2d1c8f2c..5d1465b41a4 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -32,7 +32,6 @@ #include "subsystems/nav.h" #include "subsystems/gps.h" -#include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "firmwares/fixedwing/autopilot.h" @@ -90,7 +89,8 @@ bool_t nav_survey_active; int nav_mode; void nav_init_stage( void ) { - last_x = estimator_x; last_y = estimator_y; + last_x = stateGetPositionEnu_f()->x; + last_y = stateGetPositionEnu_f()->y; stage_time = 0; nav_circle_radians = 0; nav_in_circle = FALSE; @@ -107,8 +107,9 @@ void nav_init_stage( void ) { /** Navigates around (x, y). Clockwise iff radius > 0 */ void nav_circle_XY(float x, float y, float radius) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; - nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x); + nav_circle_trigo_qdr = atan2(pos->y - y, pos->x - x); if (nav_in_circle) { float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr; @@ -116,7 +117,7 @@ void nav_circle_XY(float x, float y, float radius) { nav_circle_radians += trigo_diff; } - float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y); + float dist2_center = DistanceSquare(pos->x, pos->y, x, y); float dist_carrot = CARROT*NOMINAL_AIRSPEED; float sign_radius = radius > 0 ? 1 : -1; @@ -129,7 +130,7 @@ void nav_circle_XY(float x, float y, float radius) { (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : - atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius)); + atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI/4); @@ -152,7 +153,7 @@ void nav_circle_XY(float x, float y, float radius) { float start_alt = waypoints[_last_wp].a; \ float diff_alt = waypoints[_wp].a - start_alt; \ float alt = start_alt + nav_leg_progress * diff_alt; \ - float pre_climb = estimator_hspeed_mod * diff_alt / nav_leg_length; \ + float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \ NavVerticalAltitudeMode(alt, pre_climb); \ } @@ -203,7 +204,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height); static void nav_ground_speed_loop( void ) { if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) { - float err = nav_ground_speed_setpoint - estimator_hspeed_mod; + float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f()); v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err; Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); } else { @@ -273,7 +274,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) { float y = ac->north - _distance*sa; fly_to_xy(x, y); #ifdef NAV_FOLLOW_PGAIN - float s = (estimator_x-x)*ca+(estimator_y-y)*sa; + float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa; nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s; nav_ground_speed_loop(); #endif @@ -294,12 +295,12 @@ float fp_pitch; /* deg */ */ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) { /** distance to waypoint in x */ - float pw_x = x - estimator_x; + float pw_x = x - stateGetPositionEnu_f()->x; /** distance to waypoint in y */ - float pw_y = y - estimator_y; + float pw_y = y - stateGetPositionEnu_f()->y; dist2_to_wp = pw_x*pw_x + pw_y *pw_y; - float min_dist = approaching_time * estimator_hspeed_mod; + float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); if (dist2_to_wp < min_dist*min_dist) return TRUE; @@ -314,19 +315,21 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap */ //static inline void fly_to_xy(float x, float y) { void fly_to_xy(float x, float y) { + struct EnuCoor_f* pos = stateGetPositionEnu_f(); desired_x = x; desired_y = y; if (nav_mode == NAV_MODE_COURSE) { - h_ctl_course_setpoint = atan2(x - estimator_x, y - estimator_y); + h_ctl_course_setpoint = atan2(x - pos->x, y - pos->y); if (h_ctl_course_setpoint < 0.) h_ctl_course_setpoint += 2 * M_PI; lateral_mode = LATERAL_MODE_COURSE; } else { - float diff = atan2(x - estimator_x, y - estimator_y) - estimator_hspeed_dir; + float diff = atan2(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(diff); BoundAbs(diff,M_PI/2.); float s = sin(diff); - h_ctl_roll_setpoint = atan(2 * estimator_hspeed_mod*estimator_hspeed_mod * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); + float speed = *stateGetHorizontalSpeedNorm_f(); + h_ctl_roll_setpoint = atan(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); lateral_mode = LATERAL_MODE_ROLL; } @@ -339,7 +342,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { float leg_x = wp_x - last_wp_x; float leg_y = wp_y - last_wp_y; float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); - nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2; + nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2; nav_leg_length = sqrt(leg2); /** distance of carrot (in meter) */ diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 47d96250280..2a846490141 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -34,6 +34,7 @@ #include "std.h" #include "paparazzi.h" +#include "state.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "subsystems/navigation/nav_survey_rectangle.h" #include "subsystems/navigation/common_flight_plan.h" @@ -129,7 +130,7 @@ extern void nav_circle_XY(float x, float y, float radius); /** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/ #define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr()) -#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(estimator_hspeed_dir)) +#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f())) /*********** Navigation along a line *************************************/ extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y); diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index c2609a9a1c2..34127ee69bd 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -23,7 +23,6 @@ */ #include "subsystems/navigation/common_nav.h" -#include "estimator.h" #include "generated/flight_plan.h" #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" @@ -47,12 +46,13 @@ float max_dist_from_home = MAX_DIST_FROM_HOME; * \a too_far_from_home */ void compute_dist2_to_home(void) { - float ph_x = waypoints[WP_HOME].x - estimator_x; - float ph_y = waypoints[WP_HOME].y - estimator_y; + struct EnuCoor_f* pos = stateGetPositionEnu_f(); + float ph_x = waypoints[WP_HOME].x - pos->x; + float ph_y = waypoints[WP_HOME].y - pos->y; dist2_to_home = ph_x*ph_x + ph_y *ph_y; too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME); #if defined InAirspace - too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y)); + too_far_from_home = too_far_from_home || !(InAirspace(pos_x, pos_y)); #endif } @@ -80,6 +80,10 @@ unit_t nav_reset_reference( void ) { nav_utm_north0 = gps.utm_pos.north/100; #endif + // reset state UTM ref + struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; + stateSetLocalUtmOrigin_f(&utm0); + previous_ground_alt = ground_alt; ground_alt = gps.hmsl/1000.; return 0; diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index d97e5c27bbb..25bf528ba0c 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -26,6 +26,7 @@ #define COMMON_NAV_H #include "std.h" +#include "state.h" #include "subsystems/navigation/common_flight_plan.h" extern float max_dist_from_home; @@ -66,8 +67,8 @@ void common_nav_periodic_task_4Hz(void); #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ - waypoints[_wp].x = estimator_x; \ - waypoints[_wp].y = estimator_y; \ + waypoints[_wp].x = stateGetPositionEnu_f()->x; \ + waypoints[_wp].y = stateGetPositionEnu_f()->y; \ FALSE; \ })