diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index bb9a6612f948c5..8e206845f2f973 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -28,7 +28,7 @@ bool ModeCircle::init(bool ignore_checks) if (copter.circle_nav->roi_at_center()) { const Vector3p &pos { copter.circle_nav->get_center() }; Location circle_center; - if (!AP::ahrs().get_location_from_origin_offset(circle_center, pos * 0.01)) { + if (!AP::ahrs().get_location_from_origin_offset_NED(circle_center, pos * 0.01)) { return false; } // point at the ground: diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 8ea52985fd17ec..660c94eef170ce 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -243,10 +243,10 @@ void ModeGuided::pos_control_start() pva_control_start(); } -// initialise guided mode's velocity controller +// initialise guided mode's acceleration controller void ModeGuided::accel_control_start() { - // set guided_mode to velocity controller + // set guided_mode to acceleration controller guided_mode = SubMode::Accel; // initialise position controller @@ -256,7 +256,7 @@ void ModeGuided::accel_control_start() // initialise guided mode's velocity and acceleration controller void ModeGuided::velaccel_control_start() { - // set guided_mode to velocity controller + // set guided_mode to velocity and acceleration controller guided_mode = SubMode::VelAccel; // initialise position controller @@ -266,7 +266,7 @@ void ModeGuided::velaccel_control_start() // initialise guided mode's position, velocity and acceleration controller void ModeGuided::posvelaccel_control_start() { - // set guided_mode to velocity controller + // set guided_mode to position, velocity and acceleration controller guided_mode = SubMode::PosVelAccel; // initialise position controller @@ -414,11 +414,14 @@ bool ModeGuided::get_wp(Location& destination) const case SubMode::Pos: destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); return true; - default: - return false; + case SubMode::Angle: + case SubMode::TakeOff: + case SubMode::Accel: + case SubMode::VelAccel: + case SubMode::PosVelAccel: + break; } - // should never get here but just in case return false; } @@ -515,7 +518,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // set_velaccel - sets guided mode's target velocity and acceleration void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { - // check we are in velocity control mode + // check we are in acceleration control mode if (guided_mode != SubMode::Accel) { accel_control_start(); } @@ -547,7 +550,7 @@ void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_ // set_velaccel - sets guided mode's target velocity and acceleration void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { - // check we are in velocity control mode + // check we are in velocity and acceleration control mode if (guided_mode != SubMode::VelAccel) { velaccel_control_start(); } @@ -589,7 +592,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const } #endif - // check we are in velocity control mode + // check we are in position, velocity and acceleration control mode if (guided_mode != SubMode::PosVelAccel) { posvelaccel_control_start(); } diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 99fd471dccee4d..97bee976dfd283 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -24,7 +24,7 @@ void Rover::Log_Write_Attitude() } // log heel to sail control for sailboats - if (rover.g2.sailboat.sail_enabled()) { + if (g2.sailboat.sail_enabled()) { logger.Write_PID(LOG_PIDR_MSG, g2.attitude_control.get_sailboat_heel_pid().get_pid_info()); } } @@ -142,13 +142,13 @@ void Rover::Log_Write_Nav_Tuning() void Rover::Log_Write_Sail() { // only log sail if present - if (!rover.g2.sailboat.sail_enabled()) { + if (!g2.sailboat.sail_enabled()) { return; } float wind_dir_tack = logger.quiet_nanf(); uint8_t current_tack = 0; - if (rover.g2.windvane.enabled()) { + if (g2.windvane.enabled()) { wind_dir_tack = degrees(g2.windvane.get_tack_threshold_wind_dir_rad()); current_tack = uint8_t(g2.windvane.get_current_tack()); } diff --git a/Rover/mode.cpp b/Rover/mode.cpp index f4bd7dfc309298..866b7a5397ce82 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -10,7 +10,7 @@ Mode::Mode() : channel_roll(rover.channel_roll), channel_pitch(rover.channel_pitch), channel_walking_height(rover.channel_walking_height), - attitude_control(rover.g2.attitude_control) + attitude_control(g2.attitude_control) { } void Mode::exit() @@ -47,7 +47,7 @@ bool Mode::enter() set_reversed(false); // clear sailboat tacking flags - rover.g2.sailboat.clear_tack(); + g2.sailboat.clear_tack(); } return ret; @@ -66,7 +66,7 @@ void Mode::get_pilot_input(float &steering_out, float &throttle_out) const } // apply RC skid steer mixing - switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get()) + switch ((enum pilot_steer_type_t)g.pilot_steer_type.get()) { case PILOT_STEER_TYPE_DEFAULT: case PILOT_STEER_TYPE_DIR_REVERSED_WHEN_REVERSING: @@ -166,7 +166,7 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_ float desired_throttle = constrain_float(rover.channel_throttle->norm_input_dz(), -1.0f, 1.0f); // handle two paddle input - if ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) { + if ((enum pilot_steer_type_t)g.pilot_steer_type.get() == PILOT_STEER_TYPE_TWO_PADDLES) { const float left_paddle = desired_steering; const float right_paddle = desired_throttle; desired_steering = (left_paddle - right_paddle) * 0.5f; @@ -279,7 +279,7 @@ void Mode::handle_tack_request() { // autopilot modes handle tacking if (is_autopilot_mode()) { - rover.g2.sailboat.handle_tack_request_auto(); + g2.sailboat.handle_tack_request_auto(); } } @@ -304,9 +304,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled) // call throttle controller and convert output to -100 to +100 range float throttle_out = 0.0f; - if (rover.g2.sailboat.sail_enabled()) { + if (g2.sailboat.sail_enabled()) { // sailboats use special throttle and mainsail controller - rover.g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out); + g2.sailboat.get_throttle_and_set_mainsail(target_speed, throttle_out); } else { // call speed or stop controller if (is_zero(target_speed) && !rover.is_balancebot()) { diff --git a/Rover/mode_acro.cpp b/Rover/mode_acro.cpp index d59f208f0aafc5..1cad71e702b11f 100644 --- a/Rover/mode_acro.cpp +++ b/Rover/mode_acro.cpp @@ -28,12 +28,12 @@ void ModeAcro::update() // handle sailboats if (!is_zero(desired_steering)) { // steering input return control to user - rover.g2.sailboat.clear_tack(); + g2.sailboat.clear_tack(); } - if (rover.g2.sailboat.tacking()) { + if (g2.sailboat.tacking()) { // call heading controller during tacking - steering_out = attitude_control.get_steering_out_heading(rover.g2.sailboat.get_tack_heading_rad(), + steering_out = attitude_control.get_steering_out_heading(g2.sailboat.get_tack_heading_rad(), g2.wp_nav.get_pivot_rate(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, @@ -60,5 +60,5 @@ bool ModeAcro::requires_velocity() const // sailboats in acro mode support user manually initiating tacking from transmitter void ModeAcro::handle_tack_request() { - rover.g2.sailboat.handle_tack_request_acro(); + g2.sailboat.handle_tack_request_acro(); } diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index dd99f363a15198..387349dfda95b0 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -141,7 +141,7 @@ void ModeAuto::update() break; case SubMode::Circle: - rover.g2.mode_circle.update(); + g2.mode_circle.update(); break; } } @@ -173,7 +173,7 @@ float ModeAuto::wp_bearing() const case SubMode::NavScriptTime: return rover.mode_guided.wp_bearing(); case SubMode::Circle: - return rover.g2.mode_circle.wp_bearing(); + return g2.mode_circle.wp_bearing(); } // this line should never be reached @@ -197,7 +197,7 @@ float ModeAuto::nav_bearing() const case SubMode::NavScriptTime: return rover.mode_guided.nav_bearing(); case SubMode::Circle: - return rover.g2.mode_circle.nav_bearing(); + return g2.mode_circle.nav_bearing(); } // this line should never be reached @@ -221,7 +221,7 @@ float ModeAuto::crosstrack_error() const case SubMode::NavScriptTime: return rover.mode_guided.crosstrack_error(); case SubMode::Circle: - return rover.g2.mode_circle.crosstrack_error(); + return g2.mode_circle.crosstrack_error(); } // this line should never be reached @@ -245,7 +245,7 @@ float ModeAuto::get_desired_lat_accel() const case SubMode::NavScriptTime: return rover.mode_guided.get_desired_lat_accel(); case SubMode::Circle: - return rover.g2.mode_circle.get_desired_lat_accel(); + return g2.mode_circle.get_desired_lat_accel(); } // this line should never be reached @@ -270,7 +270,7 @@ float ModeAuto::get_distance_to_destination() const case SubMode::NavScriptTime: return rover.mode_guided.get_distance_to_destination(); case SubMode::Circle: - return rover.g2.mode_circle.get_distance_to_destination(); + return g2.mode_circle.get_distance_to_destination(); } // this line should never be reached @@ -299,7 +299,7 @@ bool ModeAuto::get_desired_location(Location& destination) const case SubMode::NavScriptTime: return rover.mode_guided.get_desired_location(destination); case SubMode::Circle: - return rover.g2.mode_circle.get_desired_location(destination); + return g2.mode_circle.get_desired_location(destination); } // we should never reach here but just in case @@ -341,7 +341,7 @@ bool ModeAuto::reached_destination() const case SubMode::NavScriptTime: return rover.mode_guided.reached_destination(); case SubMode::Circle: - return rover.g2.mode_circle.reached_destination(); + return g2.mode_circle.reached_destination(); } // we should never reach here but just in case, return true to allow missions to continue @@ -366,7 +366,7 @@ bool ModeAuto::set_desired_speed(float speed) case SubMode::NavScriptTime: return rover.mode_guided.set_desired_speed(speed); case SubMode::Circle: - return rover.g2.mode_circle.set_desired_speed(speed); + return g2.mode_circle.set_desired_speed(speed); } return false; } @@ -902,7 +902,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) // if a location target was set, return true once vehicle is close if (guided_target.valid) { - if (rover.current_loc.get_distance(guided_target.loc) <= rover.g2.wp_nav.get_radius()) { + if (rover.current_loc.get_distance(guided_target.loc) <= g2.wp_nav.get_radius()) { return true; } } diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp index 86ba7dbdae1193..06109952481e3e 100644 --- a/Rover/mode_circle.cpp +++ b/Rover/mode_circle.cpp @@ -149,7 +149,7 @@ void ModeCircle::update() _distance_to_destination = center_to_veh.length(); dist_to_edge_m = fabsf(_distance_to_destination - config.radius); if (!reached_edge) { - const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST); + const float dist_thresh_m = MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST); reached_edge = dist_to_edge_m <= dist_thresh_m; } @@ -266,8 +266,8 @@ void ModeCircle::check_config_speed() void ModeCircle::check_config_radius() { // ensure radius is at least as large as vehicle's turn radius - if (config.radius < rover.g2.turn_radius) { - config.radius = rover.g2.turn_radius; - gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)rover.g2.turn_radius); + if (config.radius < g2.turn_radius) { + config.radius = g2.turn_radius; + gcs().send_text(MAV_SEVERITY_WARNING, "Circle: radius increased to TURN_RADIUS (%4.1f)", (double)g2.turn_radius); } } diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 80f0a5fa4477bb..338a734ff99eca 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -443,5 +443,5 @@ bool ModeGuided::limit_breached() const // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) bool ModeGuided::use_scurves_for_navigation() const { - return ((rover.g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0); + return ((g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0); } diff --git a/Rover/mode_loiter.cpp b/Rover/mode_loiter.cpp index 918144c9fa444d..b208c595b14ddc 100644 --- a/Rover/mode_loiter.cpp +++ b/Rover/mode_loiter.cpp @@ -23,16 +23,16 @@ void ModeLoiter::update() // get distance (in meters) to destination _distance_to_destination = rover.current_loc.get_distance(_destination); - const float loiter_radius = rover.g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius; + const float loiter_radius = g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius; // if within loiter radius slew desired speed towards zero and use existing desired heading if (_distance_to_destination <= loiter_radius) { // sailboats should not stop unless motoring - const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f; + const float desired_speed_within_radius = g2.sailboat.tack_enabled() ? 0.1f : 0.0f; _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt); // if we have a sail but not trying to use it then point into the wind - if (!rover.g2.sailboat.tack_enabled() && rover.g2.sailboat.sail_enabled()) { + if (!g2.sailboat.tack_enabled() && g2.sailboat.sail_enabled()) { _desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f; } } else { diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index f16c4b20891685..186d9f10311642 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -103,6 +103,8 @@ AP_HW_JDMINIF405 144 AP_HW_KAKUTEF7_MINI 145 AP_HW_H757I_EVAL 146 AP_HW_F4BY 20 # value due to previous release by vendor +AP_HW_MAZZYSTARDRONE 188 + AP_HW_VRBRAIN_V51 1151 AP_HW_VRBRAIN_V52 1152 AP_HW_VRBRAIN_V54 1154 @@ -111,7 +113,7 @@ AP_HW_VRUBRAIN_V51 1351 AP_HW_F103_PERIPH 1000 AP_HW_CUAV_GPS 1001 AP_HW_OMNIBUSF4 1002 -AP_HW_CUBEBLACK+ 1003 +AP_HW_CUBEBLACKPLUS 1003 AP_HW_F303_PERIPH 1004 AP_HW_ZUBAXGNSS 1005 AP_HW_NIGHTCRAWLER 1006 @@ -200,7 +202,7 @@ AP_HW_HolybroCompass 1088 AP_HW_FOXEERH743_V1 1089 AP_HW_PixFlamingoL4R5_V1 1090 -AP_HW_Sierra-TrueNav Pro 1091 +AP_HW_Sierra-TrueNavPro 1091 AP_HW_Sierra-TrueNav 1092 AP_HW_Sierra-TrueNorth 1093 AP_HW_Sierra-TrueSpeed 1094 @@ -287,6 +289,8 @@ AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 +AP_HW_KHA_ETH 1315 + AP_HW_CUBEORANGE_PERIPH 1400 AP_HW_CUBEBLACK_PERIPH 1401 AP_HW_PIXRACER_PERIPH 1402 diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 5f5ce5434b121b..c8d1bbbc42b2e0 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -187,7 +187,7 @@ def srcpath(path): cfg.env.prepend_value('INCLUDES', [ cfg.srcnode.find_dir('libraries/AP_Common/missing').abspath() ]) - if os.path.exists(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json')): + if os.path.exists(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json')) and 'AP_NO_COMPILE_COMMANDS' not in os.environ: # change c_cpp_properties.json configure the VSCode Intellisense env c_cpp_properties = json.load(open(os.path.join(env.SRCROOT, '.vscode/c_cpp_properties.json'))) for config in c_cpp_properties['configurations']: @@ -944,6 +944,19 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_GPS = 1, ) +class sitl_periph_battmon(sitl_periph): + def configure_env(self, cfg, env): + cfg.env.AP_PERIPH = 1 + super(sitl_periph_battmon, self).configure_env(cfg, env) + env.DEFINES.update( + HAL_BUILD_AP_PERIPH = 1, + PERIPH_FW = 1, + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_battmon"', + APJ_BOARD_ID = 101, + + HAL_PERIPH_ENABLE_BATTERY = 1, + ) + class esp32(Board): abstract = True toolchain = 'xtensa-esp32-elf' diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index fd9b8fa666be59..36c705e41464a9 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1922,6 +1922,22 @@ def GPSGlitchAuto(self, timeout=180): self.show_gps_and_sim_positions(False) raise e + # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode + self.change_mode("LOITER") + self.set_parameters({ + "SIM_GPS_DISABLE": 1, + }) + self.delay_sim_time(2) + self.set_parameters({ + "SIM_GPS_DISABLE": 0, + }) + # regaining GPS should not result in it falling back to a non-navigation mode + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + # It should still be navigating after enougnh time has passed for any pending timeouts to activate. + self.delay_sim_time(10) + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1) + self.change_mode("AUTO") + # record time and position tstart = self.get_sim_time() @@ -2585,16 +2601,39 @@ def AutoTuneSwitch(self): "RC8_OPTION": 17, "ATC_RAT_RLL_FLTT": 20, }) - rlld = self.get_parameter("ATC_RAT_RLL_D") - rlli = self.get_parameter("ATC_RAT_RLL_I") - rllp = self.get_parameter("ATC_RAT_RLL_P") - rllt = self.get_parameter("ATC_RAT_RLL_FLTT") - self.progress("AUTOTUNE pre-gains are P:%f I:%f D:%f" % - (self.get_parameter("ATC_RAT_RLL_P"), - self.get_parameter("ATC_RAT_RLL_I"), - self.get_parameter("ATC_RAT_RLL_D"))) + self.takeoff(10, mode='LOITER') + def print_gains(name, gains): + self.progress(f"AUTOTUNE {name} gains are P:%f I:%f D:%f" % ( + gains["ATC_RAT_RLL_P"], + gains["ATC_RAT_RLL_I"], + gains["ATC_RAT_RLL_D"] + )) + + def get_roll_gains(name): + ret = self.get_parameters([ + "ATC_RAT_RLL_D", + "ATC_RAT_RLL_I", + "ATC_RAT_RLL_P", + ], verbose=False) + print_gains(name, ret) + return ret + + def gains_same(gains1, gains2): + for c in 'P', 'I', 'D': + p_name = f"ATC_RAT_RLL_{c}" + if abs(gains1[p_name] - gains2[p_name]) > 0.00001: + return False + return True + + self.progress("Take a copy of original gains") + original_gains = get_roll_gains("pre-tuning") + scaled_original_gains = copy.copy(original_gains) + scaled_original_gains["ATC_RAT_RLL_I"] *= 0.1 + + pre_rllt = self.get_parameter("ATC_RAT_RLL_FLTT") + # hold position in loiter and run autotune self.set_rc(8, 1850) self.wait_mode('AUTOTUNE') @@ -2610,46 +2649,60 @@ def AutoTuneSwitch(self): if m is None: continue self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text)) + if "Determination Failed" in m.text: + break if "AutoTune: Success" in m.text: self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart)) - # Check original gains are re-instated + post_gains = get_roll_gains("post") + if gains_same(original_gains, post_gains): + raise NotAchievedException("AUTOTUNE gains not changed") + + # because of the way AutoTune works, once autotune is + # complete we return the original parameters via + # parameter-fetching, but fly on the tuned parameters + # (both sets with the I term scaled down). This test + # makes sure that's still the case. It would be nice + # if the PIDs parameters were `set` on success, but + # they aren't... Note that if we use the switch to + # restore the original gains and then start testing + # again (with the switch) then we see the new gains! + + # gains are scaled during the testing phase: + if not gains_same(scaled_original_gains, post_gains): + raise NotAchievedException("AUTOTUNE gains were reported as just original gains in test-mode. If you're fixing this, good!") # noqa + + self.progress("Check original gains are re-instated by switch") self.set_rc(8, 1100) self.delay_sim_time(1) - self.progress("AUTOTUNE original gains are P:%f I:%f D:%f" % - (self.get_parameter("ATC_RAT_RLL_P"), self.get_parameter("ATC_RAT_RLL_I"), - self.get_parameter("ATC_RAT_RLL_D"))) - if (rlld != self.get_parameter("ATC_RAT_RLL_D") or - rlli != self.get_parameter("ATC_RAT_RLL_I") or - rllp != self.get_parameter("ATC_RAT_RLL_P")): - raise NotAchievedException("AUTOTUNE gains still present") - # Use autotuned gains + current_gains = get_roll_gains("set-original") + if not gains_same(original_gains, current_gains): + raise NotAchievedException("AUTOTUNE original gains not restored") + + self.progress("Use autotuned gains") self.set_rc(8, 1850) self.delay_sim_time(1) - self.progress("AUTOTUNE testing gains are P:%f I:%f D:%f" % - (self.get_parameter("ATC_RAT_RLL_P"), self.get_parameter("ATC_RAT_RLL_I"), - self.get_parameter("ATC_RAT_RLL_D"))) - if (rlld == self.get_parameter("ATC_RAT_RLL_D") or - rlli == self.get_parameter("ATC_RAT_RLL_I") or - rllp == self.get_parameter("ATC_RAT_RLL_P")): - raise NotAchievedException("AUTOTUNE gains not present in pilot testing") - # land without changing mode + tuned_gains = get_roll_gains("tuned") + if gains_same(tuned_gains, original_gains): + raise NotAchievedException("AUTOTUNE tuned gains same as pre gains") + if gains_same(tuned_gains, scaled_original_gains): + raise NotAchievedException("AUTOTUNE tuned gains same as scaled pre gains") + + self.progress("land without changing mode") self.set_rc(3, 1000) self.wait_altitude(-1, 5, relative=True) self.wait_disarmed() - # Check gains are still there after disarm - if (rlld == self.get_parameter("ATC_RAT_RLL_D") or - rlli == self.get_parameter("ATC_RAT_RLL_I") or - rllp == self.get_parameter("ATC_RAT_RLL_P")): + self.progress("Check gains are still there after disarm") + disarmed_gains = get_roll_gains("post-disarm") + if not gains_same(tuned_gains, disarmed_gains): raise NotAchievedException("AUTOTUNE gains not present on disarm") self.reboot_sitl() - # Check gains are still there after reboot - if (rlld == self.get_parameter("ATC_RAT_RLL_D") or - rlli == self.get_parameter("ATC_RAT_RLL_I") or - rllp == self.get_parameter("ATC_RAT_RLL_P")): + self.progress("Check gains are still there after reboot") + reboot_gains = get_roll_gains("post-reboot") + if not gains_same(tuned_gains, reboot_gains): raise NotAchievedException("AUTOTUNE gains not present on reboot") - # Check FLTT is unchanged - if rllt != self.get_parameter("ATC_RAT_RLL_FLTT"): + self.progress("Check FLTT is unchanged") + if pre_rllt != self.get_parameter("ATC_RAT_RLL_FLTT"): raise NotAchievedException("AUTOTUNE FLTT was modified") return @@ -5437,32 +5490,6 @@ def Mount(self): ) self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) - # point gimbal at specified location - self.progress("Point gimbal at Location using MOUNT_CONTROL (GPS)") - self.do_pitch(despitch) - self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) - - # Delay here to allow the attitude to command to timeout and level out the copter a bit - self.delay_sim_time(3) - - start = self.mav.location() - self.progress("start=%s" % str(start)) - (t_lat, t_lon) = mavextra.gps_offset(start.lat, start.lng, 10, 20) - t_alt = 0 - - self.progress("loc %f %f %f" % (start.lat, start.lng, start.alt)) - self.progress("targetting %f %f %f" % (t_lat, t_lon, t_alt)) - self.do_pitch(despitch) - self.mav.mav.mount_control_send( - 1, # target system - 1, # target component - int(t_lat * 1e7), # lat - int(t_lon * 1e7), # lon - t_alt * 100, # alt - 0 # save position - ) - self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) - # this is a one-off; ArduCopter *will* time out this directive! self.progress("Levelling aircraft") self.mav.mav.set_attitude_target_send( @@ -7386,33 +7413,28 @@ def OBSTACLE_DISTANCE_3D(self): def AC_Avoidance_Proximity(self): '''Test proximity avoidance slide behaviour''' + self.context_push() - ex = None - try: - self.load_fence("copter-avoidance-fence.txt") - self.set_parameters({ - "FENCE_ENABLE": 1, - "PRX1_TYPE": 10, - "PRX_LOG_RAW": 1, - "RC10_OPTION": 40, # proximity-enable - }) - self.reboot_sitl() - self.progress("Enabling proximity") - self.set_rc(10, 2000) - self.check_avoidance_corners() - self.assert_current_onboard_log_contains_message("PRX") - self.assert_current_onboard_log_contains_message("PRXR") + self.load_fence("copter-avoidance-fence.txt") + self.set_parameters({ + "FENCE_ENABLE": 1, + "PRX1_TYPE": 10, + "PRX_LOG_RAW": 1, + "RC10_OPTION": 40, # proximity-enable + }) + self.reboot_sitl() + self.progress("Enabling proximity") + self.set_rc(10, 2000) + self.check_avoidance_corners() + + self.assert_current_onboard_log_contains_message("PRX") + self.assert_current_onboard_log_contains_message("PRXR") - except Exception as e: - self.print_exception_caught(e) - ex = e self.context_pop() - self.clear_fence() + self.disarm_vehicle(force=True) self.reboot_sitl() - if ex is not None: - raise ex def ProximitySensors(self): '''ensure proximity sensors return appropriate data''' @@ -7573,19 +7595,6 @@ def AC_Avoidance_Fence(self): self.set_parameter("FENCE_ENABLE", 1) self.check_avoidance_corners() - def global_position_int_for_location(self, loc, time_boot, heading=0): - return self.mav.mav.global_position_int_encode( - int(time_boot * 1000), # time_boot_ms - int(loc.lat * 1e7), - int(loc.lng * 1e7), - int(loc.alt * 1000), # alt in mm - 20, # relative alt - urp. - vx=0, - vy=0, - vz=0, - hdg=heading - ) - def ModeFollow(self): '''Fly follow mode''' foll_ofs_x = 30 # metres @@ -7596,6 +7605,7 @@ def ModeFollow(self): "FOLL_OFS_TYPE": 1, # relative to other vehicle heading }) self.takeoff(10, mode="LOITER") + self.context_push() self.set_parameter("SIM_SPEEDUP", 1) self.change_mode("FOLLOW") new_loc = self.mav.location() @@ -7615,6 +7625,8 @@ def ModeFollow(self): (expected_loc.lat, expected_loc.lng)) self.progress("expected_loc: %s" % str(expected_loc)) + origin = self.poll_message('GPS_GLOBAL_ORIGIN') + last_sent = 0 tstart = self.get_sim_time() while True: @@ -7622,9 +7634,17 @@ def ModeFollow(self): if now - tstart > 60: raise NotAchievedException("Did not FOLLOW") if now - last_sent > 0.5: - gpi = self.global_position_int_for_location(new_loc, - now, - heading=heading) + gpi = self.mav.mav.global_position_int_encode( + int(now * 1000), # time_boot_ms + int(new_loc.lat * 1e7), + int(new_loc.lng * 1e7), + int(new_loc.alt * 1000), # alt in mm + int(new_loc.alt * 1000 - origin.altitude), # relative alt - urp. + vx=0, + vy=0, + vz=0, + hdg=heading + ) gpi.pack(self.mav.mav) self.mav.mav.send(gpi) self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) @@ -7634,6 +7654,7 @@ def ModeFollow(self): self.progress("position delta=%f (want <%f)" % (delta, max_delta)) if delta < max_delta: break + self.context_pop() self.do_RTL() def get_global_position_int(self, timeout=30): @@ -11428,6 +11449,78 @@ def GuidedWeatherVane(self): self.wait_heading(90, timeout=60, minimum_duration=10) self.do_RTL() + def Clamp(self): + '''test Copter docking clamp''' + clamp_ch = 11 + self.set_parameters({ + "SIM_CLAMP_CH": clamp_ch, + }) + + self.takeoff(1, mode='LOITER') + + self.context_push() + self.context_collect('STATUSTEXT') + self.progress("Ensure can't take off with clamp in place") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000) + self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True) + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(0, 5, minimum_duration=5, relative=True) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000) + self.wait_statustext("SITL: Clamp: released vehicle", check_context=True) + self.wait_altitude(5, 5000, minimum_duration=1, relative=True) + self.do_RTL() + self.set_rc(3, 1000) + self.change_mode('LOITER') + self.context_pop() + + self.progress("Same again for repeatability") + self.context_push() + self.context_collect('STATUSTEXT') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000) + self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True) + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(0, 1, minimum_duration=5, relative=True) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000) + self.wait_statustext("SITL: Clamp: released vehicle", check_context=True) + self.wait_altitude(5, 5000, minimum_duration=1, relative=True) + self.do_RTL() + self.set_rc(3, 1000) + self.change_mode('LOITER') + self.context_pop() + + here = self.mav.location() + loc = self.offset_location_ne(here, 10, 0) + self.takeoff(5, mode='GUIDED') + self.do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL) + self.wait_location(loc, timeout=120) + self.land_and_disarm() + + # explicitly set home so we RTL to the right spot + self.set_home(here) + + self.context_push() + self.context_collect('STATUSTEXT') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000) + self.wait_statustext("SITL: Clamp: missed vehicle", check_context=True) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000) + self.context_pop() + + self.takeoff(5, mode='GUIDED') + self.do_RTL() + + self.takeoff(5, mode='GUIDED') + self.land_and_disarm() + + self.context_push() + self.context_collect('STATUSTEXT') + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000) + self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True) + self.context_pop() + + self.reboot_sitl() # because we set home + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11516,7 +11609,8 @@ def tests2b(self): # this block currently around 9.5mins here self.EK3_OGN_HGT_MASK, self.FarOrigin, self.GuidedForceArm, - self.GuidedWeatherVane + self.GuidedWeatherVane, + self.Clamp, ]) return ret @@ -11527,6 +11621,57 @@ def testcan(self): ]) return ret + def BattCANSplitAuxInfo(self): + '''test CAN battery periphs''' + self.start_subtest("Swap UAVCAN backend at runtime") + self.set_parameters({ + "CAN_P1_DRIVER": 1, + "BATT_MONITOR": 4, # 4 is ananlog volt+curr + "BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo + "BATT_SERIAL_NUM": 0, + "BATT2_SERIAL_NUM": 0, + "BATT_OPTIONS": 128, # allow split auxinfo + "BATT2_OPTIONS": 128, # allow split auxinfo + }) + self.reboot_sitl() + self.delay_sim_time(2) + self.set_parameters({ + "BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo + "BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo + }) + self.delay_sim_time(2) + self.set_parameters({ + "BATT_MONITOR": 4, # 8 is UAVCAN_BatteryInfo + "BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo + }) + self.delay_sim_time(2) + self.set_parameters({ + "BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo + "BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo + }) + self.delay_sim_time(2) + + def BattCANReplaceRuntime(self): + '''test CAN battery periphs''' + self.start_subtest("Replace UAVCAN backend at runtime") + self.set_parameters({ + "CAN_P1_DRIVER": 1, + "BATT_MONITOR": 11, # 4 is ananlog volt+curr + }) + self.reboot_sitl() + self.delay_sim_time(2) + self.set_parameters({ + "BATT_MONITOR": 8, # 4 is UAVCAN batterinfo + }) + self.delay_sim_time(2) + + def testcanbatt(self): + ret = ([ + self.BattCANReplaceRuntime, + self.BattCANSplitAuxInfo, + ]) + return ret + def tests(self): ret = [] ret.extend(self.tests1a()) @@ -11590,3 +11735,9 @@ class AutoTestCAN(AutoTestCopter): def tests(self): return self.testcan() + + +class AutoTestBattCAN(AutoTestCopter): + + def tests(self): + return self.testcanbatt() diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ba7a2eed20db4a..4eadb92c2f8b68 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -2136,6 +2136,9 @@ def AIRSPEED_AUTOCAL(self): self.fly_home_land_and_disarm() def deadreckoning_main(self, disable_airspeed_sensor=False): + self.set_parameter("EK3_OPTIONS", 1) + self.set_parameter("AHRS_OPTIONS", 3) + self.set_parameter("LOG_REPLAY", 1) self.reboot_sitl() self.wait_ready_to_arm() self.gpi = None @@ -2153,7 +2156,10 @@ def validate_global_position_int_against_simstate(mav, m): if self.simstate is None: return divergence = self.get_distance_int(self.gpi, self.simstate) - max_allowed_divergence = 200 + if disable_airspeed_sensor: + max_allowed_divergence = 300 + else: + max_allowed_divergence = 150 if (time.time() - self.last_print > 1 or divergence > self.max_divergence): self.progress("position-estimate-divergence=%fm" % (divergence,)) @@ -2188,15 +2194,42 @@ def validate_global_position_int_against_simstate(mav, m): frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, ) self.wait_location(loc, accuracy=100) - self.progress("Stewing") - self.delay_sim_time(20) + self.progress("Orbit with GPS and learn wind") + # allow longer to learn wind if there is no airspeed sensor + if disable_airspeed_sensor: + self.delay_sim_time(60) + else: + self.delay_sim_time(20) self.set_parameter("SIM_GPS_DISABLE", 1) - self.progress("Roasting") + self.progress("Continue orbit without GPS") self.delay_sim_time(20) self.change_mode("RTL") self.wait_distance_to_home(100, 200, timeout=200) + # go into LOITER to create additonal time for a GPS re-enable test + self.change_mode("LOITER") self.set_parameter("SIM_GPS_DISABLE", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + if self.get_sim_time() < (t_enabled+9): + raise NotAchievedException("GPS use re-started too quickly") + # wait for EKF and vehicle position to stabilise, then test response to jamming + self.delay_sim_time(20) + + self.set_parameter("AHRS_OPTIONS", 1) + self.set_parameter("SIM_GPS_JAM", 1) self.delay_sim_time(10) + self.set_parameter("SIM_GPS_JAM", 0) + t_enabled = self.get_sim_time() + # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning + # to prevent bad GPS being used when coming back after loss of lock due to interence. + # The EKF_STATUS_REPORT does not tell us when the good to align check passes, so the minimum time + # value of 3.0 seconds is an arbitrary value set on inspection of dataflash logs from this test + self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15) + time_since_jamming_stopped = self.get_sim_time() - t_enabled + if time_since_jamming_stopped < 3: + raise NotAchievedException("GPS use re-started %f sec after jamming stopped" % time_since_jamming_stopped) self.set_rc(3, 1000) self.fly_home_land_and_disarm() self.progress("max-divergence: %fm" % (self.max_divergence,)) @@ -5356,6 +5389,17 @@ def ClimbThrottleSaturation(self): self.disarm_vehicle(force=True) self.reboot_sitl() + def GuidedAttitudeNoGPS(self): + '''test that guided-attitude still works with no GPS''' + self.takeoff(50) + self.change_mode('GUIDED') + self.context_push() + self.set_parameter('SIM_GPS_DISABLE', 1) + self.delay_sim_time(30) + self.set_attitude_target() + self.context_pop() + self.fly_home_land_and_disarm() + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -5466,6 +5510,7 @@ def tests(self): self.MAV_CMD_NAV_RETURN_TO_LAUNCH, self.MinThrottle, self.ClimbThrottleSaturation, + self.GuidedAttitudeNoGPS, ]) return ret diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 596a42db019982..d11c11ff0e0808 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -287,7 +287,9 @@ def should_run_step(step): "BalanceBot": "ardurover", "Sailboat": "ardurover", "SITLPeriphUniversal": ("sitl_periph_universal", "AP_Periph"), + "SITLPeriphBattMon": ("sitl_periph_battmon", "AP_Periph"), "CAN": "arducopter", + "BattCAN": "arducopter", } @@ -358,11 +360,15 @@ def find_specific_test_to_run(step): "test.Sub": ardusub.AutoTestSub, "test.Tracker": antennatracker.AutoTestTracker, "test.CAN": arducopter.AutoTestCAN, + "test.BattCAN": arducopter.AutoTestBattCAN, } supplementary_test_binary_map = { "test.CAN": ["sitl_periph_universal:AP_Periph:0:Tools/autotest/default_params/periph.parm,Tools/autotest/default_params/quad-periph.parm", # noqa: E501 "sitl_periph_universal:AP_Periph:1:Tools/autotest/default_params/periph.parm"], + "test.BattCAN": [ + "sitl_periph_battmon:AP_Periph:0:Tools/autotest/default_params/periph-battmon.parm,Tools/autotest/default_params/quad-periph.parm", # noqa: E501 + ], } @@ -445,6 +451,10 @@ def run_step(step): vehicle_binary = 'bin/AP_Periph' board = 'sitl_periph_universal' + if step == 'build.SITLPeriphBattMon': + vehicle_binary = 'bin/AP_Periph' + board = 'sitl_periph_battmon' + if step == 'build.Replay': return util.build_replay(board='SITL') @@ -1081,6 +1091,9 @@ def format_epilog(self, formatter): 'build.SITLPeriphUniversal', 'test.CAN', + 'build.SITLPeriphBattMon', + 'test.BattCAN', + # convertgps disabled as it takes 5 hours # 'convertgpx', ] diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index d057def1a881d5..7fdef2bcc8450f 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -34,6 +34,7 @@ import tempfile import threading import enum +from inspect import currentframe, getframeinfo from pathlib import Path from MAVProxy.modules.lib import mp_util @@ -6026,11 +6027,11 @@ def get_parameter_mavproxy(self, mavproxy, name, attempts=1, timeout=60): pass raise NotAchievedException("Failed to retrieve parameter (%s)" % name) - def get_parameters(self, some_list): + def get_parameters(self, some_list, **kwargs): ret = {} for n in some_list: - ret[n] = self.get_parameter(n) + ret[n] = self.get_parameter(n, **kwargs) return ret @@ -6225,19 +6226,20 @@ def run_cmd_int(self, command_name = mavutil.mavlink.enums["MAV_CMD"][command].name except KeyError: command_name = "UNKNOWNu" - self.progress("Sending COMMAND_INT to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%u p6=%u p7=%f)" % - ( - target_sysid, - target_compid, - command_name, - command, - p1, - p2, - p3, - p4, - x, - y, - z)) + self.progress("Sending COMMAND_INT to (%u,%u) (%s=%u) (p1=%f p2=%f p3=%f p4=%f p5=%u p6=%u p7=%f f=%u)" % ( + target_sysid, + target_compid, + command_name, + command, + p1, + p2, + p3, + p4, + x, + y, + z, + frame + )) mav.mav.command_int_send(target_sysid, target_compid, frame, @@ -6417,6 +6419,12 @@ def verify_parameter_values(self, parameter_stuff, max_delta=0.0): ################################################# # UTILITIES ################################################# + def lineno(self): + '''return line number''' + frameinfo = getframeinfo(currentframe().f_back) + # print(frameinfo.filename, frameinfo.lineno) + return frameinfo.lineno + @staticmethod def longitude_scale(lat): ret = math.cos(lat * (math.radians(1))) @@ -8456,7 +8464,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= ex = e # reset the message hooks; we've failed-via-exception and # can't expect the hooks to have been cleaned up - for h in self.mav.message_hooks: + for h in copy.copy(self.mav.message_hooks): if h not in start_message_hooks: self.mav.message_hooks.remove(h) hooks_removed = True @@ -8508,6 +8516,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= self.reset_SITL_commandline() else: self.progress("Force-rebooting SITL") + self.zero_throttle() self.reboot_sitl() # that'll learn it passed = False elif ardupilot_alive and not passed: # implicit reboot after a failed test: @@ -13695,8 +13704,8 @@ def GPSTypes(self): (1, "UBLOX", None, "u-blox", 5, 'probing'), (5, "NMEA", 5, "NMEA", 5, 'probing'), (6, "SBP", None, "SBP", 5, 'probing'), - # (7, "SBP2", 9, "SBP2", 5), # broken, "waiting for config data" (8, "NOVA", 15, "NOVA", 5, 'probing'), # no attempt to auto-detect this in AP_GPS + (9, "SBP2", None, "SBP2", 5, 'probing'), (11, "GSOF", 11, "GSOF", 5, 'specified'), # no attempt to auto-detect this in AP_GPS (19, "MSP", 19, "MSP", 32, 'specified'), # no attempt to auto-detect this in AP_GPS # (9, "FILE"), diff --git a/libraries/AC_Fence/AC_Fence_config.h b/libraries/AC_Fence/AC_Fence_config.h index 4abb471eac8319..119616c98194b9 100644 --- a/libraries/AC_Fence/AC_Fence_config.h +++ b/libraries/AC_Fence/AC_Fence_config.h @@ -12,6 +12,10 @@ #define AP_FENCE_ENABLED 2 #endif +// CODE_REMOVAL +// ArduPilot 4.6 sends deprecation warnings for FENCE_POINT/FENCE_FETCH_POINT +// ArduPilot 4.7 stops compiling them in +// ArduPilot 4.8 removes the code entirely #ifndef AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT #define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT HAL_GCS_ENABLED && AP_FENCE_ENABLED #endif diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 454bf0203e1fa1..8b7c1faa0caddd 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -3536,7 +3536,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const // return location corresponding to vector relative to the // vehicle's origin -bool AP_AHRS::get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const +bool AP_AHRS::get_location_from_origin_offset_NED(Location &loc, const Vector3p &offset_ned) const { if (!get_origin(loc)) { return false; @@ -3548,7 +3548,7 @@ bool AP_AHRS::get_location_from_origin_offset(Location &loc, const Vector3p &off // return location corresponding to vector relative to the // vehicle's home location -bool AP_AHRS::get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const +bool AP_AHRS::get_location_from_home_offset_NED(Location &loc, const Vector3p &offset_ned) const { if (!home_is_set()) { return false; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index d71912c9f58e19..16ec1009e03bf1 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -277,8 +277,8 @@ class AP_AHRS { // return location corresponding to vector relative to the // vehicle's origin - bool get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; - bool get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; + bool get_location_from_origin_offset_NED(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; + bool get_location_from_home_offset_NED(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED; // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for. diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 8c2f58e12df65e..facf80f93d63a4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -481,7 +481,8 @@ AP_BattMonitor::init() memset(&state[instance].cell_voltages, 0xFF, sizeof(cells)); state[instance].instance = instance; - switch (get_type(instance)) { + const auto allocation_type = configured_type(instance); + switch (allocation_type) { #if AP_BATTERY_ANALOG_ENABLED case Type::ANALOG_VOLTAGE_ONLY: case Type::ANALOG_VOLTAGE_AND_CURRENT: @@ -613,7 +614,9 @@ AP_BattMonitor::init() default: break; } - + if (drivers[instance] != nullptr) { + state[instance].type = allocation_type; + } // if the backend has some local parameters then make those available in the tree if (drivers[instance] && state[instance].var_info) { backend_var_info[instance] = state[instance].var_info; @@ -705,7 +708,16 @@ void AP_BattMonitor::read() #endif for (uint8_t i=0; i<_num_instances; i++) { - if (drivers[i] != nullptr && get_type(i) != Type::NONE) { + if (drivers[i] == nullptr) { + continue; + } + if (allocated_type(i) != configured_type(i)) { + continue; + } + // allow run-time disabling; this is technically redundant + if (configured_type(i) == Type::NONE) { + continue; + } drivers[i]->read(); drivers[i]->update_resistance_estimate(); @@ -720,7 +732,6 @@ void AP_BattMonitor::read() drivers[i]->Log_Write_BCL(i, time_us); } #endif - } } check_failsafes(); @@ -1132,7 +1143,14 @@ void AP_BattMonitor::MPPT_set_powered_state(const uint8_t instance, const bool p bool AP_BattMonitor::healthy() const { for (uint8_t i=0; i< _num_instances; i++) { - if (get_type(i) != Type::NONE && !healthy(i)) { + if (allocated_type(i) != configured_type(i)) { + return false; + } + // allow run-time disabling; this is technically redundant + if (configured_type(i) == Type::NONE) { + continue; + } + if (!healthy(i)) { return false; } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 368fa85d130a71..ded686e742b443 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -163,6 +163,7 @@ class AP_BattMonitor uint8_t state_of_health_pct; // state of health (SOH) in percent bool has_state_of_health_pct; // state_of_health_pct is only valid if this is true uint8_t instance; // instance number of this backend + Type type; // allocated instance type const struct AP_Param::GroupInfo *var_info; }; @@ -222,11 +223,14 @@ class AP_BattMonitor /// returns the highest failsafe action that has been triggered int8_t get_highest_failsafe_priority(void) const { return _highest_failsafe_priority; }; - /// get_type - returns battery monitor type - enum Type get_type() const { return get_type(AP_BATT_PRIMARY_INSTANCE); } - enum Type get_type(uint8_t instance) const { + /// configured_type - returns battery monitor type as configured in parameters + enum Type configured_type(uint8_t instance) const { return (Type)_params[instance]._type.get(); } + /// allocated_type - returns battery monitor type as allocated + enum Type allocated_type(uint8_t instance) const { + return state[instance].type; + } /// get_serial_number - returns battery serial number int32_t get_serial_number() const { return get_serial_number(AP_BATT_PRIMARY_INSTANCE); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index e6adb47ecf580a..24bb0b69f5f088 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -79,7 +79,7 @@ AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneC const auto &batt = AP::battery(); for (uint8_t i = 0; i < batt._num_instances; i++) { if (batt.drivers[i] == nullptr || - batt.get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { + batt.allocated_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { continue; } AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)batt.drivers[i]; @@ -90,7 +90,7 @@ AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneC // find empty uavcan driver for (uint8_t i = 0; i < batt._num_instances; i++) { if (batt.drivers[i] != nullptr && - batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && + batt.allocated_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && match_battery_id(i, battery_id)) { AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)batt.drivers[i]; @@ -241,7 +241,7 @@ void AP_BattMonitor_DroneCAN::handle_battery_info_aux_trampoline(AP_DroneCAN *ap for (uint8_t i = 0; i < batt._num_instances; i++) { const auto *drv = batt.drivers[i]; if (drv != nullptr && - batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && + batt.allocated_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && drv->option_is_set(AP_BattMonitor_Params::Options::AllowSplitAuxInfo) && batt.get_serial_number(i) == int32_t(msg.battery_id)) { driver = (AP_BattMonitor_DroneCAN *)batt.drivers[i]; diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 273ecb9fe50b16..27803566d0bfd2 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -222,7 +222,10 @@ bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const } #if AP_AHRS_ENABLED -bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const +// converts location to a vector from origin; if this method returns +// false then vec_ne is unmodified +template +bool Location::get_vector_xy_from_origin_NE(T &vec_ne) const { Location ekf_origin; if (!AP::ahrs().get_origin(ekf_origin)) { @@ -233,22 +236,39 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const return true; } -bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const -{ - // convert lat, lon - if (!get_vector_xy_from_origin_NE(vec_neu.xy())) { - return false; - } +// define for float and position vectors +template bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const; +#if HAL_WITH_POSTYPE_DOUBLE +template bool Location::get_vector_xy_from_origin_NE(Vector2p &vec_ne) const; +#endif +// converts location to a vector from origin; if this method returns +// false then vec_neu is unmodified +template +bool Location::get_vector_from_origin_NEU(T &vec_neu) const +{ // convert altitude int32_t alt_above_origin_cm = 0; if (!get_alt_cm(AltFrame::ABOVE_ORIGIN, alt_above_origin_cm)) { return false; } + + // convert lat, lon + if (!get_vector_xy_from_origin_NE(vec_neu.xy())) { + return false; + } + vec_neu.z = alt_above_origin_cm; return true; } + +// define for float and position vectors +template bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const; +#if HAL_WITH_POSTYPE_DOUBLE +template bool Location::get_vector_from_origin_NEU(Vector3p &vec_neu) const; +#endif + #endif // AP_AHRS_ENABLED // return horizontal distance in meters between two locations diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index e84be52611105e..f61756d7d9ece8 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -55,12 +55,17 @@ class Location // - above-origin and origin is not set bool change_alt_frame(AltFrame desired_frame); - // get position as a vector (in cm) from origin (x,y only or x,y,z) - // return false on failure to get the vector which can only - // happen if the EKF origin has not been set yet - // x, y and z are in centimetres - bool get_vector_xy_from_origin_NE(Vector2f &vec_ne) const WARN_IF_UNUSED; - bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED; + // get position as a vector (in cm) from origin (x,y only or + // x,y,z) return false on failure to get the vector which can only + // happen if the EKF origin has not been set yet x, y and z are in + // centimetres. If this method returns false then vec_ne is + // unmodified. + template + bool get_vector_xy_from_origin_NE(T &vec_ne) const WARN_IF_UNUSED; + // converts location to a vector from origin; if this method returns + // false then vec_neu is unmodified + template + bool get_vector_from_origin_NEU(T &vec_neu) const WARN_IF_UNUSED; // return horizontal distance in meters between two locations ftype get_distance(const Location &loc2) const; diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index d545c2c754df14..c4a7cd90dcf9c1 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -280,8 +280,9 @@ TEST(Location, Tests) EXPECT_EQ(0, test_location4.loiter_xtrack); EXPECT_TRUE(test_location4.initialised()); - const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; - EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3)); + // can't create a Location using a vector here as there's no origin for the vector to be relative to: + // const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; + // EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3)); } TEST(Location, Distance) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index a93f79d667ce21..8723f101bdadec 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -272,27 +272,40 @@ bool AP_Follow::get_target_heading_deg(float &heading) const return true; } -// handle mavlink DISTANCE_SENSOR messages -void AP_Follow::handle_msg(const mavlink_message_t &msg) +// returns true if we should extract information from msg +bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const { // exit immediately if not enabled if (!_enabled) { - return; + return false; } // skip our own messages if (msg.sysid == mavlink_system.sysid) { - return; + return false; } // skip message if not from our target if (_sysid != 0 && msg.sysid != _sysid) { - if (_automatic_sysid) { - // maybe timeout who we were following... - if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) { - _sysid.set(0); - } + return false; + } + + return true; +} + +// handle mavlink DISTANCE_SENSOR messages +void AP_Follow::handle_msg(const mavlink_message_t &msg) +{ + // this method should be called from an "update()" method: + if (_automatic_sysid) { + // maybe timeout who we were following... + if ((_last_location_update_ms == 0) || + (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) { + _sysid.set(0); } + } + + if (!should_handle_message(msg)) { return; } @@ -301,13 +314,31 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) switch (msg.msgid) { case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { + updated = handle_global_position_int_message(msg); + break; + } + case MAVLINK_MSG_ID_FOLLOW_TARGET: { + updated = handle_follow_target_message(msg); + break; + } + } + + if (updated) { +#if HAL_LOGGING_ENABLED + Log_Write_FOLL(); +#endif + } +} + +bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg) +{ // decode message mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(&msg, &packet); // ignore message if lat and lon are (exactly) zero if ((packet.lat == 0 && packet.lon == 0)) { - return; + return false; } _target_location.lat = packet.lat; @@ -337,21 +368,22 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) _sysid.set(msg.sysid); _automatic_sysid = true; } - updated = true; - break; - } - case MAVLINK_MSG_ID_FOLLOW_TARGET: { + return true; +} + +bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg) +{ // decode message mavlink_follow_target_t packet; mavlink_msg_follow_target_decode(&msg, &packet); // ignore message if lat and lon are (exactly) zero if ((packet.lat == 0 && packet.lon == 0)) { - return; + return false; } // require at least position if ((packet.est_capabilities & (1<<0)) == 0) { - return; + return false; } Location new_loc = _target_location; @@ -363,7 +395,7 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) // above home if we are configured for relative alt if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE && !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { - return; + return false; } _target_location = new_loc; @@ -391,13 +423,14 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) _sysid.set(msg.sysid); _automatic_sysid = true; } - updated = true; - break; - } - } - if (updated) { + return true; +} + +// write out an onboard-log message to help diagnose follow problems: #if HAL_LOGGING_ENABLED +void AP_Follow::Log_Write_FOLL() +{ // get estimated location and velocity Location loc_estimate{}; Vector3f vel_estimate; @@ -432,9 +465,8 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) loc_estimate.lng, loc_estimate.alt ); -#endif - } } +#endif // HAL_LOGGING_ENABLED // get velocity estimate in m/s in NED frame using dt since last update bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const @@ -492,9 +524,10 @@ bool AP_Follow::get_offsets_ned(Vector3f &offset) const Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const { // rotate roll, pitch input from north facing to vehicle's perspective - const float cos_yaw = cosf(radians(angle_deg)); - const float sin_yaw = sinf(radians(angle_deg)); - return Vector3f((vec.x * cos_yaw) - (vec.y * sin_yaw), (vec.y * cos_yaw) + (vec.x * sin_yaw), vec.z); + Vector3f ret = vec; + ret.xy().rotate(radians(angle_deg)); + + return ret; } // set recorded distance and bearing to target to zero diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 5918215352a7ca..48c43fc9ff4706 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -118,6 +118,9 @@ class AP_Follow private: static AP_Follow *_singleton; + // returns true if we should extract information from msg + bool should_handle_message(const mavlink_message_t &msg) const; + // get velocity estimate in m/s in NED frame using dt since last update bool get_velocity_ned(Vector3f &vel_ned, float dt) const; @@ -133,6 +136,13 @@ class AP_Follow // set recorded distance and bearing to target to zero void clear_dist_and_bearing_to_target(); + // handle various mavlink messages supplying position: + bool handle_global_position_int_message(const mavlink_message_t &msg); + bool handle_follow_target_message(const mavlink_message_t &msg); + + // write out an onboard-log message to help diagnose follow problems: + void Log_Write_FOLL(); + // parameters AP_Int8 _enabled; // 1 if this subsystem is enabled AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 6f98b115b9acb2..f9308b8fc3cdcc 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -334,6 +334,12 @@ AP_GPS_UBLOX::_request_next_config(void) } break; case STEP_POLL_GNSS: + if (supports_F9_config()) { + if (last_configured_gnss != params.gnss_mode) { + _unconfigured_messages |= CONFIG_F9; + } + break; + } if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) { _next_message--; } @@ -455,7 +461,43 @@ AP_GPS_UBLOX::_request_next_config(void) #endif break; + case STEP_F9: { + if (_hardware_generation == UBLOX_F9) { + uint8_t cfg_count = populate_F9_gnss(); + // special handling of F9 config + if (cfg_count > 0) { + CFG_Debug("Sending F9 settings, GNSS=%u", params.gnss_mode); + if (!_configure_list_valset(config_GNSS, cfg_count, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { + _next_message--; + break; + } + _f9_config_time = AP_HAL::millis(); + } + } + break; + } + + case STEP_F9_VALIDATE: { + if (_hardware_generation == UBLOX_F9) { + // GNSS takes 0.5s to reset + if (AP_HAL::millis() - _f9_config_time < 500) { + _next_message--; + break; + } + _f9_config_time = 0; + uint8_t cfg_count = populate_F9_gnss(); + // special handling of F9 config + if (cfg_count > 0) { + CFG_Debug("Validating F9 settings, GNSS=%u", params.gnss_mode); + // now validate all of the settings, this is a no-op if the first call succeeded + if (!_configure_config_set(config_GNSS, cfg_count, CONFIG_F9, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { + _next_message--; + } + } + } + break; + } case STEP_M10: { if (_hardware_generation == UBLOX_M10) { // special handling of M10 config @@ -1029,6 +1071,7 @@ AP_GPS_UBLOX::_parse_gps(void) _unconfigured_messages &= ~CONFIG_TP5; break; } + break; case CLASS_MON: switch(_buffer.ack.msgID) { @@ -1046,6 +1089,7 @@ AP_GPS_UBLOX::_parse_gps(void) case CLASS_CFG: switch(_buffer.nack.msgID) { case MSG_CFG_VALGET: + CFG_Debug("NACK VALGET 0x%x", (unsigned)_buffer.nack.msgID); if (active_config.list != nullptr) { /* likely this device does not support fetching multiple keys at once, go one at a time @@ -1072,6 +1116,15 @@ AP_GPS_UBLOX::_parse_gps(void) } } break; + case MSG_CFG_VALSET: + CFG_Debug("NACK VALSET 0x%x 0x%x", (unsigned)_buffer.nack.msgID, + unsigned(active_config.list[active_config.set_index].key)); + if (is_gnss_key(active_config.list[active_config.set_index].key)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GPS %u: unable to configure band 0x%02x", + unsigned(state.instance + 1), unsigned(active_config.list[active_config.set_index].key)); + + } + break; } } } @@ -1114,7 +1167,7 @@ AP_GPS_UBLOX::_parse_gps(void) #if UBLOX_GNSS_SETTINGS case MSG_CFG_GNSS: - if (params.gnss_mode != 0) { + if (params.gnss_mode != 0 && !supports_F9_config()) { struct ubx_cfg_gnss start_gnss = _buffer.gnss; uint8_t gnssCount = 0; Debug("Got GNSS Settings %u %u %u %u:\n", @@ -1290,12 +1343,16 @@ AP_GPS_UBLOX::_parse_gps(void) // see if it is in active config list int8_t cfg_idx = find_active_config_index(id); if (cfg_idx >= 0) { + CFG_Debug("valset(0x%lx): %u", uint32_t(id), (*cfg_data) & 0x1); const uint8_t key_size = config_key_size(id); - if (cfg_len < key_size || - memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) { + if (cfg_len < key_size + // for keys of length 1 only the LSB is significant + || (key_size == 1 && (active_config.list[cfg_idx].value & 0x1) != (*cfg_data & 0x1)) + || memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) { _configure_valset(id, &active_config.list[cfg_idx].value, active_config.layers); _unconfigured_messages |= active_config.unconfig_bit; active_config.done_mask &= ~(1U << cfg_idx); + active_config.set_index = cfg_idx; _cfg_needs_save = true; } else { active_config.done_mask |= (1U << cfg_idx); @@ -1318,6 +1375,8 @@ AP_GPS_UBLOX::_parse_gps(void) unsigned(active_config.list[active_config.fetch_index].key)); } } + } else { + CFG_Debug("valget no active config for 0x%lx", (uint32_t)id); } // step over the value @@ -1349,8 +1408,14 @@ AP_GPS_UBLOX::_parse_gps(void) _have_version = true; strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion)); strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion)); + void* mod = memmem(_buffer.mon_ver.extension, sizeof(_buffer.mon_ver.extension), "MOD=", 4); + if (mod != nullptr) { + strncpy(_module, (char*)mod+4, UBLOX_MODULE_LEN-1); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, - "u-blox %d HW: %s SW: %s", + "u-blox %s%s%d HW: %s SW: %s", + _module, mod != nullptr ? " " : "", state.instance + 1, _version.hwVersion, _version.swVersion); @@ -1364,6 +1429,13 @@ AP_GPS_UBLOX::_parse_gps(void) _unconfigured_messages |= CONFIG_TMODE_MODE; } _hardware_generation = UBLOX_F9; + _unconfigured_messages |= CONFIG_F9; + _unconfigured_messages &= ~CONFIG_GNSS; + if (strncmp(_module, "ZED-F9P", UBLOX_MODULE_LEN) == 0) { + _hardware_variant = UBLOX_F9_ZED; + } else if (strncmp(_module, "NEO-F9P", UBLOX_MODULE_LEN) == 0) { + _hardware_variant = UBLOX_F9_NEO; + } } if (strncmp(_version.swVersion, "EXT CORE 4", 10) == 0) { // a M9 @@ -1866,6 +1938,7 @@ AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value, uint8_t layers if (!supports_F9_config()) { return false; } + const uint8_t len = config_key_size(key); struct ubx_cfg_valset msg {}; uint8_t buf[sizeof(msg)+len]; @@ -1939,6 +2012,48 @@ AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint return _send_message(CLASS_CFG, MSG_CFG_VALGET, buf, sizeof(buf)); } +/* + * configure F9 based key/value pair for a complete configuration set + * + * this method sends all the key/value pairs in a block, but makes no attempt to check + * the results. Sending in a block is necessary for updates such as GNSS where certain + * combinations are invalid and setting one at a time will not produce the correct result + * if the result needs to be validated then a subsequent _configure_config_set() can be + * issued which will get all the values and reset those that are not properly updated. + */ +bool +AP_GPS_UBLOX::_configure_list_valset(const config_list *list, uint8_t count, uint8_t layers) +{ + if (!supports_F9_config()) { + return false; + } + + struct ubx_cfg_valset msg {}; + uint8_t buf[sizeof(msg)+sizeof(config_list)*count]; + if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) { + return false; + } + msg.version = 1; + msg.layers = layers; + msg.transaction = 0; + uint32_t msg_len = sizeof(msg) - sizeof(msg.key); + memcpy(buf, &msg, msg_len); + + uint8_t* payload = &buf[msg_len]; + for (uint8_t i=0; i1024) @@ -68,6 +69,7 @@ #define UBX_TIMEGPS_VALID_WEEK_MASK 0x2 #define UBLOX_MAX_PORTS 6 +#define UBLOX_MODULE_LEN 9 #define RATE_POSLLH 1 #define RATE_STATUS 1 @@ -99,9 +101,10 @@ #define CONFIG_TMODE_MODE (1<<16) #define CONFIG_RTK_MOVBASE (1<<17) #define CONFIG_TIM_TM2 (1<<18) -#define CONFIG_M10 (1<<19) -#define CONFIG_L5 (1<<20) -#define CONFIG_LAST (1<<21) // this must always be the last bit +#define CONFIG_F9 (1<<19) +#define CONFIG_M10 (1<<20) +#define CONFIG_L5 (1<<21) +#define CONFIG_LAST (1<<22) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) @@ -518,7 +521,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend struct PACKED ubx_mon_ver { char swVersion[30]; char hwVersion[10]; - char extension[50]; // extensions are not enabled + char extension[30*UBLOX_MAX_EXTENSIONS]; // extensions are not enabled }; struct PACKED ubx_nav_svinfo_header { uint32_t itow; @@ -719,6 +722,12 @@ class AP_GPS_UBLOX : public AP_GPS_Backend // flagging state in the driver }; + enum ubx_hardware_variant { + UBLOX_F9_ZED, // comes from MON_VER extension strings + UBLOX_F9_NEO, // comes from MON_VER extension strings + UBLOX_UNKNOWN_HARDWARE_VARIANT = 0xff + }; + enum config_step { STEP_PVT = 0, STEP_NAV_RATE, // poll NAV rate @@ -742,6 +751,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend STEP_VERSION, STEP_RTK_MOVBASE, // setup moving baseline STEP_TIM_TM2, + STEP_F9, + STEP_F9_VALIDATE, STEP_M10, STEP_L5, STEP_LAST @@ -765,13 +776,16 @@ class AP_GPS_UBLOX : public AP_GPS_Backend uint32_t _last_cfg_sent_time; uint8_t _num_cfg_save_tries; uint32_t _last_config_time; + uint32_t _f9_config_time; uint16_t _delay_time; uint8_t _next_message; uint8_t _ublox_port; bool _have_version; struct ubx_mon_ver _version; + char _module[UBLOX_MODULE_LEN]; uint32_t _unconfigured_messages; uint8_t _hardware_generation; + uint8_t _hardware_variant; uint32_t _last_pvt_itow; uint32_t _last_relposned_itow; uint32_t _last_relposned_ms; @@ -798,8 +812,17 @@ class AP_GPS_UBLOX : public AP_GPS_Backend bool havePvtMsg; + // structure for list of config key/value pairs for + // specific configurations + struct PACKED config_list { + ConfigKey key; + // support up to 4 byte values, assumes little-endian + uint32_t value; + }; + bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); bool _configure_valset(ConfigKey key, const void *value, uint8_t layers=UBX_VALSET_LAYER_ALL); + bool _configure_list_valset(const config_list *list, uint8_t count, uint8_t layers=UBX_VALSET_LAYER_ALL); bool _configure_valget(ConfigKey key); void _configure_rate(void); void _configure_sbas(bool enable); @@ -828,14 +851,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend } #endif - // structure for list of config key/value pairs for - // specific configurations - struct PACKED config_list { - ConfigKey key; - // support up to 4 byte values, assumes little-endian - uint32_t value; - }; - // return size of a config key payload uint8_t config_key_size(ConfigKey key) const; @@ -849,6 +864,13 @@ class AP_GPS_UBLOX : public AP_GPS_Backend // return true if GPS is capable of F9 config bool supports_F9_config(void) const; + // is the config key a GNSS key + bool is_gnss_key(ConfigKey key) const; + + // populate config_GNSS for F9P + uint8_t populate_F9_gnss(void); + uint8_t last_configured_gnss; + uint8_t _pps_freq = 1; #ifdef HAL_GPIO_PPS void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us); @@ -863,6 +885,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend uint32_t unconfig_bit; uint8_t layers; int8_t fetch_index; + int8_t set_index; } active_config; #if GPS_MOVING_BASELINE @@ -882,6 +905,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend static const config_list config_M10[]; static const config_list config_L5_ovrd_ena[]; static const config_list config_L5_ovrd_dis[]; + // scratch space for GNSS config + config_list* config_GNSS; }; #endif diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 24b6a66d3b5c68..cd9dab25655eb2 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -381,4 +381,8 @@ #error "HAL_GPIO_LED_OFF must not be defined, it is implicitly !HAL_GPIO_LED_ON" #endif +#ifndef HAL_WITH_POSTYPE_DOUBLE +#define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024 +#endif + #define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON) diff --git a/libraries/AP_HAL/UARTDriver.cpp b/libraries/AP_HAL/UARTDriver.cpp index 547cd55ab75656..55412be3185c7f 100644 --- a/libraries/AP_HAL/UARTDriver.cpp +++ b/libraries/AP_HAL/UARTDriver.cpp @@ -179,6 +179,11 @@ bool AP_HAL::UARTDriver::flow_control_enabled(enum flow_control flow_control_set return false; } +uint8_t AP_HAL::UARTDriver::get_parity(void) +{ + return AP_HAL::UARTDriver::parity; +} + #if HAL_UART_STATS_ENABLED // Take cumulative bytes and return the change since last call uint32_t AP_HAL::UARTDriver::StatsTracker::ByteTracker::update(uint32_t bytes) diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index db395550e00dd7..317ddd1fba5437 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -78,6 +78,9 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // read buffer from a locked port. If port is locked and key is not correct then -1 is returned ssize_t read_locked(uint8_t *buf, size_t count, uint32_t key) WARN_IF_UNUSED; + + // get current parity for passthrough use + uint8_t get_parity(void); // control optional features virtual bool set_options(uint16_t options) { _last_options = options; return options==0; } @@ -189,6 +192,9 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { // return true requested baud on USB port virtual uint32_t get_usb_baud(void) const { return 0; } + // return requested parity on USB port + virtual uint8_t get_usb_parity(void) const { return parity; } + // disable TX/RX pins for unusued uart virtual void disable_rxtx(void) const {} @@ -213,6 +219,8 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { uint32_t lock_write_key; uint32_t lock_read_key; + uint8_t parity; + /* backend begin method */ diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 70ee7f2875c411..d0fedce5479151 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -672,6 +672,19 @@ uint32_t UARTDriver::get_usb_baud() const return 0; } +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero. +*/ +uint8_t UARTDriver::get_usb_parity() const +{ +#if HAL_USE_SERIAL_USB + if (sdef.is_usb) { + return ::get_usb_parity(sdef.endpoint_id); + } +#endif + return 0; +} + uint32_t UARTDriver::_available() { if (!_rx_initialised || _uart_owner_thd != chThdGetSelfX()) { @@ -1429,6 +1442,7 @@ void UARTDriver::configure_parity(uint8_t v) // not possible return; } + UARTDriver::parity = v; #if HAL_USE_SERIAL == TRUE // stop and start to take effect sdStop((SerialDriver*)sdef.serial); diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 900086bbc03726..e50230d427ae6b 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -38,6 +38,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { bool is_initialized() override; bool tx_pending() override; uint32_t get_usb_baud() const override; + uint8_t get_usb_parity() const override; // disable TX/RX pins for unusued uart void disable_rxtx(void) const override; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat index bfc89616e307dd..3a2959b13352cd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat @@ -110,7 +110,6 @@ BARO MS56XX SPI:ms5611 PB8 TIM4_CH3 TIM4 PWM(1) PB9 TIM4_CH4 TIM4 PWM(2) - define GPS_MAX_RECEIVERS 1 define GPS_MAX_INSTANCES 1 define HAL_COMPASS_MAX_SENSORS 1 @@ -132,6 +131,7 @@ define HAL_PERIPH_GPS_PORT_DEFAULT 3 # for ProfiLed we need RC out and notify define HAL_PERIPH_ENABLE_RC_OUT define HAL_PERIPH_ENABLE_NOTIFY +define AP_SERIALLED_ENABLED 1 define HAL_SERIAL_ESC_COMM_ENABLED 1 define HAL_RCIN_THREAD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/README.md b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/README.md index 495eca8f9b9b98..8030bd8d2ef14e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/README.md @@ -55,8 +55,8 @@ Contact dheeranlabs@gmail.com for sales | 1 | VCC | +5V | | 2 | UART_TX6 | +3.3V | | 3 | UART_RX6 | +3.3V | -| 4 | CTS | +3.3V | -| 5 | RTS | +3.3V | +| 4 | X | X | +| 5 | X | X | | 6 | GND | GND | **GPS1** @@ -70,7 +70,7 @@ Contact dheeranlabs@gmail.com for sales | 5 | I2C2_SDA | +3.3V | | 6 | GND | GND | -**GPS2** +**SERIAL5** | Pin | Signal | Volt | | :--: | :-----: | :---: | @@ -78,8 +78,17 @@ Contact dheeranlabs@gmail.com for sales | 2 | UART_TX7 | +3.3V | | 3 | UART_RX7 | +3.3V | | 4 | X | X | -| 5 | X | X | -| 6 | GND | GND | +| 5 | GND | GND | + +**SERIAL6, GPIO** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | USART_TX2| +3.3V | +| 3 | USART_RX2| +3.3V | +| 4 | GPIO | +3.3V | +| 5 | GND | GND | **SAFETY** @@ -103,7 +112,7 @@ receive pin for UARTn. The Tn pin is the transmit pin for UARTn. - SERIAL2 -> UART6 (TELEM2) with DMA Enabled - SERIAL3 -> UART1 (GPS1) Tx(NODMA), Rx(DMA Enabled) - SERIAL4 -> EMPTY - - SERIAL5 -> UART7 (GPS2) NODMA + - SERIAL5 -> UART7 (User) NODMA - SERIAL6 -> USART2 (User) NODMA ## RC Input diff --git a/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/README.md b/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/README.md index 7b5b1eeacc02c5..d77638e315fbb1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/README.md @@ -43,7 +43,7 @@ and how to use the "PWR:VBAT" jumper. - SERIAL1 -> UART1 (DMA-enabled, MSP DisplayPort OSD) - SERIAL2 -> UART2 (DMA-enabled, GPS) - SERIAL3 -> UART3 (DMA-enabled, RCin) - - SERIAL4 -> UART4 (spare) + - SERIAL4 -> UART4 (ESC telemetry) - SERIAL5 -> UART5 (spare) - SERIAL6 -> UART6 (spare, PWM 9 and 10 by default, use BRD_ALT_CONFIG = 1 for UART) - SERIAL7 -> UART7 (spare, RX is on HD connector for RC input, TX is not connected to external pad) @@ -87,29 +87,64 @@ In addition to voltage sensing, the board also has an input for an external curr ## Loading Firmware -The RADIX 2 HD uses a proprietary bootloader. To load firmware, download the firmware binary file -from the [BrainFPV website](https://www.brainfpv.com/firmware) and copy it to the USB drive -that appears when connecting the RADIX 2 HD to your computer when it is in bootloader mode -(hold the BOOT button and release when connecting to USB). +The RADIX 2 HD uses a proprietary bootloader which needs a firmware file in a custom +file format. There are several ways of obtaining the firmware file, as explained below. +Once you have obtained the file, copy it to the USB drive that appears when connecting +the RADIX 2 HD to your computer when it is in bootloader mode (hold the BOOT button and +release when connecting to USB). Once it finishes copying, safely remove the drive. +At this point the RADIX 2 HD will reboot and run the ArduPilot firmware. -Note: When using Ardupilot, it is necessary to have a microSD card inserted, without it +Note: When using ArduPilot, it is necessary to have a microSD card inserted, without it the firmware won't run. -Alternatively, you can create your own firmware file using the [BrainFPV Firmware Packer](https://github.com/BrainFPV/brainfpv_fw_packer). -To create a file for Ardupilot, install the BrainFPV Firmware Packer: +### Option 1: Download the Firmware File the BrainFPV Website - pip install git+https://github.com/BrainFPV/brainfpv_fw_packer.git +The easiest way to get firmware files for your RADIX 2 HD is to download them from the +BrainFPV website. You can do so [here](https://www.brainfpv.com/firmware). -After that, build the Copter firmware: +### Option 2: Download the Firmware From the ArduPilot Build Server + +Download the ELF file from the [ArduPilot Build Server](https://firmware.ardupilot.org/). +Make sure you download the file for the "RADIX2HD" target. For example, the ELF file for ArduCopter +is called "arducopter.elf". + +In order to use the ELF file with your RADIX 2 HD, it needs to be converted using the +[BrainFPV Firmware Packer](https://github.com/BrainFPV/brainfpv_fw_packer). This utility +is implemented in Python, so you will need a Python installation. + +If you are using Linux, use your package manager to install Python 3. If you are using +Windows, download the Python 3 installer from the [Python Website](https://www.python.org/downloads/) +and run it. When installing, make sure to select "Add Python to PATH", so you will +be able to use Python from the Windows Command Prompt. + +After installing Python, start the Command Prompt and install the BrainFPV Firmware Packer +using the following command: + + pip install https://github.com/BrainFPV/brainfpv_fw_packer/archive/main.zip + +After installing it, you can use the following command to convert the "arducopter.elf" (or other vehicle elf file) +file to a "arducopter.bin" file that can be used with the RADIX 2 HD: + + brainfpv_fw_packer.py --name arducopter --in arducopter.elf --out arducopter.bin ^ + --dev radix2hd -t firmware -b 0x90400000 -z --noheader + +### Option 3: Compile the Firmware Yourself + +If you have a working [ArduPilot build environment](https://ardupilot.org/dev/docs/building-the-code.html), +you can compile the firmware yourself and then convert it to the format needed by the BrainFPV +bootloader. You will also need the [BrainFPV Firmware Packer](https://github.com/BrainFPV/brainfpv_fw_packer) +to do so. Install it using the "pip install" command shown above. + +For Copter, build the firmware as follows: ./waf configure --board RADIX2HD ./waf copter -Finally, use the firmware packer script to create the firmware file that can be used with the +other vehicles can be built, but the RADIX 2 HD is used primarily for copter applications. +Then use the firmware packer script to create the firmware file that can be used with the BrainFPV bootloader: ./libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/pack_firmware.sh copter To use it, copy the resulting `arducopter_{VERSION}_brainfpv.bin` to the USB drive that appears -when the RADIX 2 HD is in bootloader mode. Once it finishes copying, safely remove the drive. -At this point the RADIX 2 HD will reboot and run the Copter firmware. +when the RADIX 2 HD is in bootloader mode. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c index eed51ebae3ea3e..e734179820926d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -251,6 +251,18 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + if (endpoint_id == 0) { + return linecoding.bParityType; + } + + return 0; +} #endif /** * @brief IN EP1 state. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 2add6e6c7f9bbf..79fa3446cb9b48 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -46,6 +46,7 @@ extern SerialUSBDriver SDU2; extern const SerialUSBConfig serusbcfg2; #endif //HAL_HAVE_DUAL_USB_CDC uint32_t get_usb_baud(uint16_t endpoint_id); +uint8_t get_usb_parity(uint16_t endpoint_id); #endif #define USB_DESC_MAX_STRLEN 100 void setup_usb_strings(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c index cb1a127287ca11..e3dab2bc2649d2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg_dualcdc.c @@ -315,6 +315,19 @@ uint32_t get_usb_baud(uint16_t endpoint_id) } return 0; } + +/* + get the requested usb parity. Valid if get_usb_baud() returned non-zero +*/ +uint8_t get_usb_parity(uint16_t endpoint_id) +{ + for (uint8_t i = 0; i < ARRAY_SIZE(linecoding); i++) { + if (endpoint_id == ep_index[i]) { + return linecoding[i].bParityType; + } + } + return 0; +} #endif /** * @brief IN EP1 state. diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 64747297136c06..169358f1659ecd 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -417,6 +417,7 @@ void UARTDriver::_timer_tick(void) } void UARTDriver::configure_parity(uint8_t v) { + UARTDriver::parity = v; _device->set_parity(v); } diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index ab4a67dd83fe26..be72972e269d4e 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -206,6 +206,11 @@ bool HAL_SITL::run_in_maintenance_mode() const } #endif +uint32_t HAL_SITL::get_uart_output_full_queue_count() const +{ + return _sitl_state->_serial_0_outqueue_full_count; +} + void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const { assert(callbacks); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 4ddc20b2cb6663..495405c62bfa47 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -41,6 +41,8 @@ class HAL_SITL : public AP_HAL::HAL { bool run_in_maintenance_mode() const; #endif + uint32_t get_uart_output_full_queue_count() const; + private: HALSITL::SITL_State *_sitl_state; diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 627b7c6c80c079..2d49bb606b07cd 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -188,6 +188,7 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) if (queue_length < 1024) { break; } + _serial_0_outqueue_full_count++; usleep(1000); } } diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index de3d75d657ac46..7bb4c04348f556 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -247,6 +247,10 @@ class HALSITL::SITL_State_Common { void multicast_state_open(void); void multicast_state_send(void); + // number of times we have paused the simulation for 1ms because + // the TCP queue is full: + uint32_t _serial_0_outqueue_full_count; + protected: enum vehicle_type _vehicle; diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 11a272d7524efd..4fc517d2c5931d 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -161,6 +161,7 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { // statistics uint32_t _tx_stats_bytes; uint32_t _rx_stats_bytes; + }; #endif diff --git a/libraries/AP_HAL_SITL/UART_utils.cpp b/libraries/AP_HAL_SITL/UART_utils.cpp index f0a7d06c70873c..a3dfcccc286086 100644 --- a/libraries/AP_HAL_SITL/UART_utils.cpp +++ b/libraries/AP_HAL_SITL/UART_utils.cpp @@ -73,6 +73,7 @@ void HALSITL::UARTDriver::configure_parity(uint8_t v) if (_fd < 0) { return; } + UARTDriver::parity = v; #ifdef USE_TERMIOS struct termios t; diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 142791c46b9e24..7567decf6b33ed 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -4,10 +4,6 @@ #include "vector2.h" #include "vector3.h" -#ifndef HAL_WITH_POSTYPE_DOUBLE -#define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024 -#endif - #if HAL_WITH_POSTYPE_DOUBLE typedef double postype_t; typedef Vector2d Vector2p; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 3bc2ab84e7d171..f2cc35f224f6df 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -404,7 +404,8 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c pitch_out = _cyclic_max/4500.0f; limit.pitch = true; } - } else { + } + if (_dual_mode != AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { if (roll_out < -_cyclic_max/4500.0f) { roll_out = -_cyclic_max/4500.0f; limit.roll = true; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp index 5a93212739d912..b5293de186a10b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp @@ -211,16 +211,17 @@ void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, fl // calculates servo output void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective) { - // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch - if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){ - collective = 1 - collective; - } - // Store inputs for logging + // Store inputs for logging, store col before col reversal to ensure logging comes out with the correct sign (+/-) _roll_input = roll; _pitch_input = pitch; _collective_input_scaled = collective; + // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch + if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED) { + collective = 1 - collective; + } + for (uint8_t i = 0; i < _max_num_servos; i++) { if (!_enabled[i]) { // This servo is not enabled diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 7d9e453c7548cb..5a9a963f6cf466 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -103,7 +103,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, // Calculate a composite yaw as a weighted average of the states for each model. // To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth // equal to the weighting value before it is summed. - Vector2F yaw_vector = {}; + Vector2F yaw_vector; for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]); yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]); @@ -196,15 +196,15 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx) // Generate attitude solution using simple complementary filter for the selected model // Calculate 'k' unit vector of earth frame rotated into body frame - const Vector3F k(AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]); + const Vector3F k{AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]}; // Calculate angular rate vector in rad/sec averaged across last sample interval - Vector3F ang_rate_delayed_raw = delta_angle / angle_dt; + const Vector3F ang_rate_delayed_raw { delta_angle / angle_dt }; // Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved). // During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward - Vector3F tilt_error_gyro_correction = {}; // (rad/sec) + Vector3F tilt_error_gyro_correction; // (rad/sec) if (accel_gain > 0.0f) { @@ -213,8 +213,11 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx) if (is_positive(true_airspeed)) { // Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector // NOTE: this assumes X axis is aligned with airspeed vector - Vector3F centripetal_accel_vec_bf = Vector3F(0.0f, ang_rate_delayed_raw[2] * true_airspeed, - ang_rate_delayed_raw[1] * true_airspeed); - + const Vector3F centripetal_accel_vec_bf { + 0.0f, + ang_rate_delayed_raw[2] * true_airspeed, + - ang_rate_delayed_raw[1] * true_airspeed + }; // Correct measured accel for centripetal acceleration accel -= centripetal_accel_vec_bf; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 6b26791589446f..2021b69ba2195d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -734,6 +734,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Units: m AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f), + // @Param: OPTIONS + // @DisplayName: Optional EKF behaviour + // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad + // @Bitmask: 0:JammingExpected + // @User: Advanced + AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 9d3303b40df2a3..f47f96e8ac157f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -448,6 +448,12 @@ class NavEKF3 { AP_Int8 _primary_core; // initial core number AP_Enum _log_level; // log verbosity level AP_Float _gpsVAccThreshold; // vertical accuracy threshold to use GPS as an altitude source + AP_Int32 _options; // bit mask of processing options + + // enum for processing options + enum class Options { + JammingExpected = (1<<0), + }; // Possible values for _flowUse #define FLOW_USE_NONE 0 @@ -487,6 +493,7 @@ class NavEKF3 { const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec) const float maxYawEstVelInnov = 2.0f; // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s) const uint16_t deadReckonDeclare_ms = 1000; // Time without equivalent position or velocity observation to constrain drift before dead reckoning is declared (msec) + const uint16_t gpsNoFixTimeout_ms = 2000; // Time without a fix required to reset GPS alignment checks when EK3_OPTIONS bit 0 is set (msec) // time at start of current filter update uint64_t imuSampleTime_us; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9a15c..a6b71e4a034077 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed() zero_range(&Kfusion[0], 16, 21); } - if (tasDataDelayed.allowFusion && !inhibitWindStates) { + if (tasDataDelayed.allowFusion && !inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]); Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]); } else { @@ -212,8 +212,10 @@ void NavEKF3_core::SelectTasFusion() readAirSpdData(); // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion + if (tasDataToFuse && statesInitialised && !inhibitWindStates) { FuseAirspeed(); + tasDataToFuse = false; prevTasStep_ms = imuSampleTime_ms; } } @@ -256,10 +258,6 @@ void NavEKF3_core::SelectBetaDragFusion() // we are required to correct only wind states airDataFusionWindOnly = true; } - // Fuse estimated airspeed to aid wind estimation - if (usingDefaultAirspeed) { - FuseAirspeed(); - } FuseSideslip(); prevBetaDragStep_ms = imuSampleTime_ms; } @@ -424,7 +422,7 @@ void NavEKF3_core::FuseSideslip() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]); Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index c886f48ab6328e..2f858548721ca8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -64,6 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode() PV_AidingMode != AID_NONE; if (!inhibitWindStates && !canEstimateWind) { inhibitWindStates = true; + lastAspdEstIsValid = false; updateStateIndexLim(); } else if (inhibitWindStates && canEstimateWind && (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) { @@ -77,7 +78,7 @@ void NavEKF3_core::setWindMagStateLearningMode() Vector3F tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); ftype trueAirspeedVariance; - const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); + const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed()); if (haveAirspeedMeasurement) { trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX); const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; @@ -337,8 +338,17 @@ void NavEKF3_core::setAidingMode() // Check if attitude drift has been constrained by a measurement source bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; - // check if velocity drift has been constrained by a measurement source - bool velAiding = gpsVelUsed || airSpdUsed || dragUsed || optFlowUsed || bodyOdmUsed; + // Check if velocity drift has been constrained by a measurement source + // Currently these are all the same source as will stabilise attitude because we do not currently have + // a sensor that only observes attitude + velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed; + + // Store the last valid airspeed estimate + windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed); + if (windStateIsObservable) { + lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length(); + lastAspdEstIsValid = true; + } // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; @@ -684,9 +694,11 @@ bool NavEKF3_core::setOrigin(const Location &loc) return true; } -// record a yaw reset event -void NavEKF3_core::recordYawReset() +// record all requested yaw resets completed +void NavEKF3_core::recordYawResetsCompleted() { + gpsYawResetRequest = false; + magYawResetRequest = false; yawAlignComplete = true; if (inFlight) { finalInflightYawInit = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 93e3db98d5b905..726b091b3e71b5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -100,7 +100,9 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const baro_index : selected_baro, gps_index : selected_gps, airspeed_index : getActiveAirspeed(), - source_set : frontend->sources.getPosVelYawSourceSet() + source_set : frontend->sources.getPosVelYawSourceSet(), + gps_good_to_align : gpsGoodToAlign, + wait_for_gps_checks : waitingForGpsChecks }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index e8b635e560fed1..402cfd97867bc6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -154,6 +154,15 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; + // get yaw variance from GPS speed uncertainty + const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); + const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F))); + if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) { + yawAlignGpsValidCount++; + } else { + yawAlignGpsValidCount = 0; + } + // correct yaw angle using GPS ground course if compass yaw bad if (badMagYaw) { // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches @@ -163,16 +172,6 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) return; } - // get yaw variance from GPS speed uncertainty - const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise)); - const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F))); - - if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) { - yawAlignGpsValidCount++; - } else { - yawAlignGpsValidCount = 0; - } - if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { yawAlignGpsValidCount = 0; // keep roll and pitch and reset yaw @@ -194,6 +193,10 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset) allMagSensorsFailed = false; } } + } else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) { + // There is no need to do a yaw reset + yawAlignGpsValidCount = 0; + recordYawResetsCompleted(); } } else { yawAlignGpsValidCount = 0; @@ -642,7 +645,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); } else { @@ -725,7 +728,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); } else { @@ -809,7 +812,7 @@ void NavEKF3_core::FuseMagnetometer() } // zero Kalman gains to inhibit wind state estimation - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); } else { @@ -1345,7 +1348,7 @@ void NavEKF3_core::FuseDeclination(ftype declErr) zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); } else { @@ -1579,7 +1582,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset) } // record the yaw reset event - recordYawReset(); + recordYawResetsCompleted(); // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly ResetVelocity(resetDataSource::DEFAULT); @@ -1655,10 +1658,5 @@ void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationO lastYawReset_ms = imuSampleTime_ms; // record the yaw reset event - recordYawReset(); - - // clear all pending yaw reset requests - gpsYawResetRequest = false; - magYawResetRequest = false; - + recordYawResetsCompleted(); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 1440a18a498285..b5a933645c7909 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -633,6 +633,27 @@ void NavEKF3_core::readGpsData() useGpsVertVel = false; } + if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) && + (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) { + const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000); + const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);; + const bool canDoWindRelNav = assume_zero_sideslip(); + const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav); + if (canDeadReckon) { + // If we can do dead reckoning with a data source other than GPS there is time to wait + // for GPS alignment checks to pass before using GPS inside the EKF. + // this gets set back to false in calcGpsGoodToAlign() when GPS checks pass + waitingForGpsChecks = true; + // force GPS checks to restart + lastPreAlignGpsCheckTime_ms = 0; + lastGpsVelFail_ms = imuSampleTime_ms; + lastGpsVelPass_ms = 0; + gpsGoodToAlign = false; + } else { + waitingForGpsChecks = false; + } + } + // Monitor quality of the GPS velocity data before and after alignment calcGpsGoodToAlign(); @@ -681,7 +702,8 @@ void NavEKF3_core::readGpsData() } // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin - if (validOrigin) { + // and are not waiting for GPs checks to pass + if (validOrigin && !waitingForGpsChecks) { gpsDataNew.lat = gpsloc.lat; gpsDataNew.lng = gpsloc.lng; if ((frontend->_originHgtMode & (1<<2)) == 0) { @@ -828,37 +850,52 @@ void NavEKF3_core::readAirSpdData() // we take a new reading, convert from EAS to TAS and set the flag letting other functions // know a new measurement is available - const auto *airspeed = dal.airspeed(); - if (airspeed && - (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { - tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; - timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); - tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; - tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); - tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed); - - // Correct for the average intersampling delay due to the filter update rate - tasDataNew.time_ms -= localFilterTimeStep_ms/2; - - // Save data into the buffer to be fused when the fusion time horizon catches up with it - storedTAS.push(tasDataNew); - } - - // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused - tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); - - float easErrVar = sq(MAX(frontend->_easNoise, 0.5f)); - // Allow use of a default value if enabled - if (!useAirspeed() && - imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 && - is_positive(defaultAirSpeed)) { - tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; - tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar)); - tasDataDelayed.allowFusion = true; - tasDataDelayed.time_ms = 0; - usingDefaultAirspeed = true; + if (useAirspeed()) { + const auto *airspeed = dal.airspeed(); + if (airspeed && + (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) { + tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed); + if (tasDataNew.allowFusion) { + tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; + timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); + tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; + tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); + // Correct for the average intersampling delay due to the filter update rate + tasDataNew.time_ms -= localFilterTimeStep_ms/2; + // Save data into the buffer to be fused when the fusion time horizon catches up with it + storedTAS.push(tasDataNew); + } + } + // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused + tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms); } else { - usingDefaultAirspeed = false; + if (is_positive(defaultAirSpeed)) { + // this is the preferred method with the autopilot providing a model based airspeed estimate + if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) { + tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; + tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f))); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + tasDataDelayed.time_ms = imuDataDelayed.time_ms; + } else { + tasDataToFuse = false; + tasDataDelayed.allowFusion = false; + } + } else if (lastAspdEstIsValid && !windStateIsObservable) { + // this uses the last airspeed estimated before dead reckoning started and + // wind states became unobservable + if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) { + tasDataDelayed.tas = lastAirspeedEstimate; + // this airspeed estimate has a lot of uncertainty + tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate)); + tasDataToFuse = true; + tasDataDelayed.allowFusion = true; + tasDataDelayed.time_ms = imuDataDelayed.time_ms; + } else { + tasDataToFuse = false; + tasDataDelayed.allowFusion = false; + } + } } } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index e457bf7a03e524..84dd164bac161a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -491,7 +491,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27); Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27); } else { @@ -668,7 +668,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27); Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index d8a28e15ad4f8a..e995a9e30c76ba 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -494,7 +494,8 @@ void NavEKF3_core::SelectVelPosFusion() readGpsYawData(); // get data that has now fallen behind the fusion time horizon - gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms); + gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms) && !waitingForGpsChecks; + if (gpsDataToFuse) { CorrectGPSForAntennaOffset(gpsDataDelayed); // calculate innovations and variances for reporting purposes only @@ -794,7 +795,7 @@ void NavEKF3_core::FuseVelPosNED() // if timed out or outside the specified uncertainty radius, reset to the external sensor // if velocity drift is being constrained, dont reset until gps passes quality checks const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax)); - if (posTimeout || posVarianceIsTooLarge) { + if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign)) { // reset the position to the current external sensor position ResetPosition(resetDataSource::DEFAULT); @@ -1045,7 +1046,7 @@ void NavEKF3_core::FuseVelPosNED() } // inhibit wind state estimation by setting Kalman gains to zero - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = P[22][stateIndex]*SK; Kfusion[23] = P[23][stateIndex]*SK; } else { @@ -1562,7 +1563,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24); Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24); } else { @@ -1739,7 +1740,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25); Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25); } else { @@ -1917,7 +1918,7 @@ void NavEKF3_core::FuseBodyVel() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24); Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 92a2ba4eeca5fa..f3b332dbf1e8a0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -234,7 +234,7 @@ void NavEKF3_core::FuseRngBcn() zero_range(&Kfusion[0], 16, 21); } - if (!inhibitWindStates) { + if (!inhibitWindStates && !treatWindStatesAsTruth) { Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9); Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9); } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 47e42773613fff..e6876b07a44844 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -12,7 +12,7 @@ */ void NavEKF3_core::calcGpsGoodToAlign(void) { - if (inFlight && assume_zero_sideslip() && !use_compass()) { + if (inFlight && !finalInflightYawInit && assume_zero_sideslip() && !use_compass()) { // this is a special case where a plane has launched without magnetometer // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible gpsGoodToAlign = true; @@ -230,6 +230,10 @@ void NavEKF3_core::calcGpsGoodToAlign(void) } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) { gpsGoodToAlign = false; } + + if (gpsGoodToAlign && waitingForGpsChecks) { + waitingForGpsChecks = false; + } } // update inflight calculaton that determines if GPS data is good enough for reliable navigation diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 6c750ff63a8914..008c9e2c01c7e5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -258,6 +258,8 @@ void NavEKF3_core::InitialiseVariables() PV_AidingModePrev = AID_NONE; posTimeout = true; velTimeout = true; + velAiding = false; + waitingForGpsChecks = false; memset(&faultStatus, 0, sizeof(faultStatus)); hgtRate = 0.0f; onGround = true; @@ -266,6 +268,9 @@ void NavEKF3_core::InitialiseVariables() prevInFlight = false; manoeuvring = false; inhibitWindStates = true; + windStateIsObservable = false; + treatWindStatesAsTruth = false; + lastAspdEstIsValid = false; windStatesAligned = false; inhibitDelVelBiasStates = true; inhibitDelAngBiasStates = true; @@ -1080,10 +1085,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) if (!inhibitWindStates) { const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; - if (isDragFusionDeadReckoning) { - // when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate + const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable; + if (newTreatWindStatesAsTruth) { + treatWindStatesAsTruth = true; P[23][23] = P[22][22] = 0.0f; } else { + if (treatWindStatesAsTruth) { + treatWindStatesAsTruth = false; + if (windStateIsObservable) { + // allow EKF to relearn wind states rapidly + P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX); + } + } ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); if (!tasDataDelayed.allowFusion) { // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor @@ -1941,7 +1954,11 @@ void NavEKF3_core::ConstrainVariances() } if (!inhibitWindStates) { - for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); + if (treatWindStatesAsTruth) { + P[23][23] = P[22][22] = 0.0f; + } else { + for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX); + } } else { zeroCols(P,22,23); zeroRows(P,22,23); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index eef68881dd4a63..2049e7278fad65 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -957,8 +957,8 @@ class NavEKF3_core : public NavEKF_core_common // zero attitude state covariances, but preserve variances void zeroAttCovOnly(); - // record a yaw reset event - void recordYawReset(); + // record all requested yaw resets completed + void recordYawResetsCompleted(); // record a magnetic field state reset event void recordMagReset(); @@ -1037,6 +1037,8 @@ class NavEKF3_core : public NavEKF_core_common bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out bool dragTimeout; // boolean true if drag measurements have failed for too long and have timed out bool badIMUdata; // boolean true if the bad IMU data is detected + bool velAiding; // boolean true if the velocity drift is constrained by observations + bool waitingForGpsChecks; // boolean true if the EKF should write GPS data to the buffer until quality checks have passed uint32_t badIMUdata_ms; // time stamp bad IMU data was last detected uint32_t goodIMUdata_ms; // time stamp good IMU data was last detected uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit @@ -1107,7 +1109,11 @@ class NavEKF3_core : public NavEKF_core_common ftype hgtTestRatio; // sum of squares of baro height innovation divided by fail threshold Vector3F magTestRatio; // sum of squares of magnetometer innovations divided by fail threshold ftype tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold - bool inhibitWindStates; // true when wind states and covariances are to remain constant + bool inhibitWindStates; // true when wind states and covariances should not be used + ftype lastAirspeedEstimate; // last true airspeed estimate (m/s) + bool lastAspdEstIsValid; // true when the last true airspeed estimate is valid (m/s) + bool windStateIsObservable; // true when wind states are observable from measurements. + bool treatWindStatesAsTruth; // true when wind states should be used as a truth reference bool windStatesAligned; // true when wind states have been aligned bool inhibitMagStates; // true when magnetic field states are inactive bool lastInhibitMagStates; // previous inhibitMagStates @@ -1143,7 +1149,6 @@ class NavEKF3_core : public NavEKF_core_common range_elements rangeDataDelayed;// Range finder data at the fusion time horizon tas_elements tasDataNew; // TAS data at the current time horizon tas_elements tasDataDelayed; // TAS data at the fusion time horizon - bool usingDefaultAirspeed; // true when a default airspeed is being used instead of a measured value mag_elements magDataDelayed; // Magnetometer data at the fusion time horizon gps_elements gpsDataNew; // GPS data at the current time horizon gps_elements gpsDataDelayed; // GPS data at the fusion time horizon diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 8e124462fcf0db..59ea98f9e42b5b 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -344,6 +344,8 @@ struct PACKED log_XKQ { // @Field: GI: GPS selection index // @Field: AI: airspeed selection index // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2) +// @Field: GPS_GTA: GPS good to align +// @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass struct PACKED log_XKFS { LOG_PACKET_HEADER; uint64_t time_us; @@ -353,6 +355,8 @@ struct PACKED log_XKFS { uint8_t gps_index; uint8_t airspeed_index; uint8_t source_set; + uint8_t gps_good_to_align; + uint8_t wait_for_gps_checks; }; // @LoggerMessage: XKTV @@ -439,8 +443,8 @@ struct PACKED log_XKV { { LOG_XKFM_MSG, sizeof(log_XKFM), \ "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \ { LOG_XKFS_MSG, sizeof(log_XKFS), \ - "XKFS","QBBBBBB","TimeUS,C,MI,BI,GI,AI,SS", "s#-----", "F------" , true }, \ - { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#----", "F-0000" , true }, \ + "XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \ + { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \ diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index ab0b0149a0b14e..f88cdeecae4731 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2064,7 +2064,7 @@ function baro:healthy(instance) end -- Serial ports serial = {} --- Returns the UART instance that allows connections from scripts (those with SERIALx_PROTOCOL = 28`). +-- Returns the UART instance that allows connections from scripts (those with SERIALx_PROTOCOL = 28). -- For instance = 0, returns first such UART, second for instance = 1, and so on. -- If such an instance is not found, returns nil. ---@param instance integer -- the 0-based index of the UART instance to return. diff --git a/libraries/AP_Scripting/drivers/EFI_Halo6000.lua b/libraries/AP_Scripting/drivers/EFI_Halo6000.lua index b353ff2d5d206e..c739449f2620e2 100644 --- a/libraries/AP_Scripting/drivers/EFI_Halo6000.lua +++ b/libraries/AP_Scripting/drivers/EFI_Halo6000.lua @@ -55,7 +55,7 @@ end local efi_backend = nil -- Setup EFI Parameters -assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'could not add EFI_H6K param table') +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add EFI_H6K param table') --[[ // @Param: EFI_H6K_ENABLE @@ -102,6 +102,17 @@ local EFI_H6K_TELEM_RT = bind_add_param('TELEM_RT', 4, 2) --]] local EFI_H6K_FUELTOT = bind_add_param('FUELTOT', 5, 20) +--[[ + // @Param: EFI_H6K_OPTIONS + // @DisplayName: Halo6000 options + // @Description: Halo6000 options + // @Bitmask: 0:LogAllCanPackets + // @User: Standard +--]] +local EFI_H6K_OPTIONS = bind_add_param('OPTIONS', 6, 0) + +local OPTION_LOGALLFRAMES = 0x01 + if EFI_H6K_ENABLE:get() == 0 then return end @@ -121,6 +132,20 @@ if not driver1 then return end +local frame_count = 0 + +--[[ + frame logging - can be replayed with Tools/scripts/CAN/CAN_playback.py +--]] +local function log_can_frame(frame) + logger.write("CANF",'Id,DLC,FC,B0,B1,B2,B3,B4,B5,B6,B7','IBIBBBBBBBB', + frame:id(), + frame:dlc(), + frame_count, + frame:data(0), frame:data(1), frame:data(2), frame:data(3), + frame:data(4), frame:data(5), frame:data(6), frame:data(7)) + frame_count = frame_count + 1 +end --[[ EFI Engine Object @@ -158,6 +183,10 @@ local function engine_control() break end + if EFI_H6K_OPTIONS:get() & OPTION_LOGALLFRAMES ~= 0 then + log_can_frame(frame) + end + -- All Frame IDs for this EFI Engine are in the 11-bit address space if not frame:isExtended() then self.handle_packet(frame) diff --git a/libraries/AP_Scripting/drivers/EFI_Halo6000.md b/libraries/AP_Scripting/drivers/EFI_Halo6000.md index d27d304c1592ae..f31853d5d665e8 100644 --- a/libraries/AP_Scripting/drivers/EFI_Halo6000.md +++ b/libraries/AP_Scripting/drivers/EFI_Halo6000.md @@ -31,6 +31,16 @@ control. This is the rate in Hz at which NAMED_VALUE_FLOAT messages are used to send additional telemetry data to the GCS for display to the operator. +## EFI_H6K_FUELTOT + +This is the total fuel tank capacity in litres + +## EFI_H6K_OPTIONS + +This provides additional options. Currently just one option is +available. If you set EFI_H6K_OPTIONS to 1 then all CAN frames will be +logged in the message CANF. + # Operation This driver should be loaded by placing the lua script in the diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 148757e0b49f0a..cf33c88572da4e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -1333,6 +1333,8 @@ class GCS uint32_t last_port1_data_ms; uint32_t baud1; uint32_t baud2; + uint8_t parity1; + uint8_t parity2; uint8_t timeout_s; HAL_Semaphore sem; } _passthru; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b4187b85f8910b..4645706588617a 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -377,7 +377,9 @@ bool GCS_MAVLINK::send_battery_status() for(uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) { const uint8_t battery_id = (last_battery_status_idx + 1) % AP_BATT_MONITOR_MAX_INSTANCES; - if (battery.get_type(battery_id) != AP_BattMonitor::Type::NONE) { + const auto configured_type = battery.configured_type(battery_id); + if (configured_type != AP_BattMonitor::Type::NONE && + configured_type == battery.allocated_type(battery_id)) { CHECK_PAYLOAD_SIZE(BATTERY_STATUS); send_battery_status(battery_id); last_battery_status_idx = battery_id; @@ -4092,6 +4094,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) #if AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT case MAVLINK_MSG_ID_FENCE_POINT: case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + send_received_message_deprecation_warning("FENCE_FETCH_POINT"); handle_fence_message(msg); break; #endif @@ -4173,6 +4176,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) #if AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED case MAVLINK_MSG_ID_RALLY_POINT: case MAVLINK_MSG_ID_RALLY_FETCH_POINT: + send_received_message_deprecation_warning("RALLY_FETCH_POINT"); handle_common_rally_message(msg); break; #endif @@ -6651,6 +6655,7 @@ void GCS::update_passthru(void) WITH_SEMAPHORE(_passthru.sem); uint32_t now = AP_HAL::millis(); uint32_t baud1, baud2; + uint8_t parity1 = 0, parity2 = 0; bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s, baud1, baud2); if (enabled && !_passthru.enabled) { @@ -6660,6 +6665,8 @@ void GCS::update_passthru(void) _passthru.last_port1_data_ms = now; _passthru.baud1 = baud1; _passthru.baud2 = baud2; + _passthru.parity1 = parity1 = _passthru.port1->get_parity(); + _passthru.parity2 = parity2 = _passthru.port2->get_parity(); gcs().send_text(MAV_SEVERITY_INFO, "Passthru enabled"); if (!_passthru.timer_installed) { _passthru.timer_installed = true; @@ -6678,6 +6685,13 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } gcs().send_text(MAV_SEVERITY_INFO, "Passthru disabled"); } else if (enabled && _passthru.timeout_s && @@ -6696,12 +6710,19 @@ void GCS::update_passthru(void) _passthru.port2->end(); _passthru.port2->begin(baud2); } + // Restore original parity + if (_passthru.parity1 != parity1) { + _passthru.port1->configure_parity(parity1); + } + if (_passthru.parity2 != parity2) { + _passthru.port2->configure_parity(parity2); + } gcs().send_text(MAV_SEVERITY_INFO, "Passthru timed out"); } } /* - called at 1kHz to handle pass-thru between SERIA0_PASSTHRU port and hal.console + called at 1kHz to handle pass-thru between SERIAL_PASS1 and SERIAL_PASS2 ports */ void GCS::passthru_timer(void) { @@ -6714,7 +6735,7 @@ void GCS::passthru_timer(void) if (_passthru.start_ms != 0) { uint32_t now = AP_HAL::millis(); if (now - _passthru.start_ms < 1000) { - // delay for 1s so the reply for the SERIAL0_PASSTHRU param set can be seen by GCS + // delay for 1s so the reply for the SERIAL_PASS2 param set can be seen by GCS return; } _passthru.start_ms = 0; @@ -6728,19 +6749,35 @@ void GCS::passthru_timer(void) _passthru.port1->lock_port(lock_key, lock_key); _passthru.port2->lock_port(lock_key, lock_key); - // Check for requested Baud rates over USB + // Check for requested Baud rates and parity over USB uint32_t baud = _passthru.port1->get_usb_baud(); - if (_passthru.baud2 != baud && baud != 0) { - _passthru.baud2 = baud; - _passthru.port2->end(); - _passthru.port2->begin_locked(baud, 0, 0, lock_key); + uint8_t parity = _passthru.port1->get_usb_parity(); + if (baud != 0) { // port1 is USB + if (_passthru.baud2 != baud) { + _passthru.baud2 = baud; + _passthru.port2->end(); + _passthru.port2->begin_locked(baud, 0, 0, lock_key); + } + + if (_passthru.parity2 != parity) { + _passthru.parity2 = parity; + _passthru.port2->configure_parity(parity); + } } baud = _passthru.port2->get_usb_baud(); - if (_passthru.baud1 != baud && baud != 0) { - _passthru.baud1 = baud; - _passthru.port1->end(); - _passthru.port1->begin_locked(baud, 0, 0, lock_key); + parity = _passthru.port2->get_usb_parity(); + if (baud != 0) { // port2 is USB + if (_passthru.baud1 != baud) { + _passthru.baud1 = baud; + _passthru.port1->end(); + _passthru.port1->begin_locked(baud, 0, 0, lock_key); + } + + if (_passthru.parity1 != parity) { + _passthru.parity1 = parity; + _passthru.port1->configure_parity(parity); + } } uint8_t buf[64]; diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index d369ef89af57bb..45a4949baa08e9 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -12,7 +12,10 @@ #define HAL_MAVLINK_BINDINGS_ENABLED HAL_GCS_ENABLED #endif +// CODE_REMOVAL // BATTERY2 is slated to be removed: +// ArduPilot 4.6 stops compiling support in +// ArduPilot 4.7 removes the code entirely #ifndef AP_MAVLINK_BATTERY2_ENABLED #define AP_MAVLINK_BATTERY2_ENABLED 0 #endif @@ -55,22 +58,33 @@ #define AP_MAVLINK_FAILURE_CREATION_ENABLED 1 #endif +// CODE_REMOVAL +// ArduPilot 4.6 sends deprecation warnings for RALLY_POINT/RALLY_FETCH_POINT +// ArduPilot 4.7 stops compiling them in by default +// ArduPilot 4.8 removes the code entirely #ifndef AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED #define AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED HAL_GCS_ENABLED && HAL_RALLY_ENABLED #endif +// CODE_REMOVAL // handling of HIL_GPS is slated to be removed in 4.7; GPS_INPUT can be used // in its place +// ArduPilot 4.6 stops compiling support in +// ArduPilot 4.7 removes the code entirely #ifndef AP_MAVLINK_MSG_HIL_GPS_ENABLED #define AP_MAVLINK_MSG_HIL_GPS_ENABLED 0 #endif +// CODE_REMOVAL +// ArduPilot 4.5 sends deprecation warnings for MOUNT_CONTROL/MOUNT_CONFIGURE +// ArduPilot 4.6 stops compiling them in +// ArduPilot 4.7 removes the code entirely #ifndef AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED -#define AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED HAL_GCS_ENABLED +#define AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED 0 #endif #ifndef AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED -#define AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED HAL_GCS_ENABLED +#define AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED 0 #endif // this is for both read and write messages: diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index fe074e9b454bab..4cc7284526e39d 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -32,11 +32,17 @@ #include #include #include +#include using namespace SITL; extern const AP_HAL::HAL& hal; +// the SITL HAL can add information about pausing the simulation and its effect on the UART. Not present when we're compiling for simulation-on-hardware +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +extern const HAL_SITL& hal_sitl; +#endif + /* parent class for all simulator types */ @@ -452,6 +458,14 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) } #if HAL_LOGGING_ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // the SITL HAL can add information about pausing the simulation + // and its effect on the UART. Not present when we're compiling + // for simulation-on-hardware + const uint32_t full_count = hal_sitl.get_uart_output_full_queue_count(); +#else + const uint32_t full_count = 0; +#endif // for EKF comparison log relhome pos and velocity at loop rate static uint16_t last_ticks; uint16_t ticks = AP::scheduler().ticks(); @@ -468,15 +482,20 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) // @Field: VD: Velocity down // @Field: As: Airspeed // @Field: ASpdU: Achieved simulation speedup value +// @Field: UFC: Number of times simulation paused for serial0 output Vector3d pos = get_position_relhome(); Vector3f vel = get_velocity_ef(); - AP::logger().WriteStreaming("SIM2", "TimeUS,PN,PE,PD,VN,VE,VD,As,ASpdU", - "Qdddfffff", - AP_HAL::micros64(), - pos.x, pos.y, pos.z, - vel.x, vel.y, vel.z, - airspeed_pitot, - achieved_rate_hz/rate_hz); + AP::logger().WriteStreaming( + "SIM2", + "TimeUS,PN,PE,PD,VN,VE,VD,As,ASpdU,UFC", + "QdddfffffI", + AP_HAL::micros64(), + pos.x, pos.y, pos.z, + vel.x, vel.y, vel.z, + airspeed_pitot, + achieved_rate_hz/rate_hz, + full_count + ); } #endif } @@ -954,6 +973,45 @@ void Aircraft::extrapolate_sensors(float delta_time) velocity_air_bf = dcm.transposed() * velocity_air_ef; } +bool Aircraft::Clamp::clamped(Aircraft &aircraft, const struct sitl_input &input) +{ + const auto clamp_ch = AP::sitl()->clamp_ch; + if (clamp_ch < 1) { + return false; + } + const uint32_t clamp_idx = clamp_ch - 1; + if (clamp_idx > ARRAY_SIZE(input.servos)) { + return false; + } + const uint16_t servo_pos = input.servos[clamp_idx]; + bool new_clamped = currently_clamped; + if (servo_pos < 1200) { + if (currently_clamped) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: released vehicle"); + new_clamped = false; + } + grab_attempted = false; + } else { + // re-clamp if < 10cm from home + if (servo_pos > 1800 && !grab_attempted) { + const Vector3d pos = aircraft.get_position_relhome(); + const float distance_from_home = pos.length(); + // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: dist=%f", distance_from_home); + if (distance_from_home < 0.5) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: grabbed vehicle"); + new_clamped = true; + } else if (!grab_attempted) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Clamp: missed vehicle"); + } + grab_attempted = true; + } + } + + currently_clamped = new_clamped; + + return currently_clamped; +} + void Aircraft::update_external_payload(const struct sitl_input &input) { external_payload_mass = 0; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 8a8ade5eebff56..56ed55860d1f7a 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -324,6 +324,15 @@ class Aircraft { // update EAS speeds void update_eas_airspeed(); + // clamp support + class Clamp { + public: + bool clamped(class Aircraft&, const struct sitl_input &input); // true if the vehicle is currently clamped down + private: + bool currently_clamped; + bool grab_attempted; // avoid warning multiple times about missed grab + } clamp; + private: uint64_t last_time_us; uint32_t frame_counter; diff --git a/libraries/SITL/SIM_GPS_SBP2.cpp b/libraries/SITL/SIM_GPS_SBP2.cpp index b8c75f5a1fb708..3acd391e06c9a6 100644 --- a/libraries/SITL/SIM_GPS_SBP2.cpp +++ b/libraries/SITL/SIM_GPS_SBP2.cpp @@ -92,8 +92,8 @@ void GPS_SBP2::publish(const GPS_Data *d) velned.n = 1e3 * d->speedN; velned.e = 1e3 * d->speedE; velned.d = 1e3 * d->speedD; - velned.h_accuracy = 5e3; - velned.v_accuracy = 5e3; + velned.h_accuracy = 1e3 * 0.5; + velned.v_accuracy = 1e3 * 0.5; velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; velned.flags = 1; sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index a834cc6f8567db..a866762593e05d 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -61,6 +61,11 @@ void MultiCopter::update(const struct sitl_input &input) Vector3f rot_accel; calculate_forces(input, rot_accel, accel_body); + // simulated clamp holding vehicle down + if (clamp.clamped(*this, input)) { + rot_accel.zero(); + accel_body.zero(); + } // estimate voltage and current frame->current_and_voltage(battery_voltage, battery_current); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index c9633291ce300f..9b895dc8a68643 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1184,6 +1184,11 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Description: Allow relay output operation when running SIM-on-hardware AP_GROUPINFO("OH_RELAY_MSK", 48, SIM, on_hardware_relay_enable_mask, SIM_DEFAULT_ENABLED_RELAY_CHANNELS), + // @Param: CLAMP_CH + // @DisplayName: Simulated Clamp Channel + // @Description: If non-zero the vehicle will be clamped in position until the value on this servo channel passes 1800PWM + AP_GROUPINFO("CLAMP_CH", 49, SIM, clamp_ch, 0), + // the IMUT parameters must be last due to the enable parameters #if HAL_INS_TEMPERATURE_CAL_ENABLE AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 61, SIM, AP_InertialSensor_TCal), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 718070bbc44b97..fdf3bddf5dad88 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -554,6 +554,9 @@ class SIM { // Master instance to use servos from with slave instances AP_Int8 ride_along_master; + // clamp simulation - servo channel starting at offset 1 (usually ailerons) + AP_Int8 clamp_ch; + #if AP_SIM_INS_FILE_ENABLED enum INSFileMode { INS_FILE_NONE = 0,