diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 12d037f07653c8..c711b06f5197ee 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -601,11 +601,11 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) // check throttle if (check_enabled(ARMING_CHECK_RC)) { - #if FRAME_CONFIG == HELI_FRAME +#if FRAME_CONFIG == HELI_FRAME const char *rc_item = "Collective"; - #else +#else const char *rc_item = "Throttle"; - #endif +#endif // check throttle is not too high - skips checks if arming from GCS/scripting in Guided,Guided_NoGPS or Auto if (!((AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS || copter.flightmode->mode_number() == Mode::Number::AUTO))) { // above top of deadband is too always high @@ -614,12 +614,12 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) return false; } // in manual modes throttle must be at zero - #if FRAME_CONFIG != HELI_FRAME +#if FRAME_CONFIG != HELI_FRAME if ((copter.flightmode->has_manual_throttle() || copter.flightmode->mode_number() == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) { check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item); return false; } - #endif +#endif } } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index b02628b1bd64a7..7c9a0e91b3402f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -474,13 +474,13 @@ bool Copter::update_target_location(const Location &old_loc, const Location &new #endif // AP_SCRIPTING_ENABLED -// returns true if vehicle is landing. Only used by Lua scripts +// returns true if vehicle is landing. bool Copter::is_landing() const { return flightmode->is_landing(); } -// returns true if vehicle is taking off. Only used by Lua scripts +// returns true if vehicle is taking off. bool Copter::is_taking_off() const { return flightmode->is_taking_off(); @@ -583,9 +583,11 @@ void Copter::ten_hz_logging_loop() } if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a1dd234b0cf36f..105b660a8e94ef 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -634,10 +634,12 @@ const AP_Param::Info Copter::var_info[] = { GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), - +#endif + #if AP_RANGEFINDER_ENABLED // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 3bfe085c2b8697..2685f89431be60 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -10,8 +10,10 @@ void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete || !motors->armed(); + // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(is_landing_or_landed); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached @@ -24,7 +26,7 @@ void Copter::fence_check() if (new_breaches) { if (!copter.ap.land_complete) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); } // if the user wants some kind of response and motors are armed @@ -81,7 +83,10 @@ void Copter::fence_check() LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + if (!copter.ap.land_complete) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); + } // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index ce191b0c1ef08d..19de3725dbbeba 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -347,6 +347,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return false; } +#if AP_FENCE_ENABLED + // may not be allowed to change mode if recovering from fence breach + if (!ignore_checks && + fence.enabled() && + fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && + fence.get_breaches() && + motors->armed() && + get_control_mode_reason() == ModeReason::FENCE_BREACHED && + !ap.land_complete) { + mode_change_failed(new_flightmode, "in fence recovery"); + return false; + } +#endif + if (!new_flightmode->init(ignore_checks)) { mode_change_failed(new_flightmode, "init failed"); return false; @@ -371,10 +385,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #endif #if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); + if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { + // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover + // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) + // but it should be harmless to disable the fence temporarily in these situations as well + fence.manual_recovery_start(); + } #endif #if AP_CAMERA_ENABLED diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 721cef3f0ad213..25b1b0702770af 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1541,6 +1541,10 @@ class ModeSmartRTL : public ModeRTL { // point while following our path home. If we take too long we // may choose to land the vehicle. uint32_t path_follow_last_pop_fail_ms; + + // backup last popped point so that it can be restored to the path + // if vehicle exits SmartRTL mode before reaching home. invalid if zero + Vector3f dest_NED_backup; }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3ac943f06dbc58..4c4ab35ac27631 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -488,11 +488,6 @@ void ModeAuto::land_start() copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; @@ -787,18 +782,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) // point the camera to a specified angle do_mount_control(cmd); break; - - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - copter.fence.enable(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - copter.fence.enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif //AP_FENCE_ENABLED - break; #if AC_NAV_GUIDED == ENABLED case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a771c0fdaa0efa..f22501e47fb70f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -685,6 +685,9 @@ void ModeGuided::takeoff_run() auto_takeoff.run(); if (auto_takeoff.complete && !takeoff_complete) { takeoff_complete = true; +#if AP_FENCE_ENABLED + copter.fence.auto_enable_fence_after_takeoff(); +#endif #if AP_LANDINGGEAR_ENABLED // optionally retract landing gear copter.landinggear.retract_after_takeoff(); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 1f34e20fbf3380..2b5b6e8879260f 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -41,11 +41,6 @@ bool ModeLand::init(bool ignore_checks) copter.landinggear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif - #if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index d3250e4265642f..cfee04ff720247 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -255,11 +255,6 @@ void ModeRTL::descent_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } // rtl_descent_run - implements the final descent to the RTL_ALT @@ -347,11 +342,6 @@ void ModeRTL::land_start() // optionally deploy landing gear copter.landinggear.deploy_for_landing(); #endif - -#if AP_FENCE_ENABLED - // disable the fence on landing - copter.fence.auto_disable_fence_for_landing(); -#endif } bool ModeRTL::is_landing() const diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index dacd7cc5f29ab0..8c2b1988c30895 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -35,6 +35,14 @@ bool ModeSmartRTL::init(bool ignore_checks) // perform cleanup required when leaving smart_rtl void ModeSmartRTL::exit() { + // restore last point if we hadn't reached it + if (smart_rtl_state == SubMode::PATH_FOLLOW && !dest_NED_backup.is_zero()) { + if (!g2.smart_rtl.add_point(dest_NED_backup)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SmartRTL: lost one point"); + } + } + dest_NED_backup.zero(); + g2.smart_rtl.cancel_request_for_thorough_cleanup(); } @@ -83,10 +91,16 @@ void ModeSmartRTL::path_follow_run() { // if we are close to current target point, switch the next point to be our target. if (wp_nav->reached_wp_destination()) { - Vector3f dest_NED; + + // clear destination backup so that it cannot be restored + dest_NED_backup.zero(); + // this pop_point can fail if the IO task currently has the // path semaphore. + Vector3f dest_NED; if (g2.smart_rtl.pop_point(dest_NED)) { + // backup destination in case we exit smart_rtl mode and need to restore it to the path + dest_NED_backup = dest_NED; path_follow_last_pop_fail_ms = 0; if (g2.smart_rtl.get_num_points() == 0) { // this is the very last point, add 2m to the target alt and move to pre-land state diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 864b1569f8bb00..1d133932fa40a3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -27,9 +27,11 @@ void Copter::init_ardupilot() // initialise battery monitor battery.init(); +#if AP_RSSI_ENABLED // Init RSSI rssi.init(); - +#endif + barometer.init(); // setup telem slots with serial ports diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 855b9a62413a19..5dcfcdfa72e6c4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -841,6 +841,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } +#if AP_FENCE_ENABLED + // reject destination if outside the fence + if (!plane.fence.check_destination_within_fence(requested_position)) { + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + return MAV_RESULT_DENIED; + } +#endif + // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index a73c6dc552ce4a..2fdfcd618bba18 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -255,9 +255,11 @@ void Plane::Log_Write_RC(void) { logger.Write_RCIN(); logger.Write_RCOUT(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif Log_Write_AETR(); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 941dcba774ac08..65198d9cb53240 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -296,6 +296,14 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(airspeed_max, "AIRSPEED_MAX", AIRSPEED_FBW_MAX), + // @Param: AIRSPEED_STALL + // @DisplayName: Stall airspeed + // @Description: If stall prevention is enabled this speed is used to calculate the minimum airspeed while banking. If this is set to 0 then the stall speed is assumed to be the minimum airspeed speed. Typically set slightly higher then true stall speed. Value is as an indicated (calibrated/apparent) airspeed. + // @Units: m/s + // @Range: 5 75 + // @User: Standard + ASCALAR(airspeed_stall, "AIRSPEED_STALL", 0), + // @Param: FBWB_ELEV_REV // @DisplayName: Fly By Wire elevator reverse // @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude. @@ -967,9 +975,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(rpm_sensor, "RPM", AP_RPM), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ba89c457728112..de514b891532cc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -343,6 +343,7 @@ class Parameters { k_param_mixing_offset, k_param_dspoiler_rud_rate, + k_param_airspeed_stall, k_param_logger = 253, // Logging Group diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b1af208397ff30..443f2d56a1fb4b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -250,6 +250,9 @@ class Plane : public AP_Vehicle { // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached bool long_failsafe_pending; + + //flag that we have already called autoenable fences once in MODE TAKEOFF + bool have_autoenabled_fences; // GCS selection GCS_Plane _gcs; // avoid using this; use gcs() diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 4766cea5c1fbae..57969f17ce8a4b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -144,21 +144,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_LAND_START: break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { // disable fence - plane.fence.enable(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled"); - } else if (cmd.p1 == 1) { // enable fence - plane.fence.enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled"); - } else if (cmd.p1 == 2) { // disable fence floor only - plane.fence.disable_floor(); - gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled"); - } -#endif - break; - case MAV_CMD_DO_AUTOTUNE_ENABLE: autotune_enable(cmd.p1); break; @@ -434,10 +419,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // if we were in an abort we need to explicitly move out of the abort state, as it's sticky set_flight_stage(AP_FixedWing::FlightStage::LAND); } - -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 33062460df6b0e..1890e709d25b0c 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -8,9 +8,32 @@ void Plane::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); + const bool armed = arming.is_armed(); + + uint16_t mission_id = plane.mission.get_current_nav_cmd().id; + bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND + || !armed +#if HAL_QUADPLANE_ENABLED + || control_mode->mode_number() == Mode::Number::QLAND + || quadplane.in_vtol_land_descent() +#endif + || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(landing_or_landed); + + /* + if we are either disarmed or we are currently not in breach and + we are not flying then clear the state associated with the + previous mode breach handling. This allows the fence state + machine to reset at the end of a fence breach action such as an + RTL and autoland + */ + if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) { + if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) { + plane.previous_mode_reason = ModeReason::UNKNOWN; + } + } if (!fence.enabled()) { // Switch back to the chosen control mode if still in @@ -34,7 +57,7 @@ void Plane::fence_check() // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached // that's not ever going to be true if we don't call check on AP_Fence while disarmed - if (!arming.is_armed()) { + if (!armed) { return; } @@ -50,7 +73,7 @@ void Plane::fence_check() } if (new_breaches) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + fence.print_fence_message("breached", new_breaches); // if the user wants some kind of response and motors are armed const uint8_t fence_act = fence.get_action(); @@ -115,7 +138,8 @@ void Plane::fence_check() } LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index 4205960f871c30..f3a08aabde7622 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -16,9 +16,6 @@ bool ModeQLand::_enter() #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif return true; } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 37d0203fcf413f..0854e512450a94 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -63,6 +63,7 @@ ModeTakeoff::ModeTakeoff() : bool ModeTakeoff::_enter() { takeoff_mode_setup = false; + plane.have_autoenabled_fences = false; return true; } @@ -154,7 +155,10 @@ void ModeTakeoff::update() } else { if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering #if AP_FENCE_ENABLED - plane.fence.auto_enable_fence_after_takeoff(); + if (!plane.have_autoenabled_fences) { + plane.fence.auto_enable_fence_after_takeoff(); + plane.have_autoenabled_fences = true; + } #endif plane.calc_nav_roll(); plane.calc_nav_pitch(); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 05a79484577301..c0c69937fa9e4c 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3554,9 +3554,6 @@ bool QuadPlane::verify_vtol_land(void) poscontrol.pilot_correction_done = false; pos_control->set_lean_angle_max_cd(0); poscontrol.xy_correction.zero(); -#if AP_FENCE_ENABLED - plane.fence.auto_disable_fence_for_landing(); -#endif #if AP_LANDINGGEAR_ENABLED plane.g2.landing_gear.deploy_for_landing(); #endif diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index d931ac158d62fa..dafe5e8bd7e1c7 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -45,7 +45,9 @@ void Plane::init_ardupilot() // initialise battery monitoring battery.init(); +#if AP_RSSI_ENABLED rssi.init(); +#endif #if AP_RPM_ENABLED rpm_sensor.init(); diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 1273636c2662cf..83f8e6207394b0 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -165,9 +165,11 @@ void Blimp::ten_hz_logging_loop() } if (should_log(MASK_LOG_RCIN)) { logger.Write_RCIN(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } if (should_log(MASK_LOG_RCOUT)) { logger.Write_RCOUT(); diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 91a1cc017ed0aa..7a1d34c239f36b 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -382,9 +382,11 @@ const AP_Param::Info Blimp::var_info[] = { GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3), #endif +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/Blimp/system.cpp b/Blimp/system.cpp index e32fa10a04c3d8..9c1df237c82659 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -21,8 +21,10 @@ void Blimp::init_ardupilot() // initialise battery monitor battery.init(); +#if AP_RSSI_ENABLED // Init RSSI rssi.init(); +#endif barometer.init(); diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6189b2d281ef9c..e718273703ce62 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -142,7 +142,12 @@ void GCS_MAVLINK_Rover::send_servo_out() 0, 0, 0, - receiver_rssi()); +#if AP_RSSI_ENABLED + receiver_rssi() +#else + 255 +#endif + ); } int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 6a3edb0184767e..3490e2a703d33d 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -237,9 +237,11 @@ void Rover::Log_Write_RC(void) { logger.Write_RCIN(); logger.Write_RCOUT(); +#if AP_RSSI_ENABLED if (rssi.enabled()) { logger.Write_RSSI(); } +#endif } void Rover::Log_Write_Vehicle_Startup_Messages() diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 49c1f9a0be6378..0c740105e5c4f5 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -358,9 +358,11 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), +#if AP_RSSI_ENABLED // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp GOBJECT(rssi, "RSSI_", AP_RSSI), +#endif // @Group: NTF_ // @Path: ../libraries/AP_Notify/AP_Notify.cpp diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index fa1ea8ece212c0..6e97b0a1d24a35 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -593,18 +593,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_set_reverse(cmd); break; - case MAV_CMD_DO_FENCE_ENABLE: -#if AP_FENCE_ENABLED - if (cmd.p1 == 0) { //disable - rover.fence.enable(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - rover.fence.enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } -#endif - break; - case MAV_CMD_DO_GUIDED_LIMITS: do_guided_limits(cmd); break; diff --git a/Rover/system.cpp b/Rover/system.cpp index 451df4614e0923..02b93ab644f86c 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -18,7 +18,9 @@ void Rover::init_ardupilot() rpm_sensor.init(); #endif +#if AP_RSSI_ENABLED rssi.init(); +#endif g2.windvane.init(serial_manager); diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 79fdb759b03ac1..5c1efea998d3ea 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -74,7 +74,7 @@ int main(void) flash_init(); -#ifdef STM32H7 +#if defined(STM32H7) && CH_CFG_USE_HEAP check_ecc_errors(); #endif diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 171f4f1249e107..ed687c3241f95c 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -312,6 +312,8 @@ AP_HW_LongbowF405 1422 AP_HW_MountainEagleH743 1444 +AP_HW_StellarF4 1500 + AP_HW_SakuraRC-H743 2714 # IDs 4200-4220 reserved for HAKRC diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 0026ffc9c5bbc5..80c6c0bb02d344 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -747,7 +747,7 @@ bool can_check_update(void) bool ret = false; #if HAL_RAM_RESERVE_START >= 256 struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; - if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC) { + if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC && comms->my_node_id != 0) { can_set_node_id(comms->my_node_id); fw_update.node_id = comms->server_node_id; for (uint8_t i=0; iexpect_delay_ms(100); hal.scheduler->delay(40); + hal.scheduler->expect_delay_ms(0); } /* diff --git a/Tools/Frame_params/ModalAI/D0013.parm b/Tools/Frame_params/ModalAI/D0013.parm new file mode 100644 index 00000000000000..1d3fcf65139cb0 --- /dev/null +++ b/Tools/Frame_params/ModalAI/D0013.parm @@ -0,0 +1,109 @@ +# parameters for the Stinger drone + +# flight modes +FLTMODE1 5 +FLTMODE4 2 +FLTMODE_CH 6 + +# enable PID logging +LOG_BITMASK 65535 + +# mag field varies quite a lot between batteries +ARMING_MAGTHRESH 200 + +# IMU orientation +# 4 = YAW 180 +AHRS_ORIENTATION 4 + +# Forced external compass +COMPASS_EXTERNAL 2 +# compass orientation +# 2 = YAW 90 +COMPASS_ORIENT 8 + +# filtering +INS_GYRO_FILTER 40 +INS_HNTCH_ENABLE 1 +INS_HNTCH_ATT 40.0 +INS_HNTCH_BW 50.0 +INS_HNTCH_FM_RAT 1.0 +INS_HNTCH_FREQ 100.0 +INS_HNTCH_HMNCS 7 +INS_HNTCH_MODE 3 +INS_HNTCH_OPTS 2 +INS_HNTCH_REF 1.0 + +# run IMU at 2kHz +INS_GYRO_RATE 1 + +# a bit more agressive loiter +PILOT_SPEED_UP 500 +LOIT_BRK_ACCEL 500 +LOIT_BRK_JERK 1000 +LOIT_BRK_DELAY 0.200000 + +# Tune parameters from Quik Tune +ATC_ACCEL_Y_MAX 30000.0 +ATC_ACCEL_R_MAX 125000.0 +ATC_ACCEL_P_MAX 125000.0 +ATC_RAT_PIT_D 0.002856164472177625 +ATC_RAT_PIT_FLTD 40.0 +ATC_RAT_PIT_FLTT 40.0 +ATC_RAT_PIT_I 0.124929137527942657 +ATC_RAT_PIT_P 0.124929137527942657 +ATC_RAT_PIT_SMAX 50.0 +ATC_RAT_RLL_D 0.001519999350421131 +ATC_RAT_RLL_FLTD 40.0 +ATC_RAT_RLL_FLTT 40.0 +ATC_RAT_RLL_I 0.075611755251884460 +ATC_RAT_RLL_P 0.075611755251884460 +ATC_RAT_RLL_SMAX 50.0 +ATC_RAT_YAW_D 0.010000008158385754 +ATC_RAT_YAW_FLTD 40.0 +ATC_RAT_YAW_FLTE 2.0 +ATC_RAT_YAW_FLTT 40.0 +ATC_RAT_YAW_I 0.050000030547380447 +ATC_RAT_YAW_P 0.500000298023223877 +ATC_RAT_YAW_SMAX 50.0 + +# battery setup +BATT_LOW_VOLT 14.2 +BATT_OPTIONS 64 +BATT_VOLT_PIN 1 +BATT_CURR_PIN 2 +BATT_VOLT_MULT 1 +BATT_AMP_PERVLT 1 +BATT_AMP_OFFSET 0.0 + +# 4S battery range +MOT_BAT_VOLT_MAX 16.800000 +MOT_BAT_VOLT_MIN 13.200000 + +# Learned hover thrust +MOT_THST_EXPO 0.550000011920928955 +MOT_THST_HOVER 0.130549192428588867 + +# quad-X +FRAME_CLASS 1 + +# tweak R/C inputs +RC1_MIN 1000 +RC1_MAX 2000 +RC1_DZ 40 +RC2_MIN 1000 +RC2_MAX 2000 +RC2_REVERSED 1 +RC3_MIN 1000 +RC3_MAX 2000 +RC4_MIN 1000 +RC4_MAX 2000 +RC4_DZ 40 + +# add arming on right switch +RC7_OPTION 153 + +# Motor mappings +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 36 +SERVO3_FUNCTION 34 +SERVO4_FUNCTION 35 diff --git a/Tools/autotest/Generic_Missions/CMAC-fence.txt b/Tools/autotest/Generic_Missions/CMAC-fence.txt new file mode 100644 index 00000000000000..a6169e4e3cbcf7 --- /dev/null +++ b/Tools/autotest/Generic_Missions/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 21efa3ef5fac15..17b500367509fc 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -961,21 +961,6 @@ def CustomController(self, timeout=300): # Tests all actions and logic behind the battery failsafe def BatteryFailsafe(self, timeout=300): '''Fly Battery Failsafe''' - self.context_push() - ex = None - try: - self.test_battery_failsafe(timeout=timeout) - except Exception as e: - self.print_exception_caught(e) - self.disarm_vehicle(force=True) - ex = e - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex - - def test_battery_failsafe(self, timeout=300): self.progress("Configure battery failsafe parameters") self.set_parameters({ 'SIM_SPEEDUP': 4, @@ -1454,7 +1439,7 @@ def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_beha self.wait_altitude(10, 100, relative=True) self.set_rc(3, 1500) self.set_rc(2, 1400) - self.wait_distance_to_home(12, 20) + self.wait_distance_to_home(12, 20, timeout=30) tstart = self.get_sim_time() push_time = 70 # push against barrier for 60 seconds failed_max = False @@ -1513,7 +1498,7 @@ def HorizontalFence(self, timeout=180): self.load_fence("fence-in-middle-of-nowhere.txt") self.delay_sim_time(5) # let fence check run so it loads-from-eeprom - self.assert_prearm_failure("vehicle outside fence") + self.assert_prearm_failure("vehicle outside Polygon fence") self.progress("Failed to arm outside fence (good!)") self.clear_fence() self.delay_sim_time(5) # let fence breach clear @@ -1523,7 +1508,7 @@ def HorizontalFence(self, timeout=180): self.start_subtest("ensure we can't arm with bad radius") self.context_push() self.set_parameter("FENCE_RADIUS", -1) - self.assert_prearm_failure("Invalid FENCE_RADIUS value") + self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value") self.context_pop() self.progress("Failed to arm with bad radius") self.drain_mav() @@ -1605,6 +1590,7 @@ def MaxAltFence(self): "FENCE_ENABLE": 1, "AVOID_ENABLE": 0, "FENCE_TYPE": 1, + "FENCE_ENABLE" : 1, }) self.change_alt(10) @@ -1692,6 +1678,7 @@ def MinAltFence(self): # enable fence, disable avoidance self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 8, "FENCE_ALT_MIN": 20, }) @@ -1728,6 +1715,59 @@ def MinAltFence(self): self.zero_throttle() + # MinAltFenceAvoid - fly down and make sure fence action does not trigger + # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance + def MinAltFenceAvoid(self): + '''Test Min Alt Fence Avoidance''' + self.takeoff(30, mode="LOITER") + """Hold loiter position.""" + + # enable fence, only min altitude + # No action, rely on avoidance to prevent the breach + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 8, + "FENCE_ALT_MIN": 20, + "FENCE_ACTION": 0, + }) + + # Try and fly past the fence + self.set_rc(3, 1120) + + # Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts + try: + self.wait_altitude(10, 15, timeout=90, relative=True) + raise NotAchievedException("Avoid should prevent reaching altitude") + except AutoTestTimeoutException: + pass + except Exception as e: + raise e + + # Check ascent is not too fast, allow 10% above the configured backup speed + max_ascent_rate = self.get_parameter("AVOID_BACKUP_SPD") * 1.1 + + def get_climb_rate(mav, m): + m_type = m.get_type() + if m_type != 'VFR_HUD': + return + if m.climb > max_ascent_rate: + raise NotAchievedException("Ascending too fast want %f got %f" % (max_ascent_rate, m.climb)) + + self.context_push() + self.install_message_hook_context(get_climb_rate) + + # Reduce fence alt, this will result in a fence breach, but there is no action. + # Avoid should then backup the vehicle to be over the new fence alt. + self.set_parameters({ + "FENCE_ALT_MIN": 30, + }) + self.wait_altitude(30, 40, timeout=90, relative=True) + + self.context_pop() + + self.set_rc(3, 1500) + self.do_RTL() + def FenceFloorEnabledLanding(self): """Ensures we can initiate and complete an RTL while the fence is enabled. @@ -1737,15 +1777,16 @@ def FenceFloorEnabledLanding(self): self.progress("Test Landing while fence floor enabled") self.set_parameters({ "AVOID_ENABLE": 0, + "FENCE_ENABLE" : 1, "FENCE_TYPE": 15, - "FENCE_ALT_MIN": 10, - "FENCE_ALT_MAX": 20, + "FENCE_ALT_MIN": 20, + "FENCE_ALT_MAX": 30, }) self.change_mode("GUIDED") self.wait_ready_to_arm() self.arm_vehicle() - self.user_takeoff(alt_min=15) + self.user_takeoff(alt_min=25) # Check fence is enabled self.do_fence_enable() @@ -1757,16 +1798,109 @@ def FenceFloorEnabledLanding(self): self.set_rc(3, 1800) self.wait_mode('RTL', timeout=120) - self.wait_landed_and_disarmed() + # center throttle + self.set_rc(3, 1500) + + # wait until we are below the fence floor and re-enter loiter + self.wait_altitude(5, 15, relative=True) + self.change_mode('LOITER') + # wait for manual recovery to expire + self.delay_sim_time(15) + + # lower throttle and try and land + self.set_rc(3, 1300) + self.wait_altitude(0, 2, relative=True) + self.wait_disarmed() self.assert_fence_enabled() - # Assert fence is not healthy + # Assert fence is not healthy since it was enabled manually self.assert_sensor_state(fence_bit, healthy=False) # Disable the fence using mavlink command to ensure cleaned up SITL state self.do_fence_disable() self.assert_fence_disabled() + def FenceFloorAutoDisableLanding(self): + """Ensures we can initiate and complete an RTL while the fence is enabled""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.progress("Test Landing while fence floor enabled") + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 1, + }) + + self.change_mode("GUIDED") + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + def FenceFloorAutoEnableOnArming(self): + """Ensures we can auto-enable fences on arming and still takeoff and land""" + + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "AVOID_ENABLE": 0, + "FENCE_TYPE": 11, + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 20, + "FENCE_AUTOENABLE" : 3, + }) + + self.change_mode("GUIDED") + # Check fence is not enabled + self.assert_fence_disabled() + + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(alt_min=15, mode="GUIDED") + + # Check fence is enabled + self.assert_fence_enabled() + + # Change to RC controlled mode + self.change_mode('LOITER') + + self.set_rc(3, 1800) + + self.wait_mode('RTL', timeout=120) + # Assert fence is not healthy now that we are in RTL + self.assert_sensor_state(fence_bit, healthy=False) + + self.wait_landed_and_disarmed(0) + # the breach should have cleared since we auto-disable the + # fence on landing + self.assert_fence_disabled() + + # Assert fences have gone now that we have landed and disarmed + self.assert_sensor_state(fence_bit, present=True, enabled=False) + + # Disable the fence using mavlink command to ensure cleaned up SITL state + self.assert_fence_disabled() + def GPSGlitchLoiter(self, timeout=30, max_distance=20): """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" @@ -2266,46 +2400,36 @@ def MagFail(self): def ModeFlip(self): '''Fly Flip Mode''' - ex = None - try: - self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100) + self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100) - self.takeoff(20) - self.hover() - old_speedup = self.get_parameter("SIM_SPEEDUP") - self.set_parameter('SIM_SPEEDUP', 1) - self.progress("Flipping in roll") - self.set_rc(1, 1700) - self.send_cmd_do_set_mode('FLIP') # don't wait for success - self.wait_attitude(despitch=0, desroll=45, tolerance=30) - self.wait_attitude(despitch=0, desroll=90, tolerance=30) - self.wait_attitude(despitch=0, desroll=-45, tolerance=30) - self.progress("Waiting for level") - self.set_rc(1, 1500) # can't change quickly enough! - self.wait_attitude(despitch=0, desroll=0, tolerance=5) - - self.progress("Regaining altitude") - self.change_mode('ALT_HOLD') - self.wait_altitude(19, 60, relative=True) - - self.progress("Flipping in pitch") - self.set_rc(2, 1700) - self.send_cmd_do_set_mode('FLIP') # don't wait for success - self.wait_attitude(despitch=45, desroll=0, tolerance=30) - # can't check roll here as it flips from 0 to -180.. - self.wait_attitude(despitch=90, tolerance=30) - self.wait_attitude(despitch=-45, tolerance=30) - self.progress("Waiting for level") - self.set_rc(2, 1500) # can't change quickly enough! - self.wait_attitude(despitch=0, desroll=0, tolerance=5) - self.set_parameter('SIM_SPEEDUP', old_speedup) - self.do_RTL() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 0) - if ex is not None: - raise ex + self.takeoff(20) + + self.progress("Flipping in roll") + self.set_rc(1, 1700) + self.send_cmd_do_set_mode('FLIP') # don't wait for success + self.wait_attitude(despitch=0, desroll=45, tolerance=30) + self.wait_attitude(despitch=0, desroll=90, tolerance=30) + self.wait_attitude(despitch=0, desroll=-45, tolerance=30) + self.progress("Waiting for level") + self.set_rc(1, 1500) # can't change quickly enough! + self.wait_attitude(despitch=0, desroll=0, tolerance=5) + + self.progress("Regaining altitude") + self.change_mode('ALT_HOLD') + self.wait_altitude(19, 60, relative=True) + + self.progress("Flipping in pitch") + self.set_rc(2, 1700) + self.send_cmd_do_set_mode('FLIP') # don't wait for success + self.wait_attitude(despitch=45, desroll=0, tolerance=30) + # can't check roll here as it flips from 0 to -180.. + self.wait_attitude(despitch=90, tolerance=30) + self.wait_attitude(despitch=-45, tolerance=30) + self.progress("Waiting for level") + self.set_rc(2, 1500) # can't change quickly enough! + self.wait_attitude(despitch=0, desroll=0, tolerance=5) + + self.do_RTL() def configure_EKFs_to_use_optical_flow_instead_of_GPS(self): '''configure EKF to use optical flow instead of GPS''' @@ -3591,82 +3715,68 @@ def InvalidJumpTags(self): def GPSViconSwitching(self): """Fly GPS and Vicon switching test""" - self.customise_SITL_commandline(["--serial5=sim:vicon:"]) - """Setup parameters including switching to EKF3""" - self.context_push() - ex = None - try: - self.set_parameters({ - "VISO_TYPE": 2, # enable vicon - "SERIAL5_PROTOCOL": 2, - "EK3_ENABLE": 1, - "EK3_SRC2_POSXY": 6, # External Nav - "EK3_SRC2_POSZ": 6, # External Nav - "EK3_SRC2_VELXY": 6, # External Nav - "EK3_SRC2_VELZ": 6, # External Nav - "EK3_SRC2_YAW": 6, # External Nav - "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align - "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector - "EK2_ENABLE": 0, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() - - # switch to use GPS - self.set_rc(8, 1000) + self.set_parameters({ + "VISO_TYPE": 2, # enable vicon + "SERIAL5_PROTOCOL": 2, + "EK3_ENABLE": 1, + "EK3_SRC2_POSXY": 6, # External Nav + "EK3_SRC2_POSZ": 6, # External Nav + "EK3_SRC2_VELXY": 6, # External Nav + "EK3_SRC2_VELZ": 6, # External Nav + "EK3_SRC2_YAW": 6, # External Nav + "RC7_OPTION": 80, # RC aux switch 7 set to Viso Align + "RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) - # ensure we can get a global position: - self.poll_home_position(timeout=120) + # switch to use GPS + self.set_rc(8, 1000) - # record starting position - old_pos = self.get_global_position_int() - print("old_pos=%s" % str(old_pos)) + # ensure we can get a global position: + self.poll_home_position(timeout=120) - # align vicon yaw with ahrs heading - self.set_rc(7, 2000) + # record starting position + old_pos = self.get_global_position_int() + print("old_pos=%s" % str(old_pos)) - # takeoff to 10m in Loiter - self.progress("Moving to ensure location is tracked") - self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) + # align vicon yaw with ahrs heading + self.set_rc(7, 2000) - # fly forward in Loiter - self.set_rc(2, 1300) + # takeoff to 10m in Loiter + self.progress("Moving to ensure location is tracked") + self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720) - # disable vicon - self.set_parameter("SIM_VICON_FAIL", 1) + # fly forward in Loiter + self.set_rc(2, 1300) - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") + # disable vicon + self.set_parameter("SIM_VICON_FAIL", 1) - # re-enable vicon - self.set_parameter("SIM_VICON_FAIL", 0) + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter - self.set_rc(8, 1500) - self.set_parameter("GPS1_TYPE", 0) + # re-enable vicon + self.set_parameter("SIM_VICON_FAIL", 0) - # ensure vehicle remain in Loiter for 15 seconds - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 15: - if not self.mode_is('LOITER'): - raise NotAchievedException("Expected to stay in loiter for >15 seconds") + # switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter + self.set_rc(8, 1500) + self.set_parameter("GPS1_TYPE", 0) - # RTL and check vehicle arrives within 10m of home - self.set_rc(2, 1500) - self.do_RTL() + # ensure vehicle remain in Loiter for 15 seconds + tstart = self.get_sim_time() + while self.get_sim_time() - tstart < 15: + if not self.mode_is('LOITER'): + raise NotAchievedException("Expected to stay in loiter for >15 seconds") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle(force=True) - self.reboot_sitl() - if ex is not None: - raise ex + # RTL and check vehicle arrives within 10m of home + self.set_rc(2, 1500) + self.do_RTL() def RTLSpeed(self): """Test RTL Speed parameters""" @@ -3746,9 +3856,6 @@ def NavDelay(self): def RangeFinder(self): '''Test RangeFinder Basic Functionality''' - ex = None - self.context_push() - self.progress("Making sure we don't ordinarily get RANGEFINDER") m = self.mav.recv_match(type='RANGEFINDER', blocking=True, @@ -3764,68 +3871,59 @@ def RangeFinder(self): if self.current_onboard_log_contains_message("RFND"): raise NotAchievedException("Found unexpected RFND message") - try: - self.set_analog_rangefinder_parameters() - self.set_parameter("RC9_OPTION", 10) # rangefinder - self.set_rc(9, 2000) - - self.reboot_sitl() + self.set_analog_rangefinder_parameters() + self.set_parameter("RC9_OPTION", 10) # rangefinder + self.set_rc(9, 2000) - self.progress("Making sure we now get RANGEFINDER messages") - m = self.assert_receive_message('RANGEFINDER', timeout=10) + self.reboot_sitl() - self.progress("Checking RangeFinder is marked as enabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") - self.progress("Disabling laser using switch") - self.set_rc(9, 1000) - self.delay_sim_time(1) - self.progress("Checking RangeFinder is marked as disabled in mavlink") - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser enabled in SYS_STATUS") + self.progress("Making sure we now get RANGEFINDER messages") + m = self.assert_receive_message('RANGEFINDER', timeout=10) - self.progress("Re-enabling rangefinder") - self.set_rc(9, 2000) - self.delay_sim_time(1) - m = self.mav.recv_match(type='SYS_STATUS', - blocking=True, - timeout=10) - flags = m.onboard_control_sensors_enabled - if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: - raise NotAchievedException("Laser not enabled in SYS_STATUS") + self.progress("Checking RangeFinder is marked as enabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") + self.progress("Disabling laser using switch") + self.set_rc(9, 1000) + self.delay_sim_time(1) + self.progress("Checking RangeFinder is marked as disabled in mavlink") + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser enabled in SYS_STATUS") - self.takeoff(10, mode="LOITER") + self.progress("Re-enabling rangefinder") + self.set_rc(9, 2000) + self.delay_sim_time(1) + m = self.mav.recv_match(type='SYS_STATUS', + blocking=True, + timeout=10) + flags = m.onboard_control_sensors_enabled + if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION: + raise NotAchievedException("Laser not enabled in SYS_STATUS") - m_r = self.mav.recv_match(type='RANGEFINDER', - blocking=True) - m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) + self.takeoff(10, mode="LOITER") - if abs(m_r.distance - m_p.relative_alt/1000) > 1: - raise NotAchievedException( - "rangefinder/global position int mismatch %0.2f vs %0.2f" % - (m_r.distance, m_p.relative_alt/1000)) + m_r = self.mav.recv_match(type='RANGEFINDER', + blocking=True) + m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT', + blocking=True) - self.land_and_disarm() + if abs(m_r.distance - m_p.relative_alt/1000) > 1: + raise NotAchievedException( + "rangefinder/global position int mismatch %0.2f vs %0.2f" % + (m_r.distance, m_p.relative_alt/1000)) - if not self.current_onboard_log_contains_message("RFND"): - raise NotAchievedException("Did not see expected RFND message") + self.land_and_disarm() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex + if not self.current_onboard_log_contains_message("RFND"): + raise NotAchievedException("Did not see expected RFND message") def SplineTerrain(self): '''Test Splines and Terrain''' @@ -5255,6 +5353,7 @@ def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1): def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True): tstart = self.get_sim_time() success_start = 0 + while True: now = self.get_sim_time_cached() if now - tstart > timeout: @@ -5265,9 +5364,9 @@ def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, despitch = self.constrained_mount_pitch(despitch) '''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS''' - mount_roll, mount_pitch, mount_yaw = self.get_mount_roll_pitch_yaw_deg() + mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() - self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) + # self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw)) if abs(despitch - mount_pitch) > despitch_tolerance: self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" % (mount_pitch, despitch, despitch_tolerance)) @@ -5323,10 +5422,11 @@ def get_mount_roll_pitch_yaw_deg(self): # wait for gimbal attitude message m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + yaw_is_absolute = m.flags & mavutil.mavlink.GIMBAL_DEVICE_FLAGS_YAW_LOCK # convert quaternion to euler angles and return q = quaternion.Quaternion(m.q) euler = q.euler - return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]) + return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), yaw_is_absolute def set_mount_mode(self, mount_mode): '''set mount mode''' @@ -5343,9 +5443,9 @@ def set_mount_mode(self, mount_mode): p3=0, # stabilize pitch (unsupported) ) - def test_mount_rc_targetting(self): + def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True): '''called in multipleplaces to make sure that mount RC targetting works''' - try: + if True: self.context_push() self.set_parameters({ 'RC6_OPTION': 0, @@ -5405,6 +5505,13 @@ def test_mount_rc_targetting(self): self.set_rc(12, 1500) + if do_rate_tests: + self.test_mount_rc_targetting_rate_control() + + self.context_pop() + + def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500): + if True: self.progress("Testing RC rate control") self.set_parameter('MNT1_RC_RATE', 10) self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5427,46 +5534,21 @@ def test_mount_rc_targetting(self): self.set_rc(12, 1500) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.context_pop() - - except Exception as e: - self.print_exception_caught(e) - self.context_pop() - raise e - - def Mount(self): - '''Test Camera/Antenna Mount''' - ex = None - self.context_push() - old_srcSystem = self.mav.mav.srcSystem - self.mav.mav.srcSystem = 250 - self.set_parameter("DISARM_DELAY", 0) - try: - '''start by disabling GCS failsafe, otherwise we immediately disarm - due to (apparently) not receiving traffic from the GCS for - too long. This is probably a function of --speedup''' - self.set_parameter("FS_GCS_ENABLE", 0) - - # setup mount parameters - self.setup_servo_mount() - self.reboot_sitl() # to handle MNT_TYPE changing - + def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True): + '''Test Camera/Antenna Mount - assumes a camera is set up and ready to go''' + if True: # make sure we're getting gimbal device attitude status - self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5) + self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True) # change mount to neutral mode (point forward, not stabilising) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - # test pitch is not stabilising - mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg() + # test pitch is not neutral to start with + mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: - raise NotAchievedException("Mount stabilising when not requested") + raise NotAchievedException("Mount not neutral") - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - - self.user_takeoff() + self.takeoff(30, mode='GUIDED') # pitch vehicle back and confirm gimbal is still not stabilising despitch = 10 @@ -5478,13 +5560,13 @@ def Mount(self): self.wait_pitch(despitch, despitch_tolerance) # check gimbal is still not stabilising - mount_roll_deg, mount_pitch_deg, mount_yaw_deg = self.get_mount_roll_pitch_yaw_deg() + mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0: raise NotAchievedException("Mount stabilising when not requested") # center RC tilt control and change mount to RC_TARGETING mode self.progress("Gimbal to RC Targetting mode") - self.set_rc(6, 1500) + self.set_rc(6, pitch_rc_neutral) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # pitch vehicle back and confirm gimbal is stabilising @@ -5530,7 +5612,10 @@ def Mount(self): self.progress("Testing mount RC targetting") self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) - self.test_mount_rc_targetting() + self.test_mount_rc_targetting( + pitch_rc_neutral=pitch_rc_neutral, + do_rate_tests=do_rate_tests, + ) self.progress("Testing mount ROI behaviour") self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) @@ -5668,28 +5753,65 @@ def Mount(self): 0, # vz 0 # heading ) - self.test_mount_pitch(68, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2) + self.test_mount_pitch( + 68, + 5, + mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, + hold=2, + constrained=constrain_sysid_target, + ) self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.disarm_vehicle(force=True) - self.mav.mav.srcSystem = old_srcSystem - self.disarm_vehicle(force=True) + self.test_mount_body_yaw() - self.context_pop() + def test_mount_body_yaw(self): + '''check reporting of yaw''' + # change mount to neutral mode (point forward, not stabilising) + self.takeoff(10, mode='GUIDED') - self.reboot_sitl() # to handle MNT1_TYPE changing + self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) - if ex is not None: - raise ex + for heading in 30, 45, 150: + self.guided_achieve_heading(heading) + + r, p , y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() + + if yaw_is_absolute: + raise NotAchievedException("Expected a relative yaw") + + if y > 1: + raise NotAchievedException("Bad yaw (y=%f)") + + self.do_RTL() + + def Mount(self): + '''test servo mount''' + self.setup_servo_mount() + self.reboot_sitl() # to handle MNT_TYPE changing + self.mount_test_body() + + def MountSolo(self): + '''test type=2, a "Solo" mount''' + self.set_parameters({ + "MNT1_TYPE": 2, + "RC6_OPTION": 213, # MOUNT1_PITCH + }) + self.customise_SITL_commandline([ + "--gimbal" # connects on port 5762 + ]) + self.mount_test_body( + pitch_rc_neutral=1818, + do_rate_tests=False, # solo can't do rate control (yet?) + constrain_sysid_target=False, # not everything constrains all angles + ) def assert_mount_rpy(self, r, p, y, tolerance=1): '''assert mount atttiude in degrees''' - got_r, got_p, got_y = self.get_mount_roll_pitch_yaw_deg() + got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg() for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"): if abs(want - got) > tolerance: raise NotAchievedException("%s incorrect; want=%f got=%f" % @@ -7617,6 +7739,54 @@ def AC_Avoidance_Fence(self): self.set_parameter("FENCE_ENABLE", 1) self.check_avoidance_corners() + def AvoidanceAltFence(self): + '''Test fence avoidance at minimum and maximum altitude''' + ex = None + try: + self.set_parameters({ + "FENCE_ENABLE": 1, + "FENCE_TYPE": 9, # min and max alt fence + "FENCE_ALT_MIN": 10, + "FENCE_ALT_MAX": 30, + }) + + self.change_mode('LOITER') + self.wait_ekf_happy() + + tstart = self.get_sim_time() + self.takeoff(15, mode='LOITER') + self.progress("Increasing throttle, vehicle should stay below 30m") + self.set_rc(3, 1920) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt > 30: + raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),)) + + self.progress("Decreasing, vehicle should stay above 10m") + self.set_rc(3, 1080) + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 20: + break + alt = self.get_altitude(relative=True) + self.progress("Altitude %s" % alt) + if alt < 10: + raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),)) + + except Exception as e: + self.progress("Caught exception: %s" % + self.get_exception_stacktrace(e)) + ex = e + self.land_and_disarm() + self.disarm_vehicle(force=True) + if ex is not None: + raise ex + def ModeFollow(self): '''Fly follow mode''' foll_ofs_x = 30 # metres @@ -10495,6 +10665,7 @@ def tests1c(self): self.AC_Avoidance_Proximity_AVOID_ALT_MIN, self.AC_Avoidance_Fence, self.AC_Avoidance_Beacon, + self.AvoidanceAltFence, self.BaroWindCorrection, self.SetpointGlobalPos, self.ThrowDoubleDrop, @@ -10513,7 +10684,10 @@ def tests1d(self): self.MaxAltFence, self.MaxAltFenceAvoid, self.MinAltFence, + self.MinAltFenceAvoid, self.FenceFloorEnabledLanding, + self.FenceFloorAutoDisableLanding, + self.FenceFloorAutoEnableOnArming, self.AutoTuneSwitch, self.GPSGlitchLoiter, self.GPSGlitchLoiter2, @@ -11630,6 +11804,58 @@ def AutoContinueOnRCFailsafe(self): self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450) self.do_RTL() + def MissionRTLYawBehaviour(self): + '''check end-of-mission yaw behaviour''' + self.set_parameters({ + "AUTO_OPTIONS": 3, + }) + + self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint-except-RTL") + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.change_mode('AUTO') + self.wait_ready_to_arm() + original_heading = self.get_heading() + if abs(original_heading) < 5: + raise NotAchievedException(f"Bad original heading {original_heading}") + self.arm_vehicle() + self.wait_current_waypoint(3) + self.wait_rtl_complete() + self.wait_disarmed() + if abs(self.get_heading()) > 5: + raise NotAchievedException("Should have yaw zero without option") + + # must change out of auto and back in again to reset state machine: + self.change_mode('LOITER') + self.change_mode('AUTO') + + self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint") + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 20, 20), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameters({ + "WP_YAW_BEHAVIOR": 1, # look at next waypoint (including in RTL) + }) + self.change_mode('AUTO') + self.wait_ready_to_arm() + original_heading = self.get_heading() + if abs(original_heading) > 1: + raise NotAchievedException("Bad original heading") + self.arm_vehicle() + self.wait_current_waypoint(3) + self.wait_rtl_complete() + self.wait_disarmed() + new_heading = self.get_heading() + if abs(new_heading - original_heading) > 5: + raise NotAchievedException(f"Should return to original heading want={original_heading} got={new_heading}") + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -11688,6 +11914,7 @@ def tests2b(self): # this block currently around 9.5mins here self.TerrainDBPreArm, self.ThrottleGainBoost, self.ScriptMountPOI, + self.MountSolo, self.FlyMissionTwice, self.FlyMissionTwiceWithReset, self.MissionIndexValidity, @@ -11724,6 +11951,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GripperReleaseOnThrustLoss, self.REQUIRE_POSITION_FOR_ARMING, self.LoggingFormat, + self.MissionRTLYawBehaviour, ]) return ret @@ -11799,7 +12027,6 @@ def tests(self): def disabled_tests(self): return { "Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702", - "HorizontalAvoidFence": "See https://github.com/ArduPilot/ardupilot/issues/11525", "AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191", "GroundEffectCompensation_takeOffExpected": "Flapping", "GroundEffectCompensation_touchDownExpected": "Flapping", diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index e57294be1121f0..6f90634470327e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1601,7 +1601,7 @@ def FenceStatic(self): self.delay_sim_time(2) # Allow breach to propagate self.assert_fence_enabled() - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Min Alt fence") self.do_fence_disable() self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) @@ -1620,7 +1620,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -1640,7 +1640,7 @@ def FenceStatic(self): self.do_fence_enable() self.assert_fence_enabled() self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside fence") + self.try_arm(False, "vehicle outside Polygon fence") self.do_fence_disable() self.clear_fence() @@ -3698,6 +3698,421 @@ def FenceAltCeilFloor(self): self.fly_home_land_and_disarm(timeout=150) + def FenceMinAltAutoEnable(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 9, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # check we can takeoff again + for i in [1, 2]: + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + # max alt fence should now be enabled + if i == 1: + self.assert_fence_enabled() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + self.fly_home_land_and_disarm() + self.change_mode("FBWA") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + + def FenceMinAltEnableAutoland(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to min alt and max alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 20, + "FENCE_AUTOENABLE": 0, + "FENCE_ENABLE" : 1, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # switch to FBWA + self.change_mode("FBWA") + self.set_rc(3, 1500) # raise throttle + self.wait_altitude(25, 35, timeout=50, relative=True) + self.set_rc(3, 1000) # lower throttle + # Now check we can land + self.fly_home_land_and_disarm() + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.set_current_waypoint(0, check_afterwards=False) + self.set_rc(3, 1000) # lower throttle + + def FenceMinAltAutoEnableAbort(self): + '''Tests autoenablement of the alt min fence and fences on arming''' + self.set_parameters({ + "FENCE_TYPE": 8, # Set fence type to min alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 25, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 3, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + # min alt fence should now be enabled + self.assert_fence_enabled() + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + self.load_generic_mission("flaps.txt") + self.change_mode("AUTO") + self.wait_distance_to_waypoint(8, 100, 10000000) + self.set_current_waypoint(8) + # abort the landing + self.wait_altitude(10, 20, timeout=200, relative=True) + self.change_mode("CRUISE") + self.set_rc(2, 1200) + # self.set_rc(3, 1600) # raise throttle + self.wait_altitude(30, 40, timeout=200, relative=True) + # min alt fence should now be re-enabled + self.assert_fence_enabled() + + self.change_mode("AUTO") + self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) + self.fly_home_land_and_disarm(timeout=150) + self.wait_disarmed() + + def FenceAutoEnableDisableSwitch(self): + '''Tests autoenablement of regular fences and manual disablement''' + self.set_parameters({ + "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_ALT_MIN": 50, + "FENCE_ALT_MAX": 100, + "FENCE_AUTOENABLE": 2, + "FENCE_OPTIONS" : 1, + "FENCE_ENABLE" : 1, + "FENCE_RADIUS" : 300, + "FENCE_RET_ALT" : 0, + "FENCE_RET_RALLY" : 0, + "FENCE_TOTAL" : 0, + "TKOFF_ALT" : 75, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + + self.wait_ready_to_arm() + cruise_alt = 75 + self.takeoff(cruise_alt, mode='TAKEOFF') + + self.progress("Fly above ceiling and check there is no breach") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + cruise_alt + 80) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + + self.progress("Fly below floor and check for no breach") + self.change_altitude(self.homeloc.alt + 25) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence Ceiling breached") + + self.progress("Fly above floor and check fence is not re-enabled") + self.set_rc(3, 2000) + self.change_altitude(self.homeloc.alt + 75) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence Ceiling re-enabled") + + self.progress("Return to cruise alt") + self.set_rc(3, 1500) + self.change_altitude(self.homeloc.alt + cruise_alt) + self.fly_home_land_and_disarm(timeout=250) + + def FenceCircleExclusionAutoEnable(self): + '''Tests autolanding when alt min fence is enabled''' + self.set_parameters({ + "FENCE_TYPE": 2, # Set fence type to circle + "FENCE_ACTION": 1, # Set action to RTL + "FENCE_AUTOENABLE": 2, + "FENCE_ENABLE" : 0, + "RTL_AUTOLAND" : 2, + }) + + # Grab Home Position + self.mav.recv_match(type='HOME_POSITION', blocking=True) + self.homeloc = self.mav.location() + + fence_loc = self.home_position_as_mav_location() + self.location_offset_ne(fence_loc, 300, 0) + + self.upload_fences_from_locations([( + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, { + "radius" : 100, + "loc" : fence_loc + } + )]) + + self.wait_ready_to_arm() + self.arm_vehicle() + + self.takeoff(alt=50, mode='TAKEOFF') + self.change_mode("FBWA") + self.set_rc(3, 1100) # lower throttle + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # Now check we can land + self.fly_home_land_and_disarm() + + def FenceEnableDisableSwitch(self): + '''Tests enablement and disablement of fences on a switch''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + self.set_parameters({ + "FENCE_TYPE": 4, # Set fence type to polyfence + "FENCE_ACTION": 6, # Set action to GUIDED + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : 0, + "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality + }) + + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("FBWA") + + self.load_fence("CMAC-fence.txt") + + self.set_rc_from_map({ + 3: 1500, + 7: 2000, + }) # Turn fence on with aux function + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for GUIDED") + tstart = self.get_sim_time() + mode = "GUIDED" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + + self.set_rc_from_map({ + 7: 1000, + }) # Turn fence off with aux function + + # wait to no longer be in breach + self.delay_sim_time(5) + self.assert_fence_disabled() + + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + + def FenceEnableDisableAux(self): + '''Tests enablement and disablement of fences via aux command''' + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + enable = 0 + self.set_parameters({ + "FENCE_TYPE": 12, # Set fence type to polyfence + AltMin + "FENCE_ALT_MIN": 10, + "FENCE_ENABLE" : enable, + }) + + if not enable: + self.progress("Checking fence is not present before being configured") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Got (%s)" % str(m)) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence enabled before being configured") + + self.load_fence("CMAC-fence.txt") + + self.wait_ready_to_arm() + # takeoff at a lower altitude to avoid immediately breaching polyfence + self.takeoff(alt=25) + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Checking fence is initially OK") + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, + present=True, + enabled=True, + healthy=True, + verbose=False, + timeout=30) + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + # check we are in breach + self.assert_fence_enabled() + self.assert_fence_sys_status(True, True, False) + + # wait until we get home + self.wait_distance_to_home(50, 100, timeout=200) + # now check we are now not in breach + self.assert_fence_sys_status(True, True, True) + # Turn fence off with aux function + self.run_auxfunc( + 11, + 0, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + # switch back to cruise + self.change_mode("CRUISE") + self.wait_distance(150, accuracy=20) + + # re-enable the fences + self.run_auxfunc( + 11, + 2, + want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED + ) + + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + self.progress("Got (%s)" % str(m)) + if m is None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + + self.progress("Waiting for RTL") + tstart = self.get_sim_time() + mode = "RTL" + while not self.mode_is(mode, drain_mav=False): + self.mav.messages['HEARTBEAT'].custom_mode + self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( + self.mav.flightmode, mode, self.get_altitude(relative=True))) + if (self.get_sim_time_cached() > tstart + 120): + raise WaitModeTimeout("Did not change mode") + self.progress("Got mode %s" % mode) + + # wait to no longer be in breach + self.wait_distance_to_home(50, 100, timeout=200) + self.assert_fence_sys_status(True, True, True) + + # fly home and land with fences still enabled + self.fly_home_land_and_disarm(timeout=250) + self.do_fence_disable() # Ensure the fence is disabled after test + def FenceBreachedChangeMode(self): '''Tests manual mode change after fence breach, as set with FENCE_OPTIONS''' """ Attempts to change mode while a fence is breached. @@ -5447,6 +5862,13 @@ def tests(self): self.FenceRTLRally, self.FenceRetRally, self.FenceAltCeilFloor, + self.FenceMinAltAutoEnable, + self.FenceMinAltEnableAutoland, + self.FenceMinAltAutoEnableAbort, + self.FenceAutoEnableDisableSwitch, + Test(self.FenceCircleExclusionAutoEnable, speedup=20), + self.FenceEnableDisableSwitch, + self.FenceEnableDisableAux, self.FenceBreachedChangeMode, self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, diff --git a/Tools/autotest/default_params/rover-omni3mecanum.parm b/Tools/autotest/default_params/rover-omni3mecanum.parm new file mode 100644 index 00000000000000..3b5bfb1feb9fa8 --- /dev/null +++ b/Tools/autotest/default_params/rover-omni3mecanum.parm @@ -0,0 +1,25 @@ +ACRO_TURN_RATE 300.000000 +ATC_SPEED_P 0.200000 +ATC_SPEED_D 0.000010 +ATC_STR_RAT_D 0.000010 +ATC_STR_RAT_FF 0.100000 +ATC_STR_RAT_I 0.010000 +ATC_STR_RAT_MAX 360.000000 +ATC_STR_RAT_P 0.030000 +CRUISE_SPEED 1.0 +CRUISE_THROTTLE 42.0 +FRAME_TYPE 4 +PSC_VEL_P 0.500000 +RC2_MAX 2000 +RC2_MIN 1000 +RC4_MAX 2000 +RC4_MIN 1000 +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO2_MAX 2000 +SERVO2_MIN 1000 +SERVO3_FUNCTION 35 +WP_PIVOT_RATE 120.0 +WP_SPEED 1.0 + + diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index 3682dbb01ac116..ca07c51b8f19df 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/Tools/autotest/pysim/vehicleinfo.py @@ -391,6 +391,11 @@ def __init__(self): "default_params_filename": ["default_params/rover.parm", "default_params/rover-skid.parm"], }, + "rover-omni3mecanum": { + "waf_target": "bin/ardurover", + "default_params_filename": ["default_params/rover.parm", + "default_params/rover-omni3mecanum.parm"], + }, "rover-vectored": { "waf_target": "bin/ardurover", "default_params_filename": ["default_params/rover.parm", @@ -407,6 +412,12 @@ def __init__(self): "default_params_filename": ["default_params/rover.parm", "default_params/motorboat.parm"], }, + "motorboat-skid": { + "waf_target": "bin/ardurover", + "default_params_filename": ["default_params/rover.parm", + "default_params/motorboat.parm", + "default_params/rover-skid.parm"], + }, "sailboat": { "waf_target": "bin/ardurover", "default_params_filename": ["default_params/rover.parm", diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 8e62d685be7a77..b11920ead0361d 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -86,86 +86,76 @@ def get_stick_arming_channel(self): def DriveSquare(self, side=50): """Learn/Drive Square with Ch7 option""" - self.context_push() - ex = None - try: - self.progress("TEST SQUARE") - self.set_parameters({ - "RC7_OPTION": 7, - "RC9_OPTION": 58, - }) + self.progress("TEST SQUARE") + self.set_parameters({ + "RC7_OPTION": 7, + "RC9_OPTION": 58, + }) - self.change_mode('MANUAL') + self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.arm_vehicle() + self.wait_ready_to_arm() + self.arm_vehicle() - self.clear_wp(9) - - # first aim north - self.progress("\nTurn right towards north") - self.reach_heading_manual(10) - # save bottom left corner of box as home AND waypoint - self.progress("Save HOME") - self.save_wp() - - self.progress("Save WP") - self.save_wp() - - # pitch forward to fly north - self.progress("\nGoing north %u meters" % side) - self.reach_distance_manual(side) - # save top left corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # roll right to fly east - self.progress("\nGoing east %u meters" % side) - self.reach_heading_manual(100) - self.reach_distance_manual(side) - # save top right corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # pitch back to fly south - self.progress("\nGoing south %u meters" % side) - self.reach_heading_manual(190) - self.reach_distance_manual(side) - # save bottom right corner of square as waypoint - self.progress("Save WP") - self.save_wp() - - # roll left to fly west - self.progress("\nGoing west %u meters" % side) - self.reach_heading_manual(280) - self.reach_distance_manual(side) - # save bottom left corner of square (should be near home) as waypoint - self.progress("Save WP") - self.save_wp() - - self.progress("Checking number of saved waypoints") - mavproxy = self.start_mavproxy() - num_wp = self.save_mission_to_file_using_mavproxy( - mavproxy, - os.path.join(testdir, "ch7_mission.txt")) - self.stop_mavproxy(mavproxy) - expected = 7 # home + 6 toggled in - if num_wp != expected: - raise NotAchievedException("Did not get %u waypoints; got %u" % - (expected, num_wp)) + self.clear_wp(9) + + # first aim north + self.progress("\nTurn right towards north") + self.reach_heading_manual(10) + # save bottom left corner of box as home AND waypoint + self.progress("Save HOME") + self.save_wp() + + self.progress("Save WP") + self.save_wp() + + # pitch forward to fly north + self.progress("\nGoing north %u meters" % side) + self.reach_distance_manual(side) + # save top left corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # roll right to fly east + self.progress("\nGoing east %u meters" % side) + self.reach_heading_manual(100) + self.reach_distance_manual(side) + # save top right corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # pitch back to fly south + self.progress("\nGoing south %u meters" % side) + self.reach_heading_manual(190) + self.reach_distance_manual(side) + # save bottom right corner of square as waypoint + self.progress("Save WP") + self.save_wp() + + # roll left to fly west + self.progress("\nGoing west %u meters" % side) + self.reach_heading_manual(280) + self.reach_distance_manual(side) + # save bottom left corner of square (should be near home) as waypoint + self.progress("Save WP") + self.save_wp() + + self.progress("Checking number of saved waypoints") + mavproxy = self.start_mavproxy() + num_wp = self.save_mission_to_file_using_mavproxy( + mavproxy, + os.path.join(testdir, "ch7_mission.txt")) + self.stop_mavproxy(mavproxy) + expected = 7 # home + 6 toggled in + if num_wp != expected: + raise NotAchievedException("Did not get %u waypoints; got %u" % + (expected, num_wp)) - # TODO: actually drive the mission + # TODO: actually drive the mission - self.clear_wp(9) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.clear_wp(9) self.disarm_vehicle() - self.context_pop() - - if ex: - raise ex def drive_left_circuit(self): """Drive a left circuit, 50m on a side.""" @@ -4061,6 +4051,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE, target_system=target_system, target_component=target_component) + self.set_parameters({ + "FENCE_TYPE": 2, # circle only + }) self.delay_sim_time(5) # let breaches clear # FIXME: should we allow this? self.progress("Ensure we can arm with no poly in place") @@ -4068,6 +4061,9 @@ def test_poly_fence_noarms(self, target_system=1, target_component=1): self.wait_ready_to_arm() self.arm_vehicle() self.disarm_vehicle() + self.set_parameters({ + "FENCE_TYPE": 6, # polyfence + circle + }) self.test_poly_fence_noarms_exclusion_circle(target_system=target_system, target_component=target_component) @@ -5586,65 +5582,55 @@ def send_obstacle_distances_expect_distance_sensor_messages(self, obstacle_dista def AP_Proximity_MAV(self): '''Test MAV proximity backend''' - self.context_push() - ex = None - try: - self.set_parameters({ - "PRX1_TYPE": 2, # AP_Proximity_MAV - "OA_TYPE": 2, # dijkstra - "OA_DB_OUTPUT": 3, # send all items - }) - self.reboot_sitl() - - # 1 laser pointing straight forward: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [234], - "increment_f": 10, - "angle_offset": 0.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 234}, - ]) - - # 5 lasers at front of vehicle, spread over 40 degrees: - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": [111, 222, 333, 444, 555], - "increment_f": 10, - "angle_offset": -20.0, - "min_distance": 0, - "max_distance": 1000, # cm - }, [ - {"orientation": 0, "distance": 111}, - ]) - - # lots of dense readings (e.g. vision camera: - distances = [0] * 72 - for i in range(0, 72): - distances[i] = 1000 + 10*abs(36-i) - - self.send_obstacle_distances_expect_distance_sensor_messages( - { - "distances": distances, - "increment_f": 90/72.0, - "angle_offset": -45.0, - "min_distance": 0, - "max_distance": 2000, # cm - }, [ - {"orientation": 0, "distance": 1000}, - {"orientation": 1, "distance": 1190}, - {"orientation": 7, "distance": 1190}, - ]) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "PRX1_TYPE": 2, # AP_Proximity_MAV + "OA_TYPE": 2, # dijkstra + "OA_DB_OUTPUT": 3, # send all items + }) self.reboot_sitl() - if ex is not None: - raise ex + + # 1 laser pointing straight forward: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [234], + "increment_f": 10, + "angle_offset": 0.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 234}, + ]) + + # 5 lasers at front of vehicle, spread over 40 degrees: + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": [111, 222, 333, 444, 555], + "increment_f": 10, + "angle_offset": -20.0, + "min_distance": 0, + "max_distance": 1000, # cm + }, [ + {"orientation": 0, "distance": 111}, + ]) + + # lots of dense readings (e.g. vision camera: + distances = [0] * 72 + for i in range(0, 72): + distances[i] = 1000 + 10*abs(36-i) + + self.send_obstacle_distances_expect_distance_sensor_messages( + { + "distances": distances, + "increment_f": 90/72.0, + "angle_offset": -45.0, + "min_distance": 0, + "max_distance": 2000, # cm + }, [ + {"orientation": 0, "distance": 1000}, + {"orientation": 1, "distance": 1190}, + {"orientation": 7, "distance": 1190}, + ]) def SendToComponents(self): '''Test ArduPilot send_to_components function''' @@ -6289,8 +6275,6 @@ def StickMixingAuto(self): def AutoDock(self): '''Test automatic docking of rover for multiple FOVs of simulated beacon''' - self.context_push() - self.set_parameters({ "PLND_ENABLED": 1, "PLND_TYPE": 4, @@ -6316,40 +6300,27 @@ def AutoDock(self): }) for type in range(0, 3): # CYLINDRICAL FOV, CONICAL FOV, SPHERICAL FOV - ex = None - try: - self.set_parameter("SIM_PLD_TYPE", type) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - initial_position = self.offset_location_ne(target, -20, -2) - self.drive_to_location(initial_position) - self.change_mode(8) # DOCK mode - max_delta = 1 - self.wait_distance_to_location(target, 0, max_delta, timeout=180) - self.disarm_vehicle() - self.assert_receive_message('GLOBAL_POSITION_INT') - new_pos = self.mav.location() - delta = abs(self.get_distance(target, new_pos) - stopping_dist) - self.progress("Docked %f metres from stopping point" % delta) - if delta > max_delta: - raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) - - if not self.current_onboard_log_contains_message("PL"): - raise NotAchievedException("Did not see expected PL message") - - except Exception as e: - self.print_exception_caught(e) - ex = e - break - - self.context_pop() + self.set_parameter("SIM_PLD_TYPE", type) + self.reboot_sitl() + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + initial_position = self.offset_location_ne(target, -20, -2) + self.drive_to_location(initial_position) + self.change_mode(8) # DOCK mode + max_delta = 1 + self.wait_distance_to_location(target, 0, max_delta, timeout=180) + self.disarm_vehicle() + self.assert_receive_message('GLOBAL_POSITION_INT') + new_pos = self.mav.location() + delta = abs(self.get_distance(target, new_pos) - stopping_dist) + self.progress("Docked %f metres from stopping point" % delta) + if delta > max_delta: + raise NotAchievedException("Did not dock close enough to stopping point (%fm > %fm" % (delta, max_delta)) - if ex is not None: - raise ex + if not self.current_onboard_log_contains_message("PL"): + raise NotAchievedException("Did not see expected PL message") - self.reboot_sitl() self.progress("All done") def PrivateChannel(self): @@ -6875,10 +6846,10 @@ def MissionPolyEnabledPreArm(self): ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.progress("Ensure we can arm when a polyfence fence is cleared when we've previously been in breach") self.clear_fence() @@ -6894,7 +6865,7 @@ def MissionPolyEnabledPreArm(self): ]), ]) self.reboot_sitl() - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.clear_fence() self.wait_ready_to_arm() @@ -6908,11 +6879,11 @@ def MissionPolyEnabledPreArm(self): self.offset_location_ne(here, 50, 20), # tl, ]), ]) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) self.set_parameter('FENCE_TYPE', 2) self.wait_ready_to_arm() self.set_parameter('FENCE_TYPE', 6) - self.assert_prearm_failure('vehicle outside fence', other_prearm_failures_fatal=False, timeout=120) + self.assert_prearm_failure('vehicle outside Polygon fence', other_prearm_failures_fatal=False, timeout=120) def OpticalFlow(self): '''lightly test OpticalFlow''' diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index ddb506b58518a2..f3458fee9d9575 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2080,6 +2080,8 @@ def roundtrip_fence_using_fencepoint_protocol(self, loc_list, target_system=1, t def load_fence(self, filename): filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) self.progress("Loading fence from (%s)" % str(filepath)) locs = [] for line in open(filepath, 'rb'): @@ -2384,11 +2386,6 @@ def test_adsb_send_threatening_adsb_message(self, here, offset_ne=None): def get_sim_parameter_documentation_get_whitelist(self): # common parameters ret = set([ - "SIM_ACC1_RND", - "SIM_ACC2_RND", - "SIM_ACC3_RND", - "SIM_ACC4_RND", - "SIM_ACC5_RND", "SIM_ACC_FILE_RW", "SIM_ACC_TRIM_X", "SIM_ACC_TRIM_Y", @@ -6999,22 +6996,17 @@ def do_set_relay_mavproxy(self, relay_num, on_off): self.mavproxy.expect("Loaded module relay") self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off)) - def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if value: - p1 = 1 - else: - p1 = 0 - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1=p1, # param1 - want_result=want_result, - ) - def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(True, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, want_result=want_result) def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(False, want_result=want_result) + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, want_result=want_result) + + def do_fence_disable_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=0, p2=8, want_result=want_result) + + def do_fence_enable_except_floor(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, p1=1, p2=7, want_result=want_result) ################################################# # WAIT UTILITIES diff --git a/Tools/bootloaders/CubePilot-PPPGW_bl.bin b/Tools/bootloaders/CubePilot-PPPGW_bl.bin index 785e416ab5b6b1..7d19a35945cbd8 100755 Binary files a/Tools/bootloaders/CubePilot-PPPGW_bl.bin and b/Tools/bootloaders/CubePilot-PPPGW_bl.bin differ diff --git a/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin b/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin index 660e745e57f220..cddd322e408474 100755 Binary files a/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin and b/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin differ diff --git a/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin b/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin index 1275617bf45113..9776ee4968e0a6 100755 Binary files a/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin and b/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin differ diff --git a/Tools/environment_install/install-prereqs-arch.sh b/Tools/environment_install/install-prereqs-arch.sh index 394d9ae3b8d3f8..8bba1d360876b2 100755 --- a/Tools/environment_install/install-prereqs-arch.sh +++ b/Tools/environment_install/install-prereqs-arch.sh @@ -26,7 +26,7 @@ BASE_PKGS="base-devel ccache git gsfonts tk wget gcc" SITL_PKGS="python-pip python-setuptools python-wheel python-wxpython opencv python-numpy python-scipy" PX4_PKGS="lib32-glibc zip zlib ncurses" -PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy==3.3.4 dronecan setuptools wheel" +PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy==3.3.4 dronecan packaging setuptools wheel" # GNU Tools for ARM Embedded Processors # (see https://launchpad.net/gcc-arm-embedded/) diff --git a/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh b/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh index 5895dbdeb5b2be..acc86a9f4090cb 100755 --- a/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh +++ b/Tools/environment_install/install-prereqs-openSUSE-Tumbleweed.sh @@ -105,7 +105,7 @@ if ! grep -Fxq "$SOURCE_LINE" ~/.bashrc; then fi fi -$PIP3 install -U pip setuptools wheel +$PIP3 install -U pip packaging setuptools wheel $PIP3 install -U attrdict3 $PIP3 install -U $PYTHON_PKGS diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 0aea76a53b8388..76ea0c3098543c 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -399,8 +399,8 @@ if [ -n "$PYTHON_VENV_PACKAGE" ]; then fi fi -# try update setuptools and wheel before installing pip package that may need compilation -$PIP install $PIP_USER_ARGUMENT -U pip setuptools wheel +# try update packaging, setuptools and wheel before installing pip package that may need compilation +$PIP install $PIP_USER_ARGUMENT -U pip packaging setuptools wheel if [ "$GITHUB_ACTIONS" == "true" ]; then PIP_USER_ARGUMENT+=" --progress-bar off" diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index 8c13f6275f11be..83014df4d52a87 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -38,7 +38,7 @@ The packages depend on: #### 1. Create a workspace folder ```bash -mkdir -p ~/ros_ws/src && cd ~/ros_ws/src +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src ``` The ROS 2 tutorials contain more details regarding [ROS 2 workspaces](https://docs.ros.org/en/humble/Tutorials/Workspace/Creating-A-Workspace.html). @@ -54,7 +54,7 @@ vcs import --recursive < ros2.repos #### 3. Update dependencies ```bash -cd ~/ros_ws +cd ~/ros2_ws source /opt/ros/humble/setup.bash sudo apt update rosdep update @@ -72,7 +72,7 @@ ROS_DISTRO=humble ``` ```bash -cd ~/ros_ws +cd ~/ros2_ws colcon build --cmake-args -DBUILD_TESTING=ON ``` @@ -92,7 +92,7 @@ must be built from source and additional compiler flags are needed. #### 1. Create a workspace folder ```bash -mkdir -p ~/ros_ws/src && cd ~/ros_ws/src +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src ``` #### 2. Get the `ros2_macos.repos` file @@ -108,7 +108,7 @@ vcs import --recursive < ros2_macos.repos #### 3. Update dependencies ```bash -cd ~/ros_ws +cd ~/ros2_ws source /{path_to_your_ros_distro_workspace}/install/setup.zsh ``` diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 1246995e8a9598..bb19e6b4c69bf2 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -213,6 +213,7 @@ def __init__(self, Feature('RC', 'RC_SUMD', 'AP_RCPROTOCOL_SUMD_ENABLED', "Enable SUMD RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_GHST', 'AP_RCPROTOCOL_GHST_ENABLED', "Enable Ghost RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_MAVLINK_RADIO', 'AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED', "Enable MAVLink RC Protocol", 0, "RC_Protocol"), # NOQA: E501 + Feature('RC', 'RSSI', 'AP_RSSI_ENABLED', 'Enable RSSI handling library', 0, None), Feature('Rangefinder', 'RANGEFINDER', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_ANALOG', 'AP_RANGEFINDER_ANALOG_ENABLED', "Enable Rangefinder - Analog", 0, "RANGEFINDER"), # NOQA: E501 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index bf21800c19e745..c1f3aa86b6cb5f 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -257,6 +257,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_ENABLE_DRONECAN_DRIVERS', r'AP_DroneCAN::init'), ('AP_MAVLINK_MSG_HIL_GPS_ENABLED', r'mavlink_msg_hil_gps_decode'), ('AP_BARO_PROBE_EXTERNAL_I2C_BUSES', r'AP_Compass::_probe_external_i2c_compasses'), + ('AP_RSSI_ENABLED', r'AP_RSSI::init'), ] def progress(self, msg): diff --git a/Tools/scripts/firmware_version_decoder.py b/Tools/scripts/firmware_version_decoder.py index cf3321701490d7..3ef137f4e30c13 100755 --- a/Tools/scripts/firmware_version_decoder.py +++ b/Tools/scripts/firmware_version_decoder.py @@ -18,6 +18,18 @@ class FirmwareVersionType(enum.Enum): Official = 255 EnumEnd = 256 + @staticmethod + def get_release(version: int) -> str: + """ + Return the closest release type for a given version type, going down. + This is required because it is common in ardupilot to increase the version type + for successive betas, such as here: + https://github.com/ArduPilot/ardupilot/blame/8890c44370a7cf27d5efc872ef6da288ae3bc41f/ArduCopter/version.h#L12 + """ + for release in reversed(FirmwareVersionType): + if version >= release.value: + return release + return "Unknown" class VehicleType(enum.Enum): Rover = 1 @@ -193,7 +205,7 @@ def unpack_fwversion(self) -> None: self.fwversion.major = self.unpack("B") self.fwversion.minor = self.unpack("B") self.fwversion.patch = self.unpack("B") - self.fwversion.firmware_type = FirmwareVersionType(self.unpack("B")) + self.fwversion.firmware_type = FirmwareVersionType.get_release(self.unpack("B")) self.fwversion.os_software_version = self.unpack("I") self.fwversion.firmware_string = self.unpack_string_from_pointer() diff --git a/Tools/scripts/sitl-on-hardware/sitl-on-hw.py b/Tools/scripts/sitl-on-hardware/sitl-on-hw.py index 291005a7a73739..16707043a929d1 100755 --- a/Tools/scripts/sitl-on-hardware/sitl-on-hw.py +++ b/Tools/scripts/sitl-on-hardware/sitl-on-hw.py @@ -66,6 +66,8 @@ def sohw_path(fname): defaults_write(open(args.defaults,"r").read() + "\n") if args.simclass: + if args.simclass == 'Glider': + hwdef_write("define AP_SIM_GLIDER_ENABLED 1\n") hwdef_write("define AP_SIM_FRAME_CLASS %s\n" % args.simclass) if args.frame: hwdef_write('define AP_SIM_FRAME_STRING "%s"\n' % args.frame) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 8adc6d7c6dc4a4..6169ad161d1fe1 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -369,6 +369,19 @@ void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, } } +// adjust vertical climb rate so vehicle does not break the vertical fence +void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { + float backup_speed = 0.0f; + adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); + if (!is_zero(backup_speed)) { + if (is_negative(backup_speed)) { + climb_rate_cms = MIN(climb_rate_cms, backup_speed); + } else { + climb_rate_cms = MAX(climb_rate_cms, backup_speed); + } + } +} + // adjust vertical climb rate so vehicle does not break the vertical fence void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt) { @@ -379,29 +392,36 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c return; } - // do not adjust climb_rate if level or descending - if (climb_rate_cms <= 0.0f) { + // do not adjust climb_rate if level + if (is_zero(climb_rate_cms)) { return; } + const AP_AHRS &_ahrs = AP::ahrs(); // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); - bool limit_alt = false; - float alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) - - const AP_AHRS &_ahrs = AP::ahrs(); - + bool limit_min_alt = false; + bool limit_max_alt = false; + float max_alt_diff = 0.0f; // distance from altitude limit to vehicle in metres (positive means vehicle is below limit) + float min_alt_diff = 0.0f; #if AP_FENCE_ENABLED // calculate distance below fence AC_Fence *fence = AP::fence(); - if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence) { // calculate distance from vehicle to safe altitude float veh_alt; _ahrs.get_relative_position_D_home(veh_alt); - // _fence.get_safe_alt_max() is UP, veh_alt is DOWN: - alt_diff = fence->get_safe_alt_max() + veh_alt; - limit_alt = true; + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + min_alt_diff = -(fence->get_safe_alt_min() + veh_alt); + limit_min_alt = true; + } + if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { + // fence.get_safe_alt_max() is UP, veh_alt is DOWN: + max_alt_diff = fence->get_safe_alt_max() + veh_alt; + limit_max_alt = true; + } } #endif @@ -413,9 +433,9 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c _ahrs.get_relative_position_D_origin(curr_alt)) { // alt_limit is UP, curr_alt is DOWN: const float ctrl_alt_diff = alt_limit + curr_alt; - if (!limit_alt || ctrl_alt_diff < alt_diff) { - alt_diff = ctrl_alt_diff; - limit_alt = true; + if (!limit_max_alt || ctrl_alt_diff < max_alt_diff) { + max_alt_diff = ctrl_alt_diff; + limit_max_alt = true; } } @@ -425,34 +445,52 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c AP_Proximity *proximity = AP::proximity(); if (proximity && proximity_avoidance_enabled() && proximity->get_upward_distance(proximity_alt_diff)) { proximity_alt_diff -= _margin; - if (!limit_alt || proximity_alt_diff < alt_diff) { - alt_diff = proximity_alt_diff; - limit_alt = true; + if (!limit_max_alt || proximity_alt_diff < max_alt_diff) { + max_alt_diff = proximity_alt_diff; + limit_max_alt = true; } } #endif // limit climb rate - if (limit_alt) { + if (limit_max_alt || limit_min_alt) { + const float max_back_spd_cms = _backup_speed_z_max * 100.0; // do not allow climbing if we've breached the safe altitude - if (alt_diff <= 0.0f) { + if (max_alt_diff <= 0.0f && limit_max_alt) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude - const float max_back_spd_cms = _backup_speed_z_max * 100.0; if (is_positive(max_back_spd_cms)) { - backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt)); + backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -max_alt_diff*100.0f, dt)); // Constrain to max backup speed backup_speed = MAX(backup_speed, -max_back_spd_cms); } return; + // do not allow descending if we've breached the safe altitude + } else if (min_alt_diff <= 0.0f && limit_min_alt) { + climb_rate_cms = MAX(climb_rate_cms, 0.0f); + // also calculate backup speed that will get us back to safe altitude + if (is_positive(max_back_spd_cms)) { + backup_speed = get_max_speed(kP, accel_cmss_limited, -min_alt_diff*100.0f, dt); + + // Constrain to max backup speed + backup_speed = MIN(backup_speed, max_back_spd_cms); + } + return; } // limit climb rate - const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff*100.0f, dt); - climb_rate_cms = MIN(max_speed, climb_rate_cms); + if (limit_max_alt) { + const float max_alt_max_speed = get_max_speed(kP, accel_cmss_limited, max_alt_diff*100.0f, dt); + climb_rate_cms = MIN(max_alt_max_speed, climb_rate_cms); + } + + if (limit_min_alt) { + const float max_alt_min_speed = get_max_speed(kP, accel_cmss_limited, min_alt_diff*100.0f, dt); + climb_rate_cms = MAX(-max_alt_min_speed, climb_rate_cms); + } } -# endif +#endif } // adjust roll-pitch to push vehicle away from objects diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 2321d74989951a..aae55725ec1a14 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -68,14 +68,7 @@ class AC_Avoid { // adjust vertical climb rate so vehicle does not break the vertical fence void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed, float dt); - void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt) { - float backup_speed = 0.0f; - adjust_velocity_z(kP, accel_cmss, climb_rate_cms, backup_speed, dt); - if (!is_zero(backup_speed)) { - climb_rate_cms = MIN(climb_rate_cms, backup_speed); - } - } - + void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt); // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in centi-degrees diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 04a238196f58fe..743877cc1e7f83 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -36,26 +36,30 @@ extern const AP_HAL::HAL& hal; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 100.0 // after fence is broken we recreate the fence 100m further out +#define AC_FENCE_OPTIONS_DEFAULT OPTIONS::DISABLE_MODE_CHANGE #else #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0 // after fence is broken we recreate the fence 20m further out +#define AC_FENCE_OPTIONS_DEFAULT 0 #endif +//#define AC_FENCE_DEBUG const AP_Param::GroupInfo AC_Fence::var_info[] = { + // @Param: ENABLE // @DisplayName: Fence enable/disable - // @Description: Allows you to enable (1) or disable (0) the fence functionality + // @Description: Allows you to enable (1) or disable (0) the fence functionality. Fences can still be enabled and disabled via mavlink or an RC option, but these changes are not persisted. // @Values: 0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("ENABLE", 0, AC_Fence, _enabled, 0), // @Param: TYPE // @DisplayName: Fence Type - // @Description: Enabled fence types held as bitmask + // @Description: Configured fence types held as bitmask. Max altitide, Circle and Polygon fences will be immediately enabled if configured. Min altitude fence will only be enabled once the minimum altitude is reached. // @Bitmask{Rover}: 1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons // @Bitmask{Copter, Plane, Sub}: 0:Max altitude,1:Circle Centered on Home,2:Inclusion/Exclusion Circles+Polygons,3:Min altitude // @User: Standard - AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_DEFAULT), + AP_GROUPINFO("TYPE", 1, AC_Fence, _configured_fences, AC_FENCE_TYPE_DEFAULT), // @Param: ACTION // @DisplayName: Fence Action @@ -126,21 +130,21 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("RET_ALT", 9, AC_Fence, _ret_altitude, 0.0f, AP_PARAM_FRAME_PLANE), - // @Param{Plane}: AUTOENABLE + // @Param{Plane, Copter}: AUTOENABLE // @DisplayName: Fence Auto-Enable - // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. - // @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed + // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences, except the minimum altitude fence (which is enabled when the minimum altitude is reached), after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences on arming, except the minimum altitude fence (which is enabled when the minimum altitude is reached), but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. + // @Values{Plane, Copter}: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed // @Range: 0 3 // @Increment: 1 // @User: Standard - AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("AUTOENABLE", 10, AC_Fence, _auto_enabled, static_cast(AutoEnable::ALWAYS_DISABLED), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), - // @Param{Plane}: OPTIONS + // @Param{Plane, Copter}: OPTIONS // @DisplayName: Fence options - // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. + // @Description: When bit 0 is set sisable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas. // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), + AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI), AP_GROUPEND }; @@ -155,49 +159,167 @@ AC_Fence::AC_Fence() #endif _singleton = this; AP_Param::setup_object_defaults(this, var_info); + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } +} + +// get a user-friendly list of fences +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) +{ + if (!fences) { + return; + } + static const char* FENCE_NAMES[] = { + "Max Alt", + "Circle", + "Polygon", + "Min Alt", + }; + uint8_t i = 0; + uint8_t nfences = 0; + while (fences !=0) { + if (fences & 0x1) { + if (nfences > 0) { + if (!(fences & ~1U)) { + msg.printf(" and "); + } else { + msg.printf(", "); + } + } + msg.printf("%s", FENCE_NAMES[i]); + nfences++; + } + fences >>= 1; + i++; + } + msg.printf(" fence"); + if (nfences>1) { + msg.printf("s"); + } +} + +// print a message about the passed in fences +void AC_Fence::print_fence_message(const char* message, uint8_t fences) const +{ + if (!fences) { + return; + } + + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(fences, e); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s %s", e.get_writeable_string(), message); } -/// enable the Fence code generally; a master switch for all fences -void AC_Fence::enable(bool value) +// should be called @10Hz to handle loading from eeprom +void AC_Fence::update() { + _poly_loader.update(); + // if someone changes the parameter we want to enable or disable everything + if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { + // reset the auto mask since we just reconfigured all of fencing + _auto_enable_mask = AC_FENCE_ALL_FENCES; + _last_enabled = _enabled; + _last_auto_enabled = _auto_enabled; + if (_enabled) { + _enabled_fences = _configured_fences.get() & ~AC_FENCE_TYPE_ALT_MIN; + } else { + _enabled_fences = 0; + } + } +#ifdef AC_FENCE_DEBUG + static uint32_t last_msg_count = 0; + if (get_enabled_fences() && last_msg_count++ % 10 == 0) { + print_fence_message("active", get_enabled_fences()); + print_fence_message("breached", get_breaches()); + } +#endif +} + +// enable or disable configured fences present in fence_types +// also updates the bitmask of auto enabled fences if update_auto_mask is true +// returns a bitmask of fences that were changed +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) +{ + uint8_t fences = _configured_fences.get() & fence_types; + uint8_t enabled_fences = _enabled_fences; + if (value) { + enabled_fences |= fences; + } else { + enabled_fences &= ~fences; + } + + // fences that were manually changed are no longer eligible for auto-enablement or disablement + if (update_auto_mask) { + _auto_enable_mask &= ~fences; + } + + uint8_t fences_to_change = _enabled_fences ^ enabled_fences; + + if (!fences_to_change) { + return 0; + } #if HAL_LOGGING_ENABLED - if (_enabled && !value) { - AP::logger().Write_Event(LogEvent::FENCE_DISABLE); - } else if (!_enabled && value) { - AP::logger().Write_Event(LogEvent::FENCE_ENABLE); + AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); + if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MAX_ENABLE : LogEvent::FENCE_ALT_MAX_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_CIRCLE) { + AP::logger().Write_Event(value ? LogEvent::FENCE_CIRCLE_ENABLE : LogEvent::FENCE_CIRCLE_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_ALT_MIN) { + AP::logger().Write_Event(value ? LogEvent::FENCE_ALT_MIN_ENABLE : LogEvent::FENCE_ALT_MIN_DISABLE); + } + if (fences_to_change & AC_FENCE_TYPE_POLYGON) { + AP::logger().Write_Event(value ? LogEvent::FENCE_POLYGON_ENABLE : LogEvent::FENCE_POLYGON_DISABLE); } #endif - _enabled.set(value); + + _enabled_fences = enabled_fences; + if (!value) { - clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); - disable_floor(); - } else { - enable_floor(); + clear_breach(fences_to_change); } + + return fences_to_change; } /// enable/disable fence floor only void AC_Fence::enable_floor() { -#if HAL_LOGGING_ENABLED - if (!_floor_enabled) { - // Floor is currently disabled, enable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE); - } -#endif - _floor_enabled = true; + enable(true, AC_FENCE_TYPE_ALT_MIN); } void AC_Fence::disable_floor() { -#if HAL_LOGGING_ENABLED - if (_floor_enabled) { - // Floor is currently enabled, disable it - AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE); + enable(false, AC_FENCE_TYPE_ALT_MIN); +} + +/* + called on arming +*/ +void AC_Fence::auto_enable_fence_on_arming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; } -#endif - _floor_enabled = false; - clear_breach(AC_FENCE_TYPE_ALT_MIN); + + const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); + print_fence_message("auto-enabled", fences); +} + +/* + called on disarming +*/ +void AC_Fence::auto_disable_fence_on_disarming(void) +{ + if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + return; + } + + const uint8_t fences = enable(false, _auto_enable_mask, false); + print_fence_message("auto-disabled", fences); } /* @@ -205,82 +327,63 @@ void AC_Fence::disable_floor() */ void AC_Fence::auto_enable_fence_after_takeoff(void) { - if (_enabled) { + if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && + auto_enabled() != AutoEnable::ENABLE_DISABLE_FLOOR_ONLY) { return; - } - switch(auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: - case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)"); - break; - default: - // fence does not auto-enable in other takeoff conditions - break; } + + const uint8_t fences = enable(true, _auto_enable_mask, false); + print_fence_message("auto-enabled", fences); } -/* - called when performing an auto landing - */ -void AC_Fence::auto_disable_fence_for_landing(void) +// return fences that should be auto-disabled when requested +uint8_t AC_Fence::get_auto_disable_fences(void) const { + uint8_t auto_disable = 0; switch (auto_enabled()) { - case AC_Fence::AutoEnable::ALWAYS_ENABLED: - enable(false); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)"); + case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: + auto_disable = _auto_enable_mask; break; case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: - disable_floor(); - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)"); - break; - default: - // fence does not auto-disable in other landing conditions + case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: + default: // when auto disable is not set we still need to disable the altmin fence on landing + auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; break; } + return auto_disable; } -bool AC_Fence::present() const +uint8_t AC_Fence::present() const { - const auto enabled_fences = _enabled_fences.get(); - // A fence is present if any of the conditions are true. - // * tin can (circle) is enabled - // * min or max alt is enabled - // * polygon fences are enabled and any fence has been uploaded - if (enabled_fences & AC_FENCE_TYPE_CIRCLE || - enabled_fences & AC_FENCE_TYPE_ALT_MIN || - enabled_fences & AC_FENCE_TYPE_ALT_MAX || - ((enabled_fences & AC_FENCE_TYPE_POLYGON) && _poly_loader.total_fence_count() > 0)) { - return true; + uint8_t mask = AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX; + if (_poly_loader.total_fence_count() > 0) { + mask |= AC_FENCE_TYPE_POLYGON; } - return false; + return _configured_fences.get() & mask; } /// get_enabled_fences - returns bitmask of enabled fences uint8_t AC_Fence::get_enabled_fences() const { - if (!_enabled && !_auto_enabled) { - return 0; - } - return _enabled_fences; + return _enabled_fences & present(); } // additional checks for the polygon fence: -bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const { - if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if (!(_configured_fences & AC_FENCE_TYPE_POLYGON)) { // not enabled; all good return true; } if (! _poly_loader.loaded()) { - fail_msg = "Fences invalid"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence(s) invalid"); return false; } if (!_poly_loader.check_inclusion_circle_margin(_margin)) { - fail_msg = "Margin is less than inclusion circle radius"; + hal.util->snprintf(failure_msg, failure_msg_len, "Polygon fence margin is less than inclusion circle radius"); return false; } @@ -288,14 +391,14 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const } // additional checks for the circle fence: -bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const { if (_circle_radius < 0) { - fail_msg = "Invalid FENCE_RADIUS value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid Circle FENCE_RADIUS value"); return false; } if (_circle_radius < _margin) { - fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS"; + hal.util->snprintf(failure_msg, failure_msg_len, "Circle FENCE_MARGIN is less than FENCE_RADIUS"); return false; } @@ -303,15 +406,15 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const } // additional checks for the alt fence: -bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const +bool AC_Fence::pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const { if (_alt_max < 0.0f) { - fail_msg = "Invalid FENCE_ALT_MAX value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MAX value"); return false; } if (_alt_min < -100.0f) { - fail_msg = "Invalid FENCE_ALT_MIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_ALT_MIN value"); return false; } return true; @@ -319,63 +422,64 @@ bool AC_Fence::pre_arm_check_alt(const char* &fail_msg) const /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully -bool AC_Fence::pre_arm_check(const char* &fail_msg) const +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { - fail_msg = nullptr; - // if fences are enabled but none selected fail pre-arm check - if (enabled() && !present()) { - fail_msg = "Fences enabled, but none selected"; + if (_enabled && !present()) { + hal.util->snprintf(failure_msg, failure_msg_len, "Fences enabled, but none selected"); return false; } // if not enabled or not fence set-up always return true - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled) || !_configured_fences) { return true; } // if we have horizontal limits enabled, check we can get a // relative position from the AHRS - if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || - (_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + if ((_configured_fences & AC_FENCE_TYPE_CIRCLE) || + (_configured_fences & AC_FENCE_TYPE_POLYGON)) { Vector2f position; if (!AP::ahrs().get_relative_position_NE_home(position)) { - fail_msg = "Fence requires position"; + hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position"); return false; } } - if (!pre_arm_check_polygon(fail_msg)) { + if (!pre_arm_check_polygon(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_circle(fail_msg)) { + if (!pre_arm_check_circle(failure_msg, failure_msg_len)) { return false; } - if (!pre_arm_check_alt(fail_msg)) { + if (!pre_arm_check_alt(failure_msg, failure_msg_len)) { return false; } // check no limits are currently breached if (_breached_fences) { - fail_msg = "vehicle outside fence"; + char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + AC_Fence::get_fence_names(_breached_fences, e); + hal.util->snprintf(failure_msg, failure_msg_len, "vehicle outside %s", e.get_writeable_string()); return false; } // validate FENCE_MARGIN parameter range if (_margin < 0.0f) { - fail_msg = "Invalid FENCE_MARGIN value"; + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid FENCE_MARGIN value"); return false; } if (_alt_max < _alt_min) { - fail_msg = "FENCE_ALT_MAX < FENCE_ALT_MIN"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_ALT_MAX < FENCE_ALT_MIN"); return false; } if (_alt_max - _alt_min <= 2.0f * _margin) { - fail_msg = "FENCE_MARGIN too big"; + hal.util->snprintf(failure_msg, failure_msg_len, "FENCE_MARGIN too big"); return false; } @@ -389,13 +493,14 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const bool AC_Fence::check_fence_alt_max() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MAX)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (_curr_alt >= _alt_max) { @@ -437,13 +542,14 @@ bool AC_Fence::check_fence_alt_max() bool AC_Fence::check_fence_alt_min() { // altitude fence check - if (!(_enabled_fences & AC_FENCE_TYPE_ALT_MIN)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)) { // not enabled; no breach return false; } - AP::ahrs().get_relative_position_D_home(_curr_alt); - _curr_alt = -_curr_alt; // translate Down to Up + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up // check if we are under the altitude fence if (_curr_alt <= _alt_min) { @@ -479,15 +585,47 @@ bool AC_Fence::check_fence_alt_min() return false; } + +/// auto enable fence floor +bool AC_Fence::auto_enable_fence_floor() +{ + // altitude fence check + if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured + || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled + || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled + || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED + || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) { + // not enabled + return false; + } + + float alt; + AP::ahrs().get_relative_position_D_home(alt); + _curr_alt = -alt; // translate Down to Up + + // check if we are over the altitude fence + if (!floor_enabled() && _curr_alt >= _alt_min + _margin) { + enable(true, AC_FENCE_TYPE_ALT_MIN, false); + gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)"); + return true; + } + + return false; +} + // check_fence_polygon - returns true if the poly fence is freshly // breached. That includes being inside exclusion zones and outside // inclusions zones bool AC_Fence::check_fence_polygon() { + if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { + // not enabled; no breach + clear_breach(AC_FENCE_TYPE_POLYGON); + return false; + } + const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; - const bool breached = ((_enabled_fences & AC_FENCE_TYPE_POLYGON) && - _poly_loader.breached()); - if (breached) { + if (_poly_loader.breached()) { if (!was_breached) { record_breach(AC_FENCE_TYPE_POLYGON); return true; @@ -506,7 +644,7 @@ bool AC_Fence::check_fence_polygon() /// fence is breached. bool AC_Fence::check_fence_circle() { - if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { + if (!(get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) { // not enabled; no breach return false; } @@ -549,49 +687,68 @@ bool AC_Fence::check_fence_circle() /// check - returns bitmask of fence types breached (if any) -uint8_t AC_Fence::check() +uint8_t AC_Fence::check(bool disable_auto_fences) { uint8_t ret = 0; + uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0; + uint8_t fences_to_disable = disabled_fences & _enabled_fences; // clear any breach from a non-enabled fence - clear_breach(~_enabled_fences); + clear_breach(~_configured_fences); + // clear any breach from disabled fences + clear_breach(fences_to_disable); + + // report on any fences that were auto-disabled + if (fences_to_disable) { + print_fence_message("auto-disabled", fences_to_disable); + } // return immediately if disabled - if ((!_enabled && !_auto_enabled) || !_enabled_fences) { + if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; } - // check if pilot is attempting to recover manually - if (_manual_recovery_start_ms != 0) { - // we ignore any fence breaches during the manual recovery period which is about 10 seconds - if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { - return 0; - } - // recovery period has passed so reset manual recovery time - // and continue with fence breach checks - _manual_recovery_start_ms = 0; - } + // disable the (temporarily) disabled fences + enable(false, disabled_fences, false); // maximum altitude fence check - if (check_fence_alt_max()) { + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) { ret |= AC_FENCE_TYPE_ALT_MAX; } - // minimum altitude fence check - if (_floor_enabled && check_fence_alt_min()) { + // minimum altitude fence check, do this before auto-disabling (e.g. because falling) + // so that any action can be taken + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) { ret |= AC_FENCE_TYPE_ALT_MIN; } + // auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required) + if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) { + auto_enable_fence_floor(); + } + // circle fence check - if (check_fence_circle()) { + if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) { ret |= AC_FENCE_TYPE_CIRCLE; } // polygon fence check - if (check_fence_polygon()) { + if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) { ret |= AC_FENCE_TYPE_POLYGON; } + // check if pilot is attempting to recover manually + // this is done last so that _breached_fences is correct + if (_manual_recovery_start_ms != 0) { + // we ignore any fence breaches during the manual recovery period which is about 10 seconds + if ((AP_HAL::millis() - _manual_recovery_start_ms) < AC_FENCE_MANUAL_RECOVERY_TIME_MIN) { + return 0; + } + // recovery period has passed so reset manual recovery time + // and continue with fence breach checks + _manual_recovery_start_ms = 0; + } + // return any new breaches that have occurred return ret; } @@ -695,6 +852,8 @@ void AC_Fence::manual_recovery_start() // record time pilot began manual recovery _manual_recovery_start_ms = AP_HAL::millis(); + + gcs().send_text(MAV_SEVERITY_INFO, "Manual recovery started"); } // methods for mavlink SYS_STATUS message (send_sys_status) @@ -712,7 +871,7 @@ bool AC_Fence::sys_status_enabled() const return false; } // Fence is only enabled when the flag is enabled - return _enabled; + return enabled(); } bool AC_Fence::sys_status_failed() const @@ -742,22 +901,27 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { AP_GROUPEND }; AC_Fence::AC_Fence() {}; -void AC_Fence::enable(bool value) {}; +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { return 0; } -void AC_Fence::disable_floor() {}; +void AC_Fence::enable_floor() {} +void AC_Fence::disable_floor() {} +void AC_Fence::update() {} -void AC_Fence::auto_enable_fence_after_takeoff() {}; -void AC_Fence::auto_disable_fence_for_landing() {}; +void AC_Fence::auto_enable_fence_after_takeoff() {} +void AC_Fence::auto_enable_fence_on_arming() {} +void AC_Fence::auto_disable_fence_on_disarming() {} -bool AC_Fence::present() const { return false; } +uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::get_enabled_fences() const { return 0; } -bool AC_Fence::pre_arm_check(const char* &fail_msg) const { return true; } +bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const { return true; } -uint8_t AC_Fence::check() { return 0; } +uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; } bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; } float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; } +void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { } +void AC_Fence::print_fence_message(const char* msg, uint8_t fences) const {} void AC_Fence::manual_recovery_start() {} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index e8eb88943e133c..3165d7b73d4d33 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -15,6 +16,8 @@ #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence #define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL +#define AC_FENCE_ARMING_FENCES (AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON) +#define AC_FENCE_ALL_FENCES (AC_FENCE_ARMING_FENCES | AC_FENCE_TYPE_ALT_MIN) // valid actions should a fence be breached #define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action @@ -34,12 +37,19 @@ class AC_Fence public: friend class AC_PolyFence_loader; - enum class AutoEnable + enum class AutoEnable : uint8_t { ALWAYS_DISABLED = 0, - ALWAYS_ENABLED = 1, - ENABLE_DISABLE_FLOOR_ONLY = 2, - ONLY_WHEN_ARMED = 3 + ENABLE_ON_AUTO_TAKEOFF = 1, // enable on auto takeoff + ENABLE_DISABLE_FLOOR_ONLY = 2, // enable on takeoff but disable floor on landing + ONLY_WHEN_ARMED = 3 // enable on arming + }; + + enum class MavlinkFenceActions : uint16_t + { + DISABLE_FENCE = 0, + ENABLE_FENCE = 1, + DISABLE_ALT_MIN_FENCE = 2 }; AC_Fence(); @@ -55,10 +65,15 @@ class AC_Fence static AC_Fence *get_singleton() { return _singleton; } /// enable - allows fence to be enabled/disabled. - void enable(bool value); + /// returns a bitmask of fences that were changed + uint8_t enable(bool value, uint8_t fence_types, bool update_auto_mask = true); + + /// enable_configured - allows configured fences to be enabled/disabled. + /// returns a bitmask of fences that were changed + uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } /// auto_enabled - automaticaly enable/disable fence depending of flight status - AutoEnable auto_enabled() { return static_cast(_auto_enabled.get()); } + AutoEnable auto_enabled() const { return static_cast(_auto_enabled.get()); } /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value void enable_floor(); @@ -69,32 +84,39 @@ class AC_Fence /// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met void auto_enable_fence_after_takeoff(); - /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing. - void auto_disable_fence_for_landing(); + /// auto_enable_fences_on_arming - auto enables all applicable fences on arming + void auto_enable_fence_on_arming(); + + /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming + void auto_disable_fence_on_disarming(); - /// enabled - returns true if fence is enabled - bool enabled() const { return _enabled; } + uint8_t get_auto_disable_fences(void) const; - /// present - returns true if fence is present - bool present() const; + /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. + bool auto_enable_fence_floor(); + + /// enabled - returns whether fencing is enabled or not + bool enabled() const { return _enabled_fences; } + + /// present - returns true if any of the provided types is present + uint8_t present() const; /// get_enabled_fences - returns bitmask of enabled fences uint8_t get_enabled_fences() const; // should be called @10Hz to handle loading from eeprom - void update() { - _poly_loader.update(); - } + void update(); /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully - bool pre_arm_check(const char* &fail_msg) const; + bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; /// /// methods to check we are within the boundaries and recover /// /// check - returns the fence type that has been breached (if any) - uint8_t check(); + /// disabled_fences can be used to disable fences for certain conditions (e.g. landing) + uint8_t check(bool disable_auto_fence = false); // returns true if the destination is within fence (used to reject waypoints outside the fence) bool check_destination_within_fence(const class Location& loc); @@ -133,6 +155,12 @@ class AC_Fence /// get_return_rally - returns whether returning to fence return point or rally point float get_return_altitude() const { return _ret_altitude; } + /// get a user-friendly list of fences + static void get_fence_names(uint8_t fences, ExpandingString& msg); + + // print a message about the fences to the GCS + void print_fence_message(const char* msg, uint8_t fences) const; + /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds /// should be called whenever the pilot changes the flight mode /// has no effect if no breaches have occurred @@ -187,14 +215,19 @@ class AC_Fence void clear_breach(uint8_t fence_type); // additional checks for the different fence types: - bool pre_arm_check_polygon(const char* &fail_msg) const; - bool pre_arm_check_circle(const char* &fail_msg) const; - bool pre_arm_check_alt(const char* &fail_msg) const; + bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const; + bool pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const; + // fence floor is enabled + bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } // parameters - AP_Int8 _enabled; // fence enable/disable control + uint8_t _enabled_fences; // fences that are currently enabled/disabled + bool _last_enabled; // value of enabled last time we checked + AP_Int8 _enabled; // overall feature control AP_Int8 _auto_enabled; // top level flag for auto enabling fence - AP_Int8 _enabled_fences; // bit mask holding which fences are enabled + uint8_t _last_auto_enabled; // value of auto_enabled last time we checked + AP_Int8 _configured_fences; // bit mask holding which fences are enabled AP_Int8 _action; // recovery action specified by user AP_Float _alt_max; // altitude upper limit in meters AP_Float _alt_min; // altitude lower limit in meters @@ -216,7 +249,7 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - bool _floor_enabled; // fence floor is enabled + uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) float _curr_alt; diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 509e039a843bbf..b97334476e0533 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -227,7 +227,7 @@ bool AC_PolyFence_loader::breached() const // returns true if location is outside the boundary bool AC_PolyFence_loader::breached(const Location& loc) const { - if (!loaded()) { + if (!loaded() || total_fence_count() == 0) { return false; } @@ -493,13 +493,15 @@ bool AC_PolyFence_loader::index_eeprom() if (!count_eeprom_fences()) { return false; } + + void_index(); + if (_eeprom_fence_count == 0) { + _num_fences = 0; _load_attempted = false; return true; } - void_index(); - Debug("Fence: Allocating %u bytes for index", (unsigned)(_eeprom_fence_count*sizeof(FenceIndex))); _index = NEW_NOTHROW FenceIndex[_eeprom_fence_count]; diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index fa8cc8aab0e620..41ec47659d650f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1280,16 +1280,12 @@ bool AP_Arming::fence_checks(bool display_failure) } // check fence is ready - const char *fail_msg = nullptr; - if (fence->pre_arm_check(fail_msg)) { + char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; + if (fence->pre_arm_check(fail_msg, ARRAY_SIZE(fail_msg))) { return true; } - if (fail_msg == nullptr) { - check_failed(display_failure, "Check fence"); - } else { - check_failed(display_failure, "%s", fail_msg); - } + check_failed(display_failure, "%s", fail_msg); #if AP_SDCARD_STORAGE_ENABLED if (fence->failed_sdcard_storage() || StorageManager::storage_failed()) { @@ -1797,11 +1793,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) if (armed) { auto *fence = AP::fence(); if (fence != nullptr) { - // If a fence is set to auto-enable, turn on the fence - if (fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(true); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fence: auto-enabled"); - } + fence->auto_enable_fence_on_arming(); } } #endif @@ -1844,9 +1836,7 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) #if AP_FENCE_ENABLED AC_Fence *fence = AP::fence(); if (fence != nullptr) { - if(fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { - fence->enable(false); - } + fence->auto_disable_fence_on_disarming(); } #endif #if defined(HAL_ARM_GPIO_PIN) diff --git a/libraries/AP_Common/ExpandingString.cpp b/libraries/AP_Common/ExpandingString.cpp index 9be2b3a91abf17..a3e7a8b4019580 100644 --- a/libraries/AP_Common/ExpandingString.cpp +++ b/libraries/AP_Common/ExpandingString.cpp @@ -25,6 +25,13 @@ extern const AP_HAL::HAL& hal; #define EXPAND_INCREMENT 512 +ExpandingString::ExpandingString(char* s, uint32_t total_len) : buf(0) +{ + set_buffer(s, total_len, 0); + memset(buf, 0, buflen); +} + + /* expand the string buffer */ diff --git a/libraries/AP_Common/ExpandingString.h b/libraries/AP_Common/ExpandingString.h index 2561332f3e89d8..d92882e1b3af6f 100644 --- a/libraries/AP_Common/ExpandingString.h +++ b/libraries/AP_Common/ExpandingString.h @@ -24,6 +24,8 @@ class ExpandingString { public: + ExpandingString() : buf(0), buflen(0), used(0), allocation_failed(false), external_buffer(false) {} + ExpandingString(char* s, uint32_t total_len); const char *get_string(void) const { return buf; diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 76df6f2bca508e..eb54a54eff09f1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -74,6 +74,14 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { #endif + // @Param: _DOMAIN_ID + // @DisplayName: DDS DOMAIN ID + // @Description: Set the ROS_DOMAIN_ID + // @Range: 0 232 + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO("_DOMAIN_ID", 4, AP_DDS_Client, domain_id, 0), + AP_GROUPEND }; @@ -819,9 +827,8 @@ bool AP_DDS_Client::create() .type = UXR_PARTICIPANT_ID }; const char* participant_name = "ardupilot_dds"; - const uint16_t domain_id = 0; const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, - domain_id, participant_name, UXR_REPLACE); + static_cast(domain_id), participant_name, UXR_REPLACE); //Participant requests constexpr uint8_t nRequestsParticipant = 1; diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index e796b78edb3fd4..ac11e53e572aa1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -213,6 +213,9 @@ class AP_DDS_Client //! @brief Parameter storage static const struct AP_Param::GroupInfo var_info[]; + //! @brief ROS_DOMAIN_ID + AP_Int32 domain_id; + //! @brief Enum used to mark a topic as a data reader or writer enum class Topic_rw : uint8_t { DataReader = 0, diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index d25fe5be3d3fc6..65b61fdf30359a 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -7,7 +7,9 @@ #if HAL_WITH_ESC_TELEM -#define ESC_TELEM_MAX_ESCS NUM_SERVO_CHANNELS +#ifndef ESC_TELEM_MAX_ESCS + #define ESC_TELEM_MAX_ESCS NUM_SERVO_CHANNELS +#endif static_assert(ESC_TELEM_MAX_ESCS > 0, "Cannot have 0 ESC telemetry instances"); #define ESC_TELEM_DATA_TIMEOUT_MS 5000UL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp index 623ad0184f748e..afc1f8b7f1a638 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_MAVliteMsgHandler.cpp @@ -153,10 +153,10 @@ MAV_RESULT AP_Frsky_MAVliteMsgHandler::handle_command_do_fence_enable(const mavl switch ((uint16_t)mav_command_long.param1) { case 0: - fence->enable(false); + fence->enable_configured(false); return MAV_RESULT_ACCEPTED; case 1: - fence->enable(true); + fence->enable_configured(true); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index e5a102998bf96f..4a18a3295526a3 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -138,6 +138,19 @@ AP_GPS_SBF::read(void) config_string = nullptr; } break; + case Config_State::Constellation: + if ((params.gnss_mode&0x6F)!=0) { + //IMES not taken into account by Septentrio receivers + if (asprintf(&config_string, "sst, %s%s%s%s%s%s\n", (params.gnss_mode&(1U<<0))!=0 ? "GPS" : "", + (params.gnss_mode&(1U<<1))!=0 ? ((params.gnss_mode&0x01)==0 ? "SBAS" : "+SBAS") : "", + (params.gnss_mode&(1U<<2))!=0 ? ((params.gnss_mode&0x03)==0 ? "GALILEO" : "+GALILEO") : "", + (params.gnss_mode&(1U<<3))!=0 ? ((params.gnss_mode&0x07)==0 ? "BEIDOU" : "+BEIDOU") : "", + (params.gnss_mode&(1U<<5))!=0 ? ((params.gnss_mode&0x0F)==0 ? "QZSS" : "+QZSS") : "", + (params.gnss_mode&(1U<<6))!=0 ? ((params.gnss_mode&0x2F)==0 ? "GLONASS" : "+GLONASS") : "") == -1) { + config_string=nullptr; + } + } + break; case Config_State::Blob: if (asprintf(&config_string, "%s\n", _initialisation_blob[_init_blob_index]) == -1) { config_string = nullptr; @@ -364,6 +377,9 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: + config_step = Config_State::Constellation; + break; + case Config_State::Constellation: config_step = Config_State::Blob; break; case Config_State::Blob: @@ -699,4 +715,5 @@ bool AP_GPS_SBF::prepare_for_arming(void) { return is_logging; } + #endif diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 8817653f42ed74..b4e1bb28c62b1e 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -76,6 +76,7 @@ class AP_GPS_SBF : public AP_GPS_Backend Blob, SBAS, SGA, + Constellation, Complete }; Config_State config_step; diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 46d7ea4796eba3..f5c47834220047 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -131,7 +131,7 @@ void SIMState::fdm_input_local(void) // output JSON state to ride along flight controllers // ride_along.send(_sitl->state,sitl_model->get_position_relhome()); -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED if (gimbal != nullptr) { gimbal->update(); } diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 0c84791f91ea39..f4343475715d2f 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -109,10 +109,10 @@ class AP_HAL::SIMState { // internal SITL model SITL::Aircraft *sitl_model; -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED // simulated gimbal bool enable_gimbal; - SITL::Gimbal *gimbal; + SITL::SoloGimbal *gimbal; #endif #if HAL_SIM_ADSB_ENABLED diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index 0eae8e932a3c1b..2710a78ff4ccb5 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -90,3 +90,5 @@ #ifndef AP_FILTER_ENABLED #define AP_FILTER_ENABLED 1 #endif + +#define HAL_SOLO_GIMBAL_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 6775192d2ad7ee..a6d802ea6429b8 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -641,8 +641,11 @@ void Util::apply_persistent_params(void) const been done by whether the IDs are configured in storage */ - if (strncmp(pname, "INS_ACC", 7) == 0 && - strcmp(pname+strlen(pname)-3, "_ID") == 0) { + bool legacy_acc_id = strncmp(pname, "INS_ACC", 7) == 0 && + strcmp(pname+strlen(pname)-3, "_ID") == 0; + bool new_acc_id = strncmp(pname, "INS", 3) == 0 && + strcmp(pname+strlen(pname)-6, "ACC_ID") == 0; + if (legacy_acc_id || new_acc_id) { enum ap_var_type ptype; AP_Int32 *ap = (AP_Int32 *)AP_Param::find(pname, &ptype); if (ap && ptype == AP_PARAM_INT32) { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat index 67443a0e65c390..8e451e2bed4fcf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat @@ -101,7 +101,7 @@ PA2 TIM2_CH3 TIM2 PWM(4) GPIO(53) # M4 # LEDs define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PB5 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # LED strip PB6 TIM4_CH1 TIM4 PWM(5) GPIO(56) # M7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat index 2d5fa82e437fcd..f4f12ed84fd984 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BlitzF745/hwdef.dat @@ -124,7 +124,7 @@ PC9 TIM8_CH4 TIM8 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PD15 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat index 0131fd18e1868b..474e761991cb7b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat @@ -189,7 +189,7 @@ PF5 SAFETY_IN INPUT PULLDOWN PE3 LED_RED OUTPUT HIGH GPIO(90) define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 define HAL_GPIO_LED_ON 1 # barometers diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat index a6141f55d57b99..3b2655f8123ff7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat @@ -72,7 +72,7 @@ define BOARD_RSSI_ANA_PIN 13 # LED define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC10 LED OUTPUT HIGH GPIO(57) -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 # SPI PA5 SPI1_SCK SPI1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat index a403ff33727a9a..d3cd91aeb33f12 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405Pro/hwdef.dat @@ -120,7 +120,7 @@ PA9 TIM1_CH2 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat index 93ae7491d42edb..6c06ea552a8ea5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat @@ -108,7 +108,7 @@ PA9 TIM1_CH2 TIM1 PWM(5) GPIO(54) # M5 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat index 075a3f8cd537a1..97e2b86c68c142 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat @@ -138,7 +138,7 @@ PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat index d1dde0be965000..e55d72117bb2e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEM_JHEF405/hwdef.dat @@ -110,7 +110,7 @@ PA9 TIM1_CH2 TIM1 PWM(9) GPIO(58) # LED define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC14 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat index 5c27d1c9e6756e..d74ef0f7cb173c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-DShot/hwdef.dat @@ -32,3 +32,7 @@ define HAL_PERIPH_ENABLE_NOTIFY define HAL_SERIAL_ESC_COMM_ENABLED 1 define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 + +# also enable relay output via hardpoint msgs +define HAL_PERIPH_ENABLE_RELAY +define AP_RELAY_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat index 1446c3fe344647..1e068c119b4e56 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat @@ -139,7 +139,7 @@ define BOARD_RSSI_ANA_PIN 0 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 41 +define AP_NOTIFY_GPIO_LED_1_PIN 41 define OSD_ENABLED 1 define HAL_OSD_TYPE_DEFAULT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat index 8c5885570b81dd..292c0777d674aa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405Mini/hwdef.dat @@ -115,7 +115,7 @@ PA8 TIM1_CH1 TIM1 PWM(5) GPIO(54) # M5 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # Dataflash setup SPIDEV dataflash SPI3 DEVID1 FLASH1_CS MODE3 104*MHZ 104*MHZ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat index b8bf2579594222..75965031cbb94e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat @@ -77,7 +77,7 @@ PA14 JTCK-SWCLK SWD PB4 TIM3_CH1 TIM3 GPIO(56) ALARM define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PB5 LED OUTPUT HIGH GPIO(57) -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat index 347b49385328b6..e1c7ddacac9e31 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/airbotf4/hwdef.dat @@ -99,7 +99,7 @@ define HAL_BATT_VOLT_SCALE 11 define HAL_BATT_CURR_SCALE 18.2 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 57 +define AP_NOTIFY_GPIO_LED_1_PIN 57 # 64kB FLASH_RESERVE_START_KB means we're lacking a lot of space: include ../include/minimize_features.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c index c1a5eeb0493a97..689f9e4acc1c84 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.c @@ -22,6 +22,16 @@ #error "Unsupported IWDG RCC MCU config" #endif +/* + define for controlling how long the watchdog is set for. +*/ +#ifndef STM32_WDG_TIMEOUT_MS +#define STM32_WDG_TIMEOUT_MS 2048 +#endif +#if STM32_WDG_TIMEOUT_MS > 4096 || STM32_WDG_TIMEOUT_MS < 20 +#error "Watchdog timeout out of range" +#endif + /* defines for working out if the reset was from the watchdog */ @@ -68,16 +78,17 @@ static bool watchdog_enabled; */ void stm32_watchdog_init(void) { - // setup for 2s reset + // setup the watchdog timeout + // t = 4 * 2^PR * (RLR+1) / 32KHz IWDGD.KR = 0x5555; - IWDGD.PR = 2; // div16 - IWDGD.RLR = 0xFFF; + IWDGD.PR = 3; // changing this would change the definition of STM32_WDG_TIMEOUT_MS + IWDGD.RLR = STM32_WDG_TIMEOUT_MS - 1; IWDGD.KR = 0xCCCC; watchdog_enabled = true; } /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h index f015dee9c1a135..de8e3941494b08 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/watchdog.h @@ -10,7 +10,7 @@ extern "C" { void stm32_watchdog_init(void); /* - pat the dog, to prevent a reset. If not called for 1s + pat the dog, to prevent a reset. If not called for STM32_WDG_TIMEOUT_MS after stm32_watchdog_init() then MCU will reset */ void stm32_watchdog_pat(void); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat index d695fb7b8ab26f..946c15584de5d3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat @@ -143,7 +143,7 @@ define HAL_BATT_CURR_SCALE 18.2 define BOARD_RSSI_ANA_PIN 0 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 41 +define AP_NOTIFY_GPIO_LED_1_PIN 41 define OSD_ENABLED 1 #font for the osd diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat index 478a827bb62a8c..752745b3922479 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat @@ -25,7 +25,7 @@ SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 PC8 LED_BLUE OUTPUT LOW GPIO(0) define AP_NOTIFY_GPIO_LED_1_ENABLED 1 -define HAL_GPIO_A_LED_PIN 0 +define AP_NOTIFY_GPIO_LED_1_PIN 0 # buzzer PC5 BUZZER OUTPUT GPIO(80) LOW diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat index 15de93a140a2e1..80e58d65d90f08 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v4/hwdef.dat @@ -123,7 +123,7 @@ PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # M9 define AP_NOTIFY_GPIO_LED_1_ENABLED 1 PC13 LED0 OUTPUT LOW GPIO(90) -define HAL_GPIO_A_LED_PIN 90 +define AP_NOTIFY_GPIO_LED_1_PIN 90 # OSD setup SPIDEV osd SPI2 DEVID1 OSD1_CS MODE0 10*MHZ 10*MHZ diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index e64253b79bc884..7011c2206b616b 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -219,11 +219,11 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) case SITL::SIM::CANTransport::MulticastUDP: transport = NEW_NOTHROW CAN_Multicast(); break; - case SITL::SIM::CANTransport::SocketCAN: #if HAL_CAN_WITH_SOCKETCAN + case SITL::SIM::CANTransport::SocketCAN: transport = NEW_NOTHROW CAN_SocketCAN(); -#endif break; +#endif case SITL::SIM::CANTransport::None: default: // if user supplies an invalid value for the parameter transport = nullptr; diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 2d49bb606b07cd..af55acaa1f8443 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -79,9 +79,11 @@ void SITL_State::_sitl_setup() if (_sitl != nullptr) { // setup some initial values _update_airspeed(0); +#if AP_SIM_SOLOGIMBAL_ENABLED if (enable_gimbal) { - gimbal = NEW_NOTHROW SITL::Gimbal(_sitl->state); + gimbal = NEW_NOTHROW SITL::SoloGimbal(); } +#endif sitl_model->set_buzzer(&_sitl->buzzer_sim); sitl_model->set_sprayer(&_sitl->sprayer_sim); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 1a45ed97d97b67..54a530bc447b17 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -332,9 +332,9 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const */ void SITL_State_Common::sim_update(void) { -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_SOLOGIMBAL_ENABLED if (gimbal != nullptr) { - gimbal->update(); + gimbal->update(*sitl_model); } #endif #if HAL_SIM_ADSB_ENABLED diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 7bb4c04348f556..95ef129c1cce7a 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -9,7 +9,7 @@ #define SITL_SERVO_PORT 20722 #include -#include +#include #include #include #include @@ -104,11 +104,11 @@ class HALSITL::SITL_State_Common { bool new_rc_input; uint16_t pwm_output[SITL_NUM_CHANNELS]; bool output_ready = false; - -#if HAL_SIM_GIMBAL_ENABLED + +#if AP_SIM_SOLOGIMBAL_ENABLED // simulated gimbal bool enable_gimbal; - SITL::Gimbal *gimbal; + SITL::SoloGimbal *gimbal; #endif #if HAL_SIM_ADSB_ENABLED diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 30cea03245060d..042bb2ce7f2128 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -439,9 +439,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case 'v': vehicle_str = gopt.optarg; break; +#if AP_SIM_SOLOGIMBAL_ENABLED case CMDLINE_GIMBAL: enable_gimbal = true; break; +#endif case CMDLINE_FGVIEW: _use_fg_view = true; break; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp index 17d89f15ac1cd5..bb71172dc9b009 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp @@ -526,6 +526,7 @@ void AP_InertialSensor::get_persistent_params(ExpandingString &str) const str.printf("INS%u_ACCOFFS_Z=%f\n", imu, aoff.z); str.printf("INS%u_ACCSCAL_X=%f\n", imu, ascl.x); str.printf("INS%u_ACCSCAL_Y=%f\n", imu, ascl.y); + str.printf("INS%u_ACCSCAL_Z=%f\n", imu, ascl.z); str.printf("INS%u_ACC_CALTEMP=%.2f\n", imu, params[i].caltemp_accel.get()); } #endif diff --git a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp index 5d3bdc2f41558a..ebc44a3937d554 100644 --- a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp +++ b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp @@ -142,10 +142,12 @@ void AP_LTM_Telem::send_Sframe(void) const uint8_t flightmode = AP_Notify::flags.flight_mode; // flight mode uint8_t rssi = 0; // radio RSSI (%a) +#if AP_RSSI_ENABLED AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); if (ap_rssi) { rssi = ap_rssi->read_receiver_rssi_uint8(); } +#endif const uint8_t armstat = AP_Notify::flags.armed; // 0: disarmed, 1: armed const uint8_t failsafe = AP_Notify::flags.failsafe_radio; // 0: normal, 1: failsafe diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index d4c605bcced530..e8a91d62da63d5 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -79,8 +79,15 @@ enum class LogEvent : uint8_t { STANDBY_ENABLE = 74, STANDBY_DISABLE = 75, - FENCE_FLOOR_ENABLE = 80, - FENCE_FLOOR_DISABLE = 81, + // Fence events + FENCE_ALT_MAX_ENABLE = 76, + FENCE_ALT_MAX_DISABLE = 77, + FENCE_CIRCLE_ENABLE = 78, + FENCE_CIRCLE_DISABLE = 79, + FENCE_ALT_MIN_ENABLE = 80, + FENCE_ALT_MIN_DISABLE = 81, + FENCE_POLYGON_ENABLE = 82, + FENCE_POLYGON_DISABLE = 83, // if the EKF's source input set is changed (e.g. via a switch or // a script), we log an event: diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ff42107ff64188..3e138e7cacb403 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -16,6 +16,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_Mission::var_info[] = { @@ -447,6 +448,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return start_command_camera(cmd); +#endif +#if AP_FENCE_ENABLED + case MAV_CMD_DO_FENCE_ENABLE: + return start_command_fence(cmd); #endif case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); @@ -1233,7 +1238,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - cmd.p1 = packet.param1; // action 0=disable, 1=enable + cmd.p1 = packet.param1; // action 0=disable, 1=enable, 2=disable floor break; case MAV_CMD_DO_AUX_FUNCTION: @@ -1746,7 +1751,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207 - packet.param1 = cmd.p1; // action 0=disable, 1=enable + packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=disable floor, 3=enable except floor break; case MAV_CMD_DO_PARACHUTE: // MAV ID: 208 diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index e76c3a638f5486..69f8fe2b46b979 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -936,6 +936,7 @@ class AP_Mission bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd); bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd); bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd); + bool start_command_fence(const AP_Mission::Mission_Command& cmd); /* handle format conversion of storage format to allow us to update diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index d44b62e6746867..bc0374232050d5 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #if AP_RC_CHANNEL_ENABLED bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) @@ -347,4 +348,30 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss return false; } +bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd) +{ +#if AP_FENCE_ENABLED + AC_Fence* fence = AP::fence(); + + if (fence == nullptr) { + return false; + } + + if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_FENCE)) { // disable fence + uint8_t fences = fence->enable_configured(false); + fence->print_fence_message("disabled", fences); + return true; + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::ENABLE_FENCE)) { // enable fence + uint8_t fences = fence->enable_configured(true); + fence->print_fence_message("enabled", fences); + return true; + } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE)) { // disable fence floor only + fence->disable_floor(); + fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN); + return true; + } +#endif // AP_FENCE_ENABLED + return false; +} + #endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index b188f945a2f23f..02c8c3f2475c1c 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SoloGimbal.h" +#include "AP_Mount_config.h" + #if HAL_SOLO_GIMBAL_ENABLED +#include "AP_Mount_SoloGimbal.h" + #include "SoloGimbal.h" #include #include diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h index 4e64233284a9fd..ed32e6697bef67 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -3,18 +3,16 @@ */ #pragma once +#include "AP_Mount_config.h" -#include +#if HAL_SOLO_GIMBAL_ENABLED #include "AP_Mount_Backend.h" -#if HAL_SOLO_GIMBAL_ENABLED #include #include #include -#include #include "SoloGimbal.h" - class AP_Mount_SoloGimbal : public AP_Mount_Backend { diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 853f5e24dd07c0..e82a3f217cb6ce 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -35,6 +35,7 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00 +# define AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME "PA2" // get model name, data bytes always 00 # define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_MODE "PTZ" // set gimbal mode, data bytes: 00:stop, 01:up, 02:down, 03:left, 04:right, 05:home position, 06:lock, 07:follow, 08:lock/follow toggle, 09:calibration, 0A:one button down # define AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE "YPR" // set the rate yaw, pitch and roll targets of the gimbal yaw in range -99 ~ +99 # define AP_MOUNT_TOPOTEK_ID3CHAR_YAW_ANGLE "GIY" // set the yaw angle target in the range -150 ~ 150, speed 0 ~ 99 (0.1deg/sec) @@ -107,8 +108,12 @@ void AP_Mount_Topotek::update() break; case 6: // request tracking info - if (_is_tracking) { - request_track_status(); + request_track_status(); + break; + case 8: + // get gimbal model name + if (!_got_gimbal_model_name) { + request_gimbal_model_name(); } break; } @@ -120,11 +125,10 @@ void AP_Mount_Topotek::update() if (_is_tracking) { // cancel tracking if mode has changed if (_last_mode != _mode) { + _last_mode = _mode; cancel_tracking(); - } else { - // image tracking is active so we do not send attitude targets - return; } + return; } _last_mode = _mode; @@ -388,6 +392,11 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& } if (send_tracking_cmd) { + // set the gimbal to the ready-to-track state when the gimbal tracking status is stopped + if (_last_tracking_state == TrackingStatus::STOPPED_TRACKING) { + send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 2); + } + // prepare data bytes uint8_t databuff[10]; databuff[0] = HIGHBYTE(track_center_x); @@ -399,7 +408,7 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& databuff[6] = HIGHBYTE(track_height); databuff[7] = LOWBYTE(track_height); databuff[8] = 0; - databuff[9] = 0; + databuff[9] = (tracking_type == TrackingType::TRK_POINT) ? 9 : 1; // when tracking point, enable fuzzy click function // send tracking command bool res = send_variablelen_packet(HeaderType::VARIABLE_LEN, @@ -407,7 +416,12 @@ bool AP_Mount_Topotek::set_tracking(TrackingType tracking_type, const Vector2f& AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING, true, (uint8_t*)databuff, ARRAY_SIZE(databuff)); - _is_tracking |= res; + + // display error message on failure + if (!res) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s tracking failed", send_message_prefix); + } + return res; } @@ -424,11 +438,11 @@ bool AP_Mount_Topotek::cancel_tracking() return false; } + // if gimbal is tracking-in-progress change to waiting state, otherwise stop + const uint8_t track_set = _last_tracking_state == TrackingStatus::TRACKING_IN_PROGRESS ? 1 : 0; + // send tracking command - if (send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, 1)) { - return true; - } - return false; + return send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_TRACKING, true, track_set); } // set camera picture-in-picture mode @@ -510,6 +524,11 @@ void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const static uint8_t model_name[32] {}; const char cam_definition_uri[140] {}; + // copy model name if available + if (_got_gimbal_model_name) { + strncpy((char*)model_name, (const char*)_model_name, ARRAY_SIZE(model_name)); + } + // capability flags const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO | CAMERA_CAP_FLAGS_CAPTURE_IMAGE | @@ -675,8 +694,8 @@ void AP_Mount_Topotek::read_incoming_packets() case ParseState::WAITING_FOR_ID1: case ParseState::WAITING_FOR_ID2: case ParseState::WAITING_FOR_ID3: - // sanity check all capital letters. eg 'GAC' - if (b >= 'A' && b <= 'Z') { + // check all uppercase letters and numbers. eg 'GAC' + if ((b >= 'A' && b <= 'Z') || (b >= '0' && b <= '9')) { // advance to next state _parser.state = (ParseState)((uint8_t)_parser.state+1); break; @@ -770,10 +789,17 @@ void AP_Mount_Topotek::request_track_status() // request gimbal version void AP_Mount_Topotek::request_gimbal_version() { - // sample command: #TPPD2rVSN00 + // sample command: #TPUD2rVSN00 send_fixedlen_packet(AddressByte::SYSTEM_AND_IMAGE, AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION, false, 0); } +// request gimbal model name +void AP_Mount_Topotek::request_gimbal_model_name() +{ + // sample command: #TPUG2rPA200 + send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GET_MODEL_NAME, false, 0); +} + // send angle target in radians to gimbal void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) { @@ -982,7 +1008,7 @@ void AP_Mount_Topotek::gimbal_sdcard_analyse() void AP_Mount_Topotek::gimbal_track_analyse() { // ignore tracking state if unchanged - uint8_t tracking_state = _msg_buff[11]; + TrackingStatus tracking_state = (TrackingStatus)_msg_buff[11]; if (tracking_state == _last_tracking_state) { return; } @@ -991,15 +1017,17 @@ void AP_Mount_Topotek::gimbal_track_analyse() // inform user const char* tracking_str = "tracking"; switch (tracking_state) { - case '0': - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s %s error", send_message_prefix, tracking_str); - break; - case '1': + case TrackingStatus::STOPPED_TRACKING: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s stopped", send_message_prefix, tracking_str); _is_tracking = false; break; - case '2': + case TrackingStatus::WAITING_FOR_TRACKING: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s waiting", send_message_prefix, tracking_str); + _is_tracking = false; + break; + case TrackingStatus::TRACKING_IN_PROGRESS: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s started", send_message_prefix, tracking_str); + _is_tracking = true; break; } } @@ -1024,33 +1052,68 @@ void AP_Mount_Topotek::gimbal_dist_info_analyse() // gimbal basic information analysis void AP_Mount_Topotek::gimbal_version_analyse() { - uint8_t major_ver = 0; - uint8_t minor_ver = 0; - uint8_t patch_ver = 0; + // version array with index 0=major, 1=minor, 2=patch + uint8_t version[3] {}; // extract firmware version + // the version can be in the format "1.2.3" or "123" const uint8_t data_buf_len = char_to_hex(_msg_buff[5]); - if (data_buf_len >= 1) { - major_ver = char_to_hex(_msg_buff[10]); - } - if (data_buf_len >= 2) { - minor_ver = char_to_hex(_msg_buff[11]); + + // check for "." + bool contains_period = false; + for (uint8_t i = 0; i < data_buf_len; i++) { + contains_period |= _msg_buff[10 + i] == '.'; } - if (data_buf_len >= 3) { - patch_ver = char_to_hex(_msg_buff[12]); + + // if contains period, extract version number + uint32_t ver_num = 0; + uint8_t ver_count = 0; + if (contains_period) { + for (uint8_t i = 0; i < data_buf_len; i++) { + if (_msg_buff[10 + i] != '.') { + ver_num = ver_num * 10 + char_to_hex(_msg_buff[10 + i]); + } else { + version[ver_count++] = ver_num; + ver_num = 0; + } + if (ver_count >= ARRAY_SIZE(version)) { + break; + } + } + } else { + if (data_buf_len >= 1) { + version[0] = char_to_hex(_msg_buff[10]); + } + if (data_buf_len >= 2) { + version[1] = char_to_hex(_msg_buff[11]); + } + if (data_buf_len >= 3) { + version[2] = char_to_hex(_msg_buff[12]); + } } - _firmware_ver = (patch_ver << 16) | (minor_ver << 8) | (major_ver); + _firmware_ver = (version[2] << 16) | (version[1] << 8) | (version[0]); // display gimbal model and firmware version to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s v%u.%u.%u", send_message_prefix, - major_ver, - minor_ver, - patch_ver); + version[0], // major version + version[1], // minor version + version[2]); // patch version _got_gimbal_version = true; } +// gimbal model name message analysis +void AP_Mount_Topotek::gimbal_model_name_analyse() +{ + strncpy((char *)_model_name, (const char *)_msg_buff + 10, char_to_hex(_msg_buff[5])); + + // display gimbal model name to user + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_message_prefix, _model_name); + + _got_gimbal_model_name = true; +} + // calculate checksum uint8_t AP_Mount_Topotek::calculate_crc(const uint8_t *cmd, uint8_t len) const { diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index c1f3e08dc8fa10..bcabd3d4e2735f 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -27,7 +27,7 @@ #include #define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal -#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 6 // parse the number of gimbal command types +#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 7 // parse the number of gimbal command types class AP_Mount_Topotek : public AP_Mount_Backend_Serial { @@ -147,6 +147,13 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial WAITING_FOR_CRC_HIGH, }; + // tracking status + enum class TrackingStatus : uint8_t { + STOPPED_TRACKING = 0x30, // not tracking + WAITING_FOR_TRACKING = 0x31, // wait to track command status + TRACKING_IN_PROGRESS = 0x32 // the status is being tracked. + }; + // identifier bytes typedef char Identifier[3]; @@ -168,6 +175,9 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // request gimbal version void request_gimbal_version(); + // request gimbal model name + void request_gimbal_model_name(); + // send angle target in radians to gimbal void send_angle_target(const MountTarget& angle_rad); @@ -195,6 +205,9 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // gimbal basic information analysis void gimbal_version_analyse(); + // gimbal model name message analysis + void gimbal_model_name_analyse(); + // gimbal distance information analysis void gimbal_dist_info_analyse(); @@ -224,13 +237,15 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // members bool _recording; // recording status (received from gimbal) bool _is_tracking; // whether to enable the tracking state - uint8_t _last_tracking_state; // last tracking state received from gimbal + TrackingStatus _last_tracking_state = TrackingStatus::STOPPED_TRACKING; // last tracking state received from gimbal uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking bool _sdcard_status; // memory card status (received from gimbal) bool _last_lock; // last lock mode sent to gimbal bool _got_gimbal_version; // true if gimbal's version has been received + bool _got_gimbal_model_name; // true if gimbal's model name has been received bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets) bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets) + uint8_t _model_name[16]; // gimbal model name uint8_t _sent_time_count; // count of current time messages sent to gimbal uint32_t _firmware_ver; // firmware version Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw) @@ -260,6 +275,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, {{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse}, {{"VSN"}, &AP_Mount_Topotek::gimbal_version_analyse}, + {{"PA2"}, &AP_Mount_Topotek::gimbal_model_name_analyse} }; }; diff --git a/libraries/AP_Notify/GPIO_LED_1.cpp b/libraries/AP_Notify/GPIO_LED_1.cpp index 950a1ce49cad2f..6ea0bd16c9aa0d 100644 --- a/libraries/AP_Notify/GPIO_LED_1.cpp +++ b/libraries/AP_Notify/GPIO_LED_1.cpp @@ -22,8 +22,8 @@ #include #include "AP_Notify.h" -#ifndef AP_NOTIFY_GPIO_LED_A_PIN -#error "define AP_NOTIFY_GPIO_LED_A_PIN" +#ifndef AP_NOTIFY_GPIO_LED_1_PIN +#error "define AP_NOTIFY_GPIO_LED_1_PIN" #endif extern const AP_HAL::HAL& hal; @@ -33,9 +33,9 @@ bool GPIO_LED_1::init(void) // when HAL_GPIO_LED_ON is 0 then we must not use pinMode() // as it could remove the OPENDRAIN attribute on the pin #if HAL_GPIO_LED_ON != 0 - hal.gpio->pinMode(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_OUTPUT); + hal.gpio->pinMode(AP_NOTIFY_GPIO_LED_1_PIN, HAL_GPIO_OUTPUT); #endif - hal.gpio->write(AP_NOTIFY_GPIO_LED_A_PIN, HAL_GPIO_LED_OFF); + hal.gpio->write(AP_NOTIFY_GPIO_LED_1_PIN, HAL_GPIO_LED_OFF); return true; } @@ -67,7 +67,7 @@ void GPIO_LED_1::update(void) last_timestep_ms = now_ms; const auto new_state = (current_pattern & (1U<write(AP_NOTIFY_GPIO_LED_A_PIN, new_state); + hal.gpio->write(AP_NOTIFY_GPIO_LED_1_PIN, new_state); next_bit++; if (next_bit > 31) { next_bit = 0; diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 0c1aa5bfe80e24..e95363f1febc8c 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -2584,8 +2584,10 @@ void AP_OSD_Screen::draw(void) DRAW_SETTING(avgcellvolt); DRAW_SETTING(avgcellrestvolt); DRAW_SETTING(restvolt); +#if AP_RSSI_ENABLED DRAW_SETTING(rssi); DRAW_SETTING(link_quality); +#endif DRAW_SETTING(current); DRAW_SETTING(batused); DRAW_SETTING(bat2used); diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index afd113d5f4c011..5fe0593f11139f 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3147,6 +3147,11 @@ function battery:current_amps(instance) end ---@return number -- resting voltage function battery:voltage_resting_estimate(instance) end +-- Returns the estimated internal battery resistance in Ohms +---@param instance integer -- battery instance +---@return number -- estimated internal resistance in Ohms +function battery:get_resistance(instance) end + -- Returns the voltage of the selected battery instance. ---@param instance integer -- battery instance ---@return number -- voltage diff --git a/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua b/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua new file mode 100644 index 00000000000000..63a811e2e2a573 --- /dev/null +++ b/libraries/AP_Scripting/examples/battery_internal_resistance_check.lua @@ -0,0 +1,35 @@ +--[[ + script implementing pre-arm check that internal resistance is sensible +--]] + +local MAX_RESISTANCE = 0.03 -- Ohms + +local auth_id = assert(arming:get_aux_auth_id()) + +local warning_last_sent_ms = uint32_t() -- time we last sent a warning message to the user +warning_interval_ms = 10000 + +function update() + local num_batts = battery:num_instances() + local ok = true + for i=0,num_batts do + local resistance = battery:get_resistance(i) + failed = resistance > MAX_RESISTANCE + if failed then + msg = string.format("Batt[%u] high internal resistance %.5f Ohms", i+1, resistance) + if millis() - warning_last_sent_ms > warning_interval_ms then + gcs:send_text(0, msg) + warning_last_sent_ms = millis() + end + arming:set_aux_auth_failed(auth_id, msg) + ok = false + end + end + + if ok then + arming:set_aux_auth_passed(auth_id) + end + return update, 500 +end + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 5528b04cf80b9b..dbeca527799586 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -94,6 +94,7 @@ singleton AP_BattMonitor depends AP_BATTERY_ENABLED singleton AP_BattMonitor method num_instances uint8_t singleton AP_BattMonitor method healthy boolean uint8_t 0 ud->num_instances() singleton AP_BattMonitor method voltage float uint8_t 0 ud->num_instances() +singleton AP_BattMonitor method get_resistance float uint8_t 0 ud->num_instances() singleton AP_BattMonitor method voltage_resting_estimate float uint8_t 0 ud->num_instances() singleton AP_BattMonitor method current_amps boolean float'Null uint8_t 0 ud->num_instances() singleton AP_BattMonitor method consumed_mah boolean float'Null uint8_t 0 ud->num_instances() diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index b2f4e300103d4c..d9c530978328cc 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2381,7 +2381,7 @@ void emit_methods(struct userdata *node) { } void emit_enum(struct userdata * data) { - fprintf(source, "struct userdata_enum %s_enums[] = {\n", data->sanatized_name); + fprintf(source, "const struct userdata_enum %s_enums[] = {\n", data->sanatized_name); struct userdata_enum *ud_enum = data->enums; while (ud_enum != NULL) { fprintf(source, " {\"%s\", %s::%s},\n", ud_enum->name, data->name, ud_enum->name); @@ -2499,68 +2499,75 @@ void emit_loaders(void) { emit_type_index(parsed_singletons, "singleton"); emit_type_index(parsed_ap_objects, "ap_object"); + fprintf(source, "static int binding_index(lua_State *L) {\n"); + fprintf(source, " const char * name = luaL_checkstring(L, 2);\n"); + fprintf(source, "\n"); + fprintf(source, " bool found = false;\n"); + fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); + fprintf(source, " if (strcmp(name, singleton_fun[i].name) == 0) {\n"); + fprintf(source, " lua_newuserdata(L, 0);\n"); + fprintf(source, " if (luaL_newmetatable(L, name)) { // need to create metatable\n"); + fprintf(source, " lua_pushcfunction(L, singleton_fun[i].func);\n"); + fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); + fprintf(source, " }\n"); + fprintf(source, " lua_setmetatable(L, -2);\n"); + fprintf(source, " found = true;\n"); + fprintf(source, " break;\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " if (!found) {\n"); + fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); + fprintf(source, " if (strcmp(name, new_userdata[i].name) == 0) {\n"); + fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); + fprintf(source, " found = true;\n"); + fprintf(source, " break;\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " }\n"); + fprintf(source, " if (!found) {\n"); + fprintf(source, " return 0;\n"); + fprintf(source, " }\n"); + fprintf(source, "\n"); + fprintf(source, " // store found value to avoid a re-index\n"); + fprintf(source, " lua_pushvalue(L, -2);\n"); + fprintf(source, " lua_pushvalue(L, -2);\n"); + fprintf(source, " lua_settable(L, -5);\n"); + fprintf(source, "\n"); + fprintf(source, " return 1;\n"); + fprintf(source, "}\n\n"); + fprintf(source, "void load_generated_bindings(lua_State *L) {\n"); fprintf(source, " luaL_checkstack(L, 5, \"Out of stack\");\n"); // this is more stack space then we need, but should never fail fprintf(source, " // userdata metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(userdata_fun); i++) {\n"); fprintf(source, " luaL_newmetatable(L, userdata_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, userdata_fun[i].func, 0);\n"); + fprintf(source, " lua_pushcfunction(L, userdata_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); fprintf(source, " if (userdata_fun[i].operators != nullptr) {\n"); fprintf(source, " luaL_setfuncs(L, userdata_fun[i].operators, 0);\n"); fprintf(source, " }\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, userdata_fun[i].name);\n"); - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, userdata_fun[i].name);\n"); fprintf(source, " }\n"); fprintf(source, "\n"); fprintf(source, " // ap object metatables\n"); fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(ap_object_fun); i++) {\n"); fprintf(source, " luaL_newmetatable(L, ap_object_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, ap_object_fun[i].func, 0);\n"); + fprintf(source, " lua_pushcfunction(L, ap_object_fun[i].func);\n"); fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, ap_object_fun[i].name);\n"); - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, ap_object_fun[i].name);\n"); fprintf(source, " }\n"); fprintf(source, "\n"); - fprintf(source, " // singleton metatables\n"); - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); - fprintf(source, " luaL_newmetatable(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_pushcclosure(L, singleton_fun[i].func, 0);\n"); - fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); - fprintf(source, " lua_pushstring(L, \"__call\");\n"); - fprintf(source, " lua_pushvalue(L, -2);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - - fprintf(source, " lua_pop(L, 1);\n"); - fprintf(source, " lua_newuserdata(L, 0);\n"); - fprintf(source, " luaL_getmetatable(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_setmetatable(L, -2);\n"); - fprintf(source, " lua_setglobal(L, singleton_fun[i].name);\n"); - fprintf(source, " }\n"); + fprintf(source, " // singletons and userdata creation funcs are loaded dynamically\n"); - fprintf(source, "\n"); fprintf(source, "}\n\n"); } -void emit_sandbox(void) { +void emit_userdata_new_funcs(void) { struct userdata *data = parsed_userdata; fprintf(source, "const struct userdata {\n"); fprintf(source, " const char *name;\n"); @@ -2609,23 +2616,14 @@ void emit_sandbox(void) { } } fprintf(source, "};\n\n"); +} +void emit_sandbox(void) { fprintf(source, "void load_generated_sandbox(lua_State *L) {\n"); - // load the singletons - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(singleton_fun); i++) {\n"); - fprintf(source, " lua_pushstring(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_getglobal(L, singleton_fun[i].name);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " }\n"); - - // load the userdata allactors and globals - fprintf(source, " for (uint32_t i = 0; i < ARRAY_SIZE(new_userdata); i++) {\n"); - fprintf(source, " lua_pushstring(L, new_userdata[i].name);\n"); - fprintf(source, " lua_pushcfunction(L, new_userdata[i].fun);\n"); - fprintf(source, " lua_settable(L, -3);\n"); - fprintf(source, " }\n"); - - fprintf(source, "\n"); + fprintf(source, " lua_createtable(L, 0, 1);\n"); + fprintf(source, " lua_pushcfunction(L, binding_index);\n"); + fprintf(source, " lua_setfield(L, -2, \"__index\");\n"); + fprintf(source, " lua_setmetatable(L, -2);\n"); fprintf(source, "}\n"); } @@ -3169,7 +3167,7 @@ int main(int argc, char **argv) { emit_methods(parsed_ap_objects); emit_index(parsed_ap_objects); - + emit_userdata_new_funcs(); emit_loaders(); emit_sandbox(); diff --git a/libraries/AP_Scripting/modules/MAVLink/Readme.md b/libraries/AP_Scripting/modules/MAVLink/Readme.md new file mode 100644 index 00000000000000..97924a1c33a075 --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/Readme.md @@ -0,0 +1,6 @@ +These .lua definition for messages can be generated using pymavlink with the following command: +(change the last path acoordingly to place the files where you need them) +``` +cd ardupilot/modules/mavlink +python ./pymavlink/tools/mavgen.py --lang Lua ./message_definitions/v1.0/all.xml --out ./modules/MAVLink +``` \ No newline at end of file diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index 0d83372cf3f88b..d2c5b439e04c97 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -41,6 +41,9 @@ class AP_SmartRTL { // get a point on the path const Vector3f& get_point(uint16_t index) const { return _path[index]; } + // add point to end of path. returns true on success, false on failure (due to failure to take the semaphore) + bool add_point(const Vector3f& point); + // get next point on the path to home, returns true on success bool pop_point(Vector3f& point); @@ -109,9 +112,6 @@ class AP_SmartRTL { IgnorePilotYaw = (1U << 2), }; - // add point to end of path - bool add_point(const Vector3f& point); - // routine cleanup attempts to remove 10 points (see SMARTRTL_CLEANUP_POINT_MIN definition) by simplification or loop pruning void routine_cleanup(uint16_t path_points_count, uint16_t path_points_complete_limit); diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 9307502caaf0bf..55fb024043b1da 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -415,7 +415,11 @@ void AP_TECS::_update_speed(float DT) if (aparm.stall_prevention) { // when stall prevention is active we raise the minimum // airspeed based on aerodynamic load factor - _TASmin *= _load_factor; + if (is_positive(aparm.airspeed_stall)) { + _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*_load_factor); + } else { + _TASmin *= _load_factor; + } } if (_TASmax < _TASmin) { diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp index f255e7e8b29e66..91a7686f1d9582 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.cpp @@ -33,29 +33,34 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Analog::var_info[] = { // @Param: A0 // @DisplayName: Temperature sensor analog 0th polynomial coefficient - // @Description: a0 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a0 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A0", 2, AP_TemperatureSensor_Analog, _a[0], 0), // @Param: A1 // @DisplayName: Temperature sensor analog 1st polynomial coefficient - // @Description: a1 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a1 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A1", 3, AP_TemperatureSensor_Analog, _a[1], 0), // @Param: A2 // @DisplayName: Temperature sensor analog 2nd polynomial coefficient - // @Description: a2 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a2 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A2", 4, AP_TemperatureSensor_Analog, _a[2], 0), // @Param: A3 // @DisplayName: Temperature sensor analog 3rd polynomial coefficient - // @Description: a3 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a3 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A3", 5, AP_TemperatureSensor_Analog, _a[3], 0), // @Param: A4 // @DisplayName: Temperature sensor analog 4th polynomial coefficient - // @Description: a4 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // @Description: a4 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 AP_GROUPINFO("A4", 6, AP_TemperatureSensor_Analog, _a[4], 0), + // @Param: A5 + // @DisplayName: Temperature sensor analog 5th polynomial coefficient + // @Description: a5 in polynomial of form temperature in deg = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 + AP_GROUPINFO("A5", 7, AP_TemperatureSensor_Analog, _a[5], 0), + AP_GROUPEND }; @@ -81,7 +86,7 @@ void AP_TemperatureSensor_Analog::update() const float voltage = _analog_source->voltage_average_ratiometric(); // Evaluate polynomial - // temperature (deg) = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + // temperature (deg) = a0 + a1*voltage + a2*voltage^2 + a3*voltage^3 + a4*voltage^4 + a5*voltage^5 float temp = 0.0; float poly = 1.0; for (uint8_t i = 0; i < ARRAY_SIZE(_a); i++) { diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h index a342c165332e9b..99c3ba4ed12002 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Analog.h @@ -35,10 +35,10 @@ class AP_TemperatureSensor_Analog : public AP_TemperatureSensor_Backend { AP_HAL::AnalogSource *_analog_source; // Pin used to measure voltage - AP_Int8 _pin; + AP_Int8 _pin; // Polynomial coefficients to calculate temperature from voltage - AP_Float _a[5]; + AP_Float _a[6]; }; diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 7aee38d5c5b35d..df3692a61b6552 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -14,6 +14,7 @@ struct AP_FixedWing { AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; + AP_Float airspeed_stall; AP_Float min_groundspeed; AP_Int8 crash_detection_enable; AP_Float roll_limit; diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 835e53192abfd0..ea42cf249e14fe 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -125,7 +125,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: OPTS // @DisplayName: Harmonic Notch Filter options // @Description: Harmonic Notch Filter options. Triple and double-notches can provide deeper attenuation across a wider bandwidth with reduced latency than single notches and are suitable for larger aircraft. Multi-Source attaches a harmonic notch to each detected noise frequency instead of simply being multiples of the base frequency, in the case of FFT it will attach notches to each of three detected noise peaks, in the case of ESC it will attach notches to each of four motor RPM values. Loop rate update changes the notch center frequency at the scheduler loop rate rather than at the default of 200Hz. If both double and triple notches are specified only double notches will take effect. - // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM failure + // @Bitmask: 0:Double notch,1:Multi-Source,2:Update at loop rate,3:EnableOnAllIMUs,4:Triple notch, 5:Use min freq on RPM source failure // @User: Advanced // @RebootRequired: True AP_GROUPINFO("OPTS", 8, HarmonicNotchFilterParams, _options, 0), diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index d6dad1a76990c9..b3bfb78a37194f 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -16,20 +16,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_int return MAV_RESULT_UNSUPPORTED; } - switch ((uint16_t)packet.param1) { - case 0: // disable fence - fence->enable(false); + uint8_t fences = AC_FENCE_ALL_FENCES; + if (uint8_t(packet.param2)) { + fences = uint8_t(packet.param2); + } + + switch (AC_Fence::MavlinkFenceActions(packet.param1)) { + case AC_Fence::MavlinkFenceActions::DISABLE_FENCE: + fence->enable(false, fences); return MAV_RESULT_ACCEPTED; - case 1: // enable fence - if (!fence->present()) - { + case AC_Fence::MavlinkFenceActions::ENABLE_FENCE: + if (!(fence->present() & fences)) { return MAV_RESULT_FAILED; } - - fence->enable(true); + + fence->enable(true, fences); return MAV_RESULT_ACCEPTED; - case 2: // disable fence floor only - fence->disable_floor(); + case AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE: + fence->enable(false, AC_FENCE_TYPE_ALT_MIN); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; @@ -82,7 +86,7 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type = FENCE_BREACH_BOUNDARY; } - // report on Avoidance liminting + // report on Avoidance limiting uint8_t breach_mitigation = FENCE_MITIGATE_UNKNOWN; #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) const AC_Avoid* avoid = AC_Avoid::get_singleton(); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index a2de561a77923d..8d27aeb426eb10 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1139,7 +1139,7 @@ void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag) return; } - fence->enable(ch_flag == AuxSwitchPos::HIGH); + fence->enable_configured(ch_flag == AuxSwitchPos::HIGH); } #endif diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index a6baf91eb9da24..9ca985a14591ab 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -181,7 +181,7 @@ void Aircraft::update_position(void) uint32_t now = AP_HAL::millis(); if (now - last_one_hz_ms >= 1000) { // shift origin of position at 1Hz to current location - // this prevents sperical errors building up in the GPS data + // this prevents spherical errors building up in the GPS data last_one_hz_ms = now; Vector2d diffNE = origin.get_distance_NE_double(location); position.xy() -= diffNE; @@ -796,6 +796,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) } } + // update slung payload +#if AP_SIM_SLUNGPAYLOAD_ENABLED + sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth); +#endif + // allow for changes in physics step adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -1227,6 +1232,19 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel) } } +#if AP_SIM_SLUNGPAYLOAD_ENABLED +// add body-frame force due to slung payload +void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +{ + Vector3f forces_ef; + sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef); + + // convert ef forces to body-frame accelerations (acceleration = force / mass) + const Vector3f accel_bf = dcm.transposed() * forces_ef / mass; + body_accel += accel_bf; +} +#endif + /* get position relative to home */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 90f07089ea60b1..e84a078ac8fafc 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,6 +322,11 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add body-frame force due to slung payload + void add_slungpayload_forces(Vector3f &body_accel); +#endif + // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index 5aede11b3b40a3..a905a38d731d09 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -13,19 +13,17 @@ along with this program. If not, see . */ /* - gimbal simulator class for MAVLink gimbal + gimbal simulator class for gimbal */ #include "SIM_Gimbal.h" -#if HAL_SIM_GIMBAL_ENABLED +#if AP_SIM_GIMBAL_ENABLED #include #include "SIM_Aircraft.h" -#include - extern const AP_HAL::HAL& hal; #define GIMBAL_DEBUG 0 @@ -38,27 +36,10 @@ extern const AP_HAL::HAL& hal; namespace SITL { -Gimbal::Gimbal(const struct sitl_fdm &_fdm) : - fdm(_fdm), - target_address("127.0.0.1"), - target_port(5762), - lower_joint_limits(radians(-40), radians(-135), radians(-7.5)), - upper_joint_limits(radians(40), radians(45), radians(7.5)), - travelLimitGain(20), - reporting_period_ms(10), - seen_heartbeat(false), - seen_gimbal_control(false), - mav_socket(false) -{ - memset(&mavlink, 0, sizeof(mavlink)); - fdm.quaternion.rotation_matrix(dcm); -} - - /* update the gimbal state */ -void Gimbal::update(void) +void Gimbal::update(const class Aircraft &aircraft) { // calculate delta time in seconds uint32_t now_us = AP_HAL::micros(); @@ -66,8 +47,10 @@ void Gimbal::update(void) float delta_t = (now_us - last_update_us) * 1.0e-6f; last_update_us = now_us; - Matrix3f vehicle_dcm; - fdm.quaternion.rotation_matrix(vehicle_dcm); + const Matrix3f &vehicle_dcm = aircraft.get_dcm(); + if (!init_done) { + dcm = vehicle_dcm; + } const Vector3f &vehicle_gyro = AP::ins().get_gyro(); const Vector3f &vehicle_accel_body = AP::ins().get_accel(); @@ -181,234 +164,21 @@ void Gimbal::update(void) // integrate velocity delta_velocity += accel * delta_t; - - // see if we should do a report - send_report(); } -static struct gimbal_param { - const char *name; - float value; -} gimbal_params[] = { - {"GMB_OFF_ACC_X", 0}, - {"GMB_OFF_ACC_Y", 0}, - {"GMB_OFF_ACC_Z", 0}, - {"GMB_GN_ACC_X", 0}, - {"GMB_GN_ACC_Y", 0}, - {"GMB_GN_ACC_Z", 0}, - {"GMB_OFF_GYRO_X", 0}, - {"GMB_OFF_GYRO_Y", 0}, - {"GMB_OFF_GYRO_Z", 0}, - {"GMB_OFF_JNT_X", 0}, - {"GMB_OFF_JNT_Y", 0}, - {"GMB_OFF_JNT_Z", 0}, - {"GMB_K_RATE", 0}, - {"GMB_POS_HOLD", 0}, - {"GMB_MAX_TORQUE", 0}, - {"GMB_SND_TORQUE", 0}, - {"GMB_SYSID", 0}, - {"GMB_FLASH", 0}, -}; - -/* - find a parameter structure - */ -struct gimbal_param *Gimbal::param_find(const char *name) -{ - for (uint8_t i=0; iname, sizeof(param_value.param_id)); - param_value.param_value = p->value; - param_value.param_count = 0; - param_value.param_index = 0; - param_value.param_type = MAV_PARAM_TYPE_REAL32; - - uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id, - vehicle_component_id, - &mavlink.status, - &msg, ¶m_value); - - uint8_t msgbuf[len]; - len = mavlink_msg_to_send_buffer(msgbuf, &msg); - if (len > 0) { - mav_socket.send(msgbuf, len); - } -} + const uint32_t now_us = AP_HAL::micros(); - -/* - send a report to the vehicle control code over MAVLink -*/ -void Gimbal::send_report(void) -{ - uint32_t now = AP_HAL::millis(); - if (now < 10000) { - // don't send gimbal reports until 10s after startup. This - // avoids a windows threading issue with non-blocking sockets - // and the initial wait on SERIAL0 - return; - } - if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { - ::printf("Gimbal connected to %s:%u\n", target_address, (unsigned)target_port); - mavlink.connected = true; - } - if (!mavlink.connected) { - return; - } - - if (param_send_last_ms && now - param_send_last_ms > 100) { - param_send(&gimbal_params[param_send_idx]); - if (++param_send_idx == ARRAY_SIZE(gimbal_params)) { - printf("Finished sending parameters\n"); - param_send_last_ms = 0; - } - } + _delta_angle = delta_angle; + _delta_velocity = delta_velocity; + _delta_time_us = now_us - delta_start_us; - // check for incoming MAVLink messages - uint8_t buf[100]; - ssize_t ret; - - while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { - for (uint8_t i=0; ivalue = pkt.param_value; - param_send(p); - } - - break; - } - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - mavlink_param_request_list_t pkt; - mavlink_msg_param_request_list_decode(&msg, &pkt); - if (pkt.target_system == 0 && pkt.target_component == MAV_COMP_ID_GIMBAL) { - // start param send - param_send_idx = 0; - param_send_last_ms = AP_HAL::millis(); - } - printf("Gimbal sending %u parameters\n", (unsigned)ARRAY_SIZE(gimbal_params)); - break; - } - default: - debug("got unexpected msg %u\n", msg.msgid); - break; - } - } - } - } - - if (!seen_heartbeat) { - return; - } - mavlink_message_t msg; - uint16_t len; - - if (now - last_heartbeat_ms >= 1000) { - mavlink_heartbeat_t heartbeat; - heartbeat.type = MAV_TYPE_GIMBAL; - heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; - heartbeat.base_mode = 0; - heartbeat.system_status = 0; - heartbeat.mavlink_version = 0; - heartbeat.custom_mode = 0; - - len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, - vehicle_component_id, - &mavlink.status, - &msg, &heartbeat); - - mav_socket.send(&msg.magic, len); - last_heartbeat_ms = now; - } - - /* - send a GIMBAL_REPORT message - */ - uint32_t now_us = AP_HAL::micros(); - if (now_us - last_report_us > reporting_period_ms*1000UL) { - mavlink_gimbal_report_t gimbal_report; - float delta_time = (now_us - last_report_us) * 1.0e-6f; - last_report_us = now_us; - gimbal_report.target_system = vehicle_system_id; - gimbal_report.target_component = vehicle_component_id; - gimbal_report.delta_time = delta_time; - gimbal_report.delta_angle_x = delta_angle.x; - gimbal_report.delta_angle_y = delta_angle.y; - gimbal_report.delta_angle_z = delta_angle.z; - gimbal_report.delta_velocity_x = delta_velocity.x; - gimbal_report.delta_velocity_y = delta_velocity.y; - gimbal_report.delta_velocity_z = delta_velocity.z; - gimbal_report.joint_roll = joint_angles.x; - gimbal_report.joint_el = joint_angles.y; - gimbal_report.joint_az = joint_angles.z; - - len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id, - vehicle_component_id, - &mavlink.status, - &msg, &gimbal_report); - - uint8_t msgbuf[len]; - len = mavlink_msg_to_send_buffer(msgbuf, &msg); - if (len > 0) { - mav_socket.send(msgbuf, len); - } - - delta_velocity.zero(); - delta_angle.zero(); - } + delta_angle.zero(); + delta_velocity.zero(); + delta_start_us = now_us; } } // namespace SITL -#endif // HAL_SIM_GIMBAL_ENABLED +#endif // AP_SIM_GIMBAL_ENABLED diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index 2cedd3a1cf9c3f..d3814383b69ba1 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -18,32 +18,31 @@ #pragma once -#include +#include "SIM_config.h" -#ifndef HAL_SIM_GIMBAL_ENABLED -#define HAL_SIM_GIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && !defined(HAL_BUILD_AP_PERIPH) -#endif +#if AP_SIM_GIMBAL_ENABLED -#if HAL_SIM_GIMBAL_ENABLED - -#include - -#include "SIM_Aircraft.h" +#include +#include namespace SITL { class Gimbal { public: - Gimbal(const struct sitl_fdm &_fdm); - void update(void); + + void update(const class Aircraft &aircraft); + void set_demanded_rates(const Vector3f &rates) { + demanded_angular_rate = rates; + } + + void get_deltas(Vector3f &_delta_angle, Vector3f &_delta_velocity, uint32_t &_delta_time_us); + void get_joint_angles(Vector3f &_angles) { _angles = joint_angles; } private: - const struct sitl_fdm &fdm; - const char *target_address; - const uint16_t target_port; // rotation matrix (gimbal body -> earth) Matrix3f dcm; + bool init_done; // time of last update uint32_t last_update_us; @@ -63,19 +62,16 @@ class Gimbal { Vector3f joint_angles; // physical constraints on joint angles in (roll, pitch, azimuth) order - Vector3f lower_joint_limits; - Vector3f upper_joint_limits; + Vector3f lower_joint_limits{radians(-40), radians(-135), radians(-7.5)}; + Vector3f upper_joint_limits{radians(40), radians(45), radians(7.5)}; - const float travelLimitGain; + const float travelLimitGain = 20; // true gyro bias Vector3f true_gyro_bias; - // reporting variables. gimbal pushes these to vehicle code over - // MAVLink at approx 100Hz - - // reporting period in ms - const float reporting_period_ms; + // time since delta angles/velocities returned + uint32_t delta_start_us; // integral of gyro vector over last time interval. In radians Vector3f delta_angle; @@ -92,32 +88,9 @@ class Gimbal { // gyro bias provided by EKF on vehicle. In rad/s. // Should be subtracted from the gyro readings to get true body // rotatation rates - Vector3f supplied_gyro_bias; - - uint32_t last_report_us; - uint32_t last_heartbeat_ms; - bool seen_heartbeat; - bool seen_gimbal_control; - uint8_t vehicle_system_id; - uint8_t vehicle_component_id; - - SocketAPM_native mav_socket; - struct { - // socket to telem2 on aircraft - bool connected; - mavlink_message_t rxmsg; - mavlink_status_t status; - uint8_t seq; - } mavlink; - - uint32_t param_send_last_ms; - uint8_t param_send_idx; - - void send_report(void); - void param_send(const struct gimbal_param *p); - struct gimbal_param *param_find(const char *name); + // Vector3f supplied_gyro_bias; }; } // namespace SITL -#endif // HAL_SIM_GIMBAL_ENABLED +#endif // AP_SIM_GIMBAL_ENABLED diff --git a/libraries/SITL/SIM_Glider.cpp b/libraries/SITL/SIM_Glider.cpp index c86535b83cc5dc..a1334aac6168c4 100644 --- a/libraries/SITL/SIM_Glider.cpp +++ b/libraries/SITL/SIM_Glider.cpp @@ -231,7 +231,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce float aileron = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4)); float elevator = filtered_servo_angle(input, 2); float rudder = filtered_servo_angle(input, 3); - float balloon = filtered_servo_range(input, 5); + float balloon = MAX(0.0f, filtered_servo_range(input, 5)); // Don't let the balloon receive downwards commands. float balloon_cut = filtered_servo_range(input, 9); // Move balloon upwards using balloon velocity from channel 6 @@ -241,7 +241,7 @@ void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_acce balloon_velocity = Vector3f(-wind_ef.x, -wind_ef.y, -wind_ef.z -balloon_rate * balloon); balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us); const float height_AMSL = 0.01f * (float)home.alt - position.z; - // release at burst height or when channel 9 goes high + // release at burst height or when balloon cut output goes high if (hal.scheduler->is_system_initialized() && (height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "pre-release at %i m AMSL\n", (int)height_AMSL); @@ -380,7 +380,7 @@ bool Glider::update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel) // NED unit vector pointing from tether attachment on plane to attachment on balloon Vector3f tether_unit_vec_ef = relative_position.normalized(); - // NED velocity of attahment point on plane + // NED velocity of attachment point on plane Vector3f attachment_velocity_ef = velocity_ef + dcm * (gyro % tether_pos_bf); // NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index a866762593e05d..4e019dde9b4de4 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -48,6 +48,11 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // add forces from slung payload + add_slungpayload_forces(body_accel); +#endif } /* diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index c1a7afb608f445..3182700dd9ad04 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace SITL { SimRover::SimRover(const char *frame_str) : @@ -41,10 +43,14 @@ SimRover::SimRover(const char *frame_str) : if (vectored_thrust) { printf("Vectored Thrust Rover Simulation Started\n"); } - lock_step_scheduled = true; -} + omni3 = strstr(frame_str, "omni3mecanum") != nullptr; + if (omni3) { + printf("Omni3 Mecanum Rover Simulation Started\n"); + } + lock_step_scheduled = true; +} /* return turning circle (diameter) in meters for steering angle proportion in degrees @@ -92,6 +98,50 @@ float SimRover::calc_lat_accel(float steering_angle, float speed) update the rover simulation by one time step */ void SimRover::update(const struct sitl_input &input) +{ + // how much time has passed? + float delta_time = frame_time_us * 1.0e-6f; + + // update gyro and accel_body according to frame type + if (omni3) { + update_omni3(input, delta_time); + } else { + update_ackermann_or_skid(input, delta_time); + } + + // common to all rovers + + // now in earth frame + Vector3f accel_earth = dcm * accel_body; + accel_earth += Vector3f(0, 0, GRAVITY_MSS); + + // we are on the ground, so our vertical accel is zero + accel_earth.z = 0; + + // work out acceleration as seen by the accelerometers. It sees the kinematic + // acceleration (ie. real movement), plus gravity + accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); + + // new velocity vector + velocity_ef += accel_earth * delta_time; + + // new position vector + position += (velocity_ef * delta_time).todouble(); + + update_external_payload(input); + + // update lat/lon/altitude + update_position(); + time_advance(); + + // update magnetic field + update_mag_field_bf(); +} + +/* + update the ackermann or skid rover simulation by one time step + */ +void SimRover::update_ackermann_or_skid(const struct sitl_input &input, float delta_time) { float steering, throttle; @@ -113,9 +163,6 @@ void SimRover::update(const struct sitl_input &input) } } - // how much time has passed? - float delta_time = frame_time_us * 1.0e-6f; - // speed in m/s in body frame Vector3f velocity_body = dcm.transposed() * velocity_ef; @@ -137,37 +184,80 @@ void SimRover::update(const struct sitl_input &input) dcm.rotate(gyro * delta_time); dcm.normalize(); - // accel in body frame due to motor + // accel in body frame due to motor (excluding gravity) accel_body = Vector3f(accel, 0, 0); // add in accel due to direction change accel_body.y += radians(yaw_rate) * speed; +} - // now in earth frame - Vector3f accel_earth = dcm * accel_body; - accel_earth += Vector3f(0, 0, GRAVITY_MSS); - - // we are on the ground, so our vertical accel is zero - accel_earth.z = 0; - - // work out acceleration as seen by the accelerometers. It sees the kinematic - // acceleration (ie. real movement), plus gravity - accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); +/* + update the omni3 rover simulation by one time step + */ +void SimRover::update_omni3(const struct sitl_input &input, float delta_time) +{ + // in omni3 mode the first three servos are motor speeds + float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f); + float motor2 = 2*((input.servos[1]-1000)/1000.0f - 0.5f); + float motor3 = 2*((input.servos[2]-1000)/1000.0f - 0.5f); + + // use forward kinematics to calculate body frame velocity + Vector3f wheel_ang_vel( + motor1 * omni3_wheel_max_ang_vel, + motor2 * omni3_wheel_max_ang_vel, + motor3 * omni3_wheel_max_ang_vel + ); + + // derivation of forward kinematics for an Omni3Mecanum rover + // A. Gfrerrer. "Geometry and kinematics of the Mecanum wheel", + // Computer Aided Geometric Design 25 (2008) 784–791. + // Retrieved from https://www.geometrie.tugraz.at/gfrerrer/publications/MecanumWheel.pdf. + // + // the frame is equilateral triangle + // + // d[i] = 0.18 m is distance from frame centre to each wheel + // r_w = 0.04725 m is the wheel radius. + // delta = radians(-45) is angle of the roller to the direction of forward rotation + // alpha[i] is the angle the wheel axis is rotated about the body z-axis + // c[i] = cos(alpha[i] + delta) + // s[i] = sin(alpha[i] + delta) + // + // wheel d[i] alpha[i] a_x[i] a_y[i] c[i] s[i] + // 1 0.18 1.04719 0.09 0.15588 0.965925 0.258819 + // 2 0.18 3.14159 -0.18 0.0 -0.707106 0.707106 + // 3 0.18 5.23598 0.09 -0.15588 -0.258819 -0.965925 + // + // k = 1/(r_w * sin(delta)) = -29.930445 is a scale factor + // + // inverse kinematic matrix + // M[i, 0] = k * c[i] + // M[i, 1] = k * s[i] + // M[i, 2] = k * (a_x[i] s[i] - a_y[i] c[i]) + // + // forward kinematics matrix: Minv = M^-1 + constexpr Matrix3f Minv( + -0.0215149, 0.01575, 0.0057649, + -0.0057649, -0.01575, 0.0215149, + 0.0875, 0.0875, 0.0875); + + // twist - this is the target linear and angular velocity + Vector3f twist = Minv * wheel_ang_vel; - // new velocity vector - velocity_ef += accel_earth * delta_time; + // speed in m/s in body frame + Vector3f velocity_body = dcm.transposed() * velocity_ef; - // new position vector - position += (velocity_ef * delta_time).todouble(); + // linear acceleration in m/s/s - very crude model + float accel_x = omni3_max_accel * (twist.x - velocity_body.x) / omni3_max_speed; + float accel_y = omni3_max_accel * (twist.y - velocity_body.y) / omni3_max_speed; - update_external_payload(input); + gyro = Vector3f(0, 0, twist.z); - // update lat/lon/altitude - update_position(); - time_advance(); + // update attitude + dcm.rotate(gyro * delta_time); + dcm.normalize(); - // update magnetic field - update_mag_field_bf(); + // accel in body frame due to motors (excluding gravity) + accel_body = Vector3f(accel_x, accel_y, 0); } } // namespace SITL diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 42009a48a177f7..113bc55dfc1a82 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -51,6 +51,15 @@ class SimRover : public Aircraft { float vectored_angle_max = 90.0f; // maximum angle (in degrees) to which thrust can be turned float vectored_turn_rate_max = 90.0f; // maximum turn rate (in deg/sec) with full throttle angled at 90deg + // omni3 Mecanum related members + bool omni3; // true if vehicle is omni-directional with 3 Mecanum wheels + float omni3_max_speed = 2.3625f; // omni vehicle's maximum forward speed in m/s + float omni3_max_accel = 1.0f; // omni vehicle's maximum forward acceleration in m/s/s + float omni3_wheel_max_ang_vel = 50.0f; // omni vehicle's wheel maximum angular velocity in rad/s + + void update_ackermann_or_skid(const struct sitl_input &input, float delta_time); + void update_omni3(const struct sitl_input &input, float delta_time); + float turn_circle(float steering) const; float calc_yaw_rate(float steering, float speed); float calc_lat_accel(float steering_angle, float speed); diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp index 7d3dba67457ad1..846f077f43f967 100644 --- a/libraries/SITL/SIM_Sailboat.cpp +++ b/libraries/SITL/SIM_Sailboat.cpp @@ -32,6 +32,8 @@ namespace SITL { #define STEERING_SERVO_CH 0 // steering controlled by servo output 1 #define MAINSAIL_SERVO_CH 3 // main sail controlled by servo output 4 #define THROTTLE_SERVO_CH 2 // throttle controlled by servo output 3 +#define MOTORLEFT_SERVO_CH 0 // skid-steering left motor controlled by servo output 1 +#define MOTORRIGHT_SERVO_CH 2 // skid-steering right motor controlled by servo output 3 #define DIRECT_WING_SERVO_CH 4 // very roughly sort of a stability factors for waves @@ -45,6 +47,7 @@ Sailboat::Sailboat(const char *frame_str) : sail_area(1.0) { motor_connected = (strcmp(frame_str, "sailboat-motor") == 0); + skid_steering = strstr(frame_str, "skid") != nullptr; lock_step_scheduled = true; } @@ -97,13 +100,19 @@ float Sailboat::get_turn_circle(float steering) const // return yaw rate in deg/sec given a steering input (in the range -1 to +1) and speed in m/s float Sailboat::get_yaw_rate(float steering, float speed) const { - if (is_zero(steering) || is_zero(speed)) { - return 0; + float rate = 0.0f; + if (is_zero(steering) || (!skid_steering && is_zero(speed))) { + return rate; + } + + if (is_zero(speed) && skid_steering) { + rate = steering * M_PI * 5; + } else { + float d = get_turn_circle(steering); + float c = M_PI * d; + float t = c / speed; + rate = 360.0f / t; } - float d = get_turn_circle(steering); - float c = M_PI * d; - float t = c / speed; - float rate = 360.0f / t; return rate; } @@ -179,7 +188,14 @@ void Sailboat::update(const struct sitl_input &input) update_wind(input); // in sailboats the steering controls the rudder, the throttle controls the main sail position - float steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f); + float steering = 0.0f; + if (skid_steering) { + float steering_left = 2.0f*((input.servos[MOTORLEFT_SERVO_CH]-1000)/1000.0f - 0.5f); + float steering_right = 2.0f*((input.servos[MOTORRIGHT_SERVO_CH]-1000)/1000.0f - 0.5f); + steering = steering_left - steering_right; + } else { + steering = 2*((input.servos[STEERING_SERVO_CH]-1000)/1000.0f - 0.5f); + } // calculate apparent wind in earth-frame (this is the direction the wind is coming from) // Note than the SITL wind direction is defined as the direction the wind is travelling to @@ -257,8 +273,14 @@ void Sailboat::update(const struct sitl_input &input) // gives throttle force == hull drag at 10m/s float throttle_force = 0.0f; if (motor_connected) { - const uint16_t throttle_out = constrain_int16(input.servos[THROTTLE_SERVO_CH], 1000, 2000); - throttle_force = (throttle_out-1500) * 0.1f; + if (skid_steering) { + const uint16_t throttle_left = constrain_int16(input.servos[MOTORLEFT_SERVO_CH], 1000, 2000); + const uint16_t throttle_right = constrain_int16(input.servos[MOTORRIGHT_SERVO_CH], 1000, 2000); + throttle_force = (0.5f*(throttle_left + throttle_right)-1500) * 0.1f; + } else { + const uint16_t throttle_out = constrain_int16(input.servos[THROTTLE_SERVO_CH], 1000, 2000); + throttle_force = (throttle_out-1500) * 0.1f; + } } // accel in body frame due acceleration from sail and deceleration from hull friction diff --git a/libraries/SITL/SIM_Sailboat.h b/libraries/SITL/SIM_Sailboat.h index 4cb70e0448309a..e17d2c05dcd424 100644 --- a/libraries/SITL/SIM_Sailboat.h +++ b/libraries/SITL/SIM_Sailboat.h @@ -41,6 +41,7 @@ class Sailboat : public Aircraft { protected: bool motor_connected; // true if this frame has a motor + bool skid_steering; // true if this vehicle is a skid-steering vehicle float sail_area; // 1.0 for normal area private: diff --git a/libraries/SITL/SIM_SlungPayload.cpp b/libraries/SITL/SIM_SlungPayload.cpp new file mode 100644 index 00000000000000..03b5363300d7cb --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.cpp @@ -0,0 +1,458 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulate a slung payload +*/ + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include "SIM_SlungPayload.h" +#include "SITL.h" +#include +#include "SIM_Aircraft.h" +#include +#include +#include + +using namespace SITL; + +// SlungPayloadSim parameters +const AP_Param::GroupInfo SlungPayloadSim::var_info[] = { + // @Param: ENABLE + // @DisplayName: Slung Payload Sim enable/disable + // @Description: Slung Payload Sim enable/disable + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, SlungPayloadSim, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: WEIGHT + // @DisplayName: Slung Payload weight + // @Description: Slung Payload weight in kg + // @Units: kg + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("WEIGHT", 2, SlungPayloadSim, weight_kg, 1.0), + + // @Param: LINELEN + // @DisplayName: Slung Payload line length + // @Description: Slung Payload line length in meters + // @Units: m + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("LINELEN", 3, SlungPayloadSim, line_length, 30.0), + + // @Param: DRAG + // @DisplayName: Slung Payload drag coefficient + // @Description: Slung Payload drag coefficient. Higher values increase drag and slow the payload more quickly + // @Units: m + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("DRAG", 4, SlungPayloadSim, drag_coef, 1), + + // @Param: SYSID + // @DisplayName: Slung Payload MAVLink system ID + // @Description: Slung Payload MAVLink system id to distinguish it from others on the same network + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SYSID", 5, SlungPayloadSim, sys_id, 2), + + AP_GROUPEND +}; + +// SlungPayloadSim handles interaction with main vehicle +SlungPayloadSim::SlungPayloadSim() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// update the SlungPayloadSim's state using the vehicle's earth-frame position, velocity and acceleration +void SlungPayloadSim::update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef) +{ + if (!enable) { + return; + } + + // initialise slung payload location + const uint32_t now_us = AP_HAL::micros(); + if (!initialised) { + // capture EKF origin + auto *sitl = AP::sitl(); + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return; + } + + // more initialisation + last_update_us = now_us; + initialised = true; + } + + // calculate dt and update slung payload + const float dt = (now_us - last_update_us)*1.0e-6; + last_update_us = now_us; + update_payload(veh_pos, veh_vel_ef, veh_accel_ef, dt); + + // send payload location to GCS at 5hz + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_report_ms >= reporting_period_ms) { + last_report_ms = now_ms; + send_report(); + write_log(); + } +} + +// get earth-frame forces on the vehicle from slung payload +// returns true on success and fills in forces_ef argument, false on failure +bool SlungPayloadSim::get_forces_on_vehicle(Vector3f& forces_ef) const +{ + if (!enable) { + return false; + } + + forces_ef = veh_forces_ef; + return true; +} + +// send a report to the vehicle control code over MAVLink +void SlungPayloadSim::send_report(void) +{ + if (!mavlink_connected && mav_socket.connect(target_address, target_port)) { + ::printf("SlungPayloadSim connected to %s:%u\n", target_address, (unsigned)target_port); + mavlink_connected = true; + } + if (!mavlink_connected) { + return; + } + + // get current time + uint32_t now_ms = AP_HAL::millis(); + + // send heartbeat at 1hz + const uint8_t component_id = MAV_COMP_ID_USER11; + if (now_ms - last_heartbeat_ms >= 1000) { + last_heartbeat_ms = now_ms; + + const mavlink_heartbeat_t heartbeat{ + custom_mode: 0, + type : MAV_TYPE_AIRSHIP, + autopilot : MAV_AUTOPILOT_INVALID, + base_mode: 0, + system_status: 0, + mavlink_version: 0, + }; + + mavlink_message_t msg; + mavlink_msg_heartbeat_encode_status( + sys_id.get(), + component_id, + &mav_status, + &msg, + &heartbeat); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + mav_socket.send(buf, len); + } + + // send a GLOBAL_POSITION_INT messages + { + Location payload_loc; + int32_t alt_amsl_cm, alt_rel_cm; + if (!get_payload_location(payload_loc) || + !payload_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) || + !payload_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) { + return; + } + const mavlink_global_position_int_t global_position_int{ + time_boot_ms: now_ms, + lat: payload_loc.lat, + lon: payload_loc.lng, + alt: alt_amsl_cm * 10, // amsl alt in mm + relative_alt: alt_rel_cm * 10, // relative alt in mm + vx: int16_t(velocity_NED.x * 100), // velocity in cm/s + vy: int16_t(velocity_NED.y * 100), // velocity in cm/s + vz: int16_t(velocity_NED.z * 100), // velocity in cm/s + hdg: 0 // heading in centi-degrees + }; + mavlink_message_t msg; + mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } + + // send ATTITUDE so MissionPlanner can display orientation + { + const mavlink_attitude_t attitude{ + time_boot_ms: now_ms, + roll: 0, + pitch: 0, + yaw: 0, // heading in radians + rollspeed: 0, + pitchspeed: 0, + yawspeed: 0 + }; + mavlink_message_t msg; + mavlink_msg_attitude_encode_status( + sys_id, + component_id, + &mav_status, + &msg, + &attitude); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } +} + +// write onboard log +void SlungPayloadSim::write_log() +{ +#if HAL_LOGGING_ENABLED + // write log of slung payload state + // @LoggerMessage: SLUP + // @Description: Slung payload + // @Field: TimeUS: Time since system startup + // @Field: Land: 1 if payload is landed, 0 otherwise + // @Field: Tens: Tension ratio, 1 if line is taut, 0 if slack + // @Field: Len: Line length + // @Field: PN: Payload position as offset from vehicle in North direction + // @Field: PE: Payload position as offset from vehicle in East direction + // @Field: PD: Payload position as offset from vehicle in Down direction + // @Field: VN: Payload velocity in North direction + // @Field: VE: Payload velocity in East direction + // @Field: VD: Payload velocity in Down direction + // @Field: AN: Payload acceleration in North direction + // @Field: AE: Payload acceleration in East direction + // @Field: AD: Payload acceleration in Down direction + // @Field: VFN: Force on vehicle in North direction + // @Field: VFE: Force on vehicle in East direction + // @Field: VFD: Force on vehicle in Down direction + AP::logger().WriteStreaming("SLUP", + "TimeUS,Land,Tens,Len,PN,PE,PD,VN,VE,VD,AN,AE,AD,VFN,VFE,VFD", // labels + "s-%mmmmnnnooo---", // units + "F-20000000000000", // multipliers + "Qbffffffffffffff", // format + AP_HAL::micros64(), + (uint8_t)landed, + (float)tension_ratio, + (float)payload_to_veh.length(), + (double)-payload_to_veh.x, + (double)-payload_to_veh.y, + (double)-payload_to_veh.z, + (double)velocity_NED.x, + (double)velocity_NED.y, + (double)velocity_NED.z, + (double)accel_NED.x, + (double)accel_NED.y, + (double)accel_NED.z, + (double)veh_forces_ef.x, + (double)veh_forces_ef.y, + (double)veh_forces_ef.z); +#endif +} + +// returns true on success and fills in payload_loc argument, false on failure +bool SlungPayloadSim::get_payload_location(Location& payload_loc) const +{ + // get EKF origin + auto *sitl = AP::sitl(); + if (sitl == nullptr) { + return false; + } + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return false; + } + + // calculate location + payload_loc = ekf_origin; + payload_loc.offset(position_NED); + return true; +} + +// update the slung payloads position, velocity, acceleration +// vehicle position, velocity and acceleration should be in earth-frame NED frame +void SlungPayloadSim::update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt) +{ + // how we calculate the payload's position, velocity and acceleration + // 1. update the payload's position, velocity using the previous iterations acceleration + // 2. check that the payload does not fall below the terrain + // 3. check if the line is taught and that the payload does not move more than the line length from the vehicle + // 4. calculate gravity and drag forces on the payload + // 5. calculate the tension force between the payload and vehicle including force countering gravity, drag and centripetal force + // 6. update the payload's acceleration using the sum of the above forces + + // initialise position_NED from vehicle position + if (position_NED.is_zero()) { + if (!veh_pos.is_zero()) { + position_NED = veh_pos; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: initialised at %f %f %f", position_NED.x, position_NED.y, position_NED.z); + } + return; + } + + // integrate previous iterations acceleration into velocity and position + velocity_NED += accel_NED * dt; + position_NED += (velocity_NED * dt).todouble(); + + // calculate distance from payload to vehicle + payload_to_veh = veh_pos - position_NED; + float payload_to_veh_length = payload_to_veh.length(); + + // update landed state by checking if payload has dropped below terrain + Location payload_loc; + if (get_payload_location(payload_loc)) { + int32_t alt_terrain_cm; + bool landed_orig = landed; + if (payload_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_terrain_cm)) { + + // landed if below terrain + if (alt_terrain_cm <= 0) { + landed = true; + + // raise payload to match terrain + position_NED.z += (alt_terrain_cm * 0.01); + + // zero out velocity and acceleration in horizontal and downward direction + velocity_NED.xy().zero(); + velocity_NED.z = MIN(velocity_NED.z, 0); + accel_NED.xy().zero(); + accel_NED.z = MIN(accel_NED.z, 0); + + // zero out forces on vehicle + veh_forces_ef.zero(); + } + + // not landed if above terrain + if (landed && (alt_terrain_cm > 1)) { + landed = false; + } + } + + // inform user if landed state has changed + if (landed != landed_orig) { + if (landed) { + // get payload location again in case it has moved + get_payload_location(payload_loc); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: landed lat:%f lon:%f alt:%4.1f", + (double)payload_loc.lat * 1e-7, + (double)payload_loc.lng * 1e-7, + (double)payload_loc.alt * 1e-2); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SlungPayload: liftoff"); + } + } + } + + // calculate forces of gravity + Vector3f force_gravity_NED = Vector3f(0.0f, 0.0f, GRAVITY_MSS * weight_kg); + + // tension force on payload (resists gravity, drag, centripetal force) + Vector3f tension_force_NED; + + // tension ratio to smooth transition from line being taut to slack + tension_ratio = 0; + + // calculate drag force (0.5 * drag_coef * air_density * velocity^2 * surface area) + Vector3f force_drag_NED; + if (drag_coef > 0 && !velocity_NED.is_zero()) { + const float air_density = 1.225; // 1.225 kg/m^3 (standard sea-level density) + const float surface_area_m2 = 0.07; // 30cm diameter sphere + const float drag_force = 0.5 * drag_coef * air_density * velocity_NED.length_squared() * surface_area_m2; + force_drag_NED = -velocity_NED.normalized() * drag_force; + } + + // sanity check payload distance from vehicle and calculate tension force + if (is_positive(payload_to_veh_length)) { + + // calculate unit vector from payload to vehicle + const Vector3f payload_to_veh_norm = payload_to_veh.normalized().tofloat(); + + // ensure payload is no more than line_length from vehicle + if (payload_to_veh_length > line_length) { + payload_to_veh *= (line_length / payload_to_veh_length); + position_NED = veh_pos - payload_to_veh; + } + + // calculate tension ratio as value between 0 and 1 + // tension ratio is 0 when payload-to-vehicle distance is 10cm less than line length + // tension ratio is 1 when payload-to-vehicle distance is equal to line length + tension_ratio = constrain_float(1.0 - (line_length - payload_to_veh_length) * 10, 0, 1); + + // calculate tension forces when line is taut + if (is_positive(tension_ratio)) { + + // tension resists gravity if vehicle is above payload + if (is_negative(payload_to_veh_norm.z)) { + tension_force_NED += -force_gravity_NED.projected(payload_to_veh_norm); + } + + // calculate tension force resulting from velocity difference between vehicle and payload + // use time constant to convert velocity to acceleration + const float velocity_to_accel_TC = 2.0; + Vector3f velocity_diff_NED = (veh_vel_ef - velocity_NED).projected(payload_to_veh_norm); + + // add to tension force if the vehicle is moving faster than the payload + if (vectors_same_direction(velocity_diff_NED, payload_to_veh_norm)) { + tension_force_NED += velocity_diff_NED / velocity_to_accel_TC * weight_kg; + } + + // tension force resisting payload drag + tension_force_NED += -force_drag_NED.projected(payload_to_veh_norm); + + // calculate centripetal force + const Vector3f velocity_parallel = velocity_NED.projected(payload_to_veh_norm); + const Vector3f velocity_perpendicular = velocity_NED - velocity_parallel; + const float tension_force_centripetal = velocity_perpendicular.length_squared() * weight_kg / line_length; + const Vector3f tension_force_centripetal_NED = payload_to_veh_norm * tension_force_centripetal; + + // add centripetal force to tension force + tension_force_NED += tension_force_centripetal_NED; + + // scale tension force by tension ratio + tension_force_NED *= tension_ratio; + } + } + + // force on vehicle is opposite to tension force on payload + veh_forces_ef = -tension_force_NED; + + // convert force to acceleration (f=m*a => a=f/m) + accel_NED = (force_gravity_NED + force_drag_NED + tension_force_NED) / weight_kg; + + // if slung payload is landed we zero out downward (e.g positive) acceleration + if (landed) { + accel_NED.z = MIN(accel_NED.z, 0); + // should probably zero out forces_ef vertical component as well? + } +} + +// returns true if the two vectors point in the same direction, false if perpendicular or opposite +bool SlungPayloadSim::vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const +{ + // check both vectors are non-zero + if (v1.is_zero() || v2.is_zero()) { + return false; + } + return v1.dot(v2) > 0; +} + +#endif diff --git a/libraries/SITL/SIM_SlungPayload.h b/libraries/SITL/SIM_SlungPayload.h new file mode 100644 index 00000000000000..3f668af5472945 --- /dev/null +++ b/libraries/SITL/SIM_SlungPayload.h @@ -0,0 +1,102 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulate a payload slung from a line under a vehicle +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SLUNGPAYLOAD_ENABLED + +#include +#include +#include +#include + +namespace SITL { + +// SlungPayloadSim handles interaction with main vehicle +class SlungPayloadSim { +public: + friend class SlungPayload; + + // constructor + SlungPayloadSim(); + + // update the SlungPayloadSim's state using thevehicle's earth-frame position, velocity and acceleration + void update(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef); + + // get earth-frame forces on the vehicle from slung payload + // returns true on success and fills in forces_ef argument, false on failure + bool get_forces_on_vehicle(Vector3f& forces_ef) const; + + // parameter table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // parameters + AP_Int8 enable; // enable parameter + AP_Float weight_kg; // payload weight in kg + AP_Float line_length; // line length in meters + AP_Int8 sys_id; // mavlink system id for reporting to GCS + AP_Float drag_coef; // drag coefficient (spheres=0.5, cubes=1.05, barrels=0.8~1.2) + + // send MAVLink messages to GCS + void send_report(); + + // write onboard log + void write_log(); + + // get payload location + // returns true on success and fills in payload_loc argument, false on failure + bool get_payload_location(Location& payload_loc) const; + + // update the slung payload's position, velocity, acceleration + // vehicle position, velocity and acceleration should be in earth-frame NED frame + void update_payload(const Vector3p& veh_pos, const Vector3f& veh_vel_ef, const Vector3f& veh_accel_ef, float dt); + + // returns true if the two vectors point in the same direction, false if perpendicular or opposite + bool vectors_same_direction(const Vector3f& v1, const Vector3f& v2) const; + + // socket connection variables + const char *target_address = "127.0.0.1"; + const uint16_t target_port = 5763; + SocketAPM_native mav_socket { false }; + bool initialised; // true if this class has been initialised + uint32_t last_update_us; // system time of last update + + // mavlink reporting variables + const float reporting_period_ms = 200; // reporting period in ms + uint32_t last_report_ms; // system time of last MAVLink report sent to GCS + uint32_t last_heartbeat_ms; // system time of last MAVLink heartbeat sent to GCS + bool mavlink_connected; // true if a mavlink connection has been established + mavlink_status_t mav_status; // reported mavlink status + + // payload variables + bool landed = true; // true if the payload is on the ground + float tension_ratio; // 0 if line is loose, 1 if completely taut + Vector3p payload_to_veh;// distance vector (in meters in NED frame) from payload to vehicle (used for reporting purposes) + Vector3p position_NED; // payload's position (as an offset from EKF origin? offset from vehicle?) in meters + Vector3f velocity_NED; // payload velocity + Vector3f accel_NED; // payload's acceleration + Vector3f veh_forces_ef; // earth-frame forces on the vehicle caused by the payload +}; + +} // namespace SITL + +#endif // AP_SIM_SLUNGPAYLOAD_ENABLED diff --git a/libraries/SITL/SIM_SoloGimbal.cpp b/libraries/SITL/SIM_SoloGimbal.cpp new file mode 100644 index 00000000000000..dccf15727249ea --- /dev/null +++ b/libraries/SITL/SIM_SoloGimbal.cpp @@ -0,0 +1,281 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + gimbal simulator class for MAVLink gimbal +*/ + +#include "SIM_SoloGimbal.h" + +#if AP_SIM_SOLOGIMBAL_ENABLED + +#include + +#include "SIM_Aircraft.h" + +extern const AP_HAL::HAL& hal; + +#define GIMBAL_DEBUG 0 + +#if GIMBAL_DEBUG +#define debug(fmt, args...) do { printf("GIMBAL: " fmt, ##args); } while(0) +#else +#define debug(fmt, args...) do { } while(0) +#endif + +namespace SITL { + +/* + update the gimbal state +*/ +void SoloGimbal::update(const Aircraft &aircraft) +{ + gimbal.update(aircraft); + + // see if we should do a report + send_report(); +} + +static struct gimbal_param { + const char *name; + float value; +} gimbal_params[] = { + {"GMB_OFF_ACC_X", 0}, + {"GMB_OFF_ACC_Y", 0}, + {"GMB_OFF_ACC_Z", 0}, + {"GMB_GN_ACC_X", 0}, + {"GMB_GN_ACC_Y", 0}, + {"GMB_GN_ACC_Z", 0}, + {"GMB_OFF_GYRO_X", 0}, + {"GMB_OFF_GYRO_Y", 0}, + {"GMB_OFF_GYRO_Z", 0}, + {"GMB_OFF_JNT_X", 0}, + {"GMB_OFF_JNT_Y", 0}, + {"GMB_OFF_JNT_Z", 0}, + {"GMB_K_RATE", 0}, + {"GMB_POS_HOLD", 0}, + {"GMB_MAX_TORQUE", 0}, + {"GMB_SND_TORQUE", 0}, + {"GMB_SYSID", 0}, + {"GMB_FLASH", 0}, +}; + +/* + find a parameter structure + */ +struct gimbal_param *SoloGimbal::param_find(const char *name) +{ + for (uint8_t i=0; iname, sizeof(param_value.param_id)); + param_value.param_value = p->value; + param_value.param_count = 0; + param_value.param_index = 0; + param_value.param_type = MAV_PARAM_TYPE_REAL32; + + uint16_t len = mavlink_msg_param_value_encode_status(vehicle_system_id, + gimbal_component_id, + &mavlink.status, + &msg, ¶m_value); + + uint8_t msgbuf[len]; + len = mavlink_msg_to_send_buffer(msgbuf, &msg); + if (len > 0) { + mav_socket.send(msgbuf, len); + } +} + + +/* + send a report to the vehicle control code over MAVLink +*/ +void SoloGimbal::send_report(void) +{ + uint32_t now = AP_HAL::millis(); + if (now < 10000) { + // don't send gimbal reports until 10s after startup. This + // avoids a windows threading issue with non-blocking sockets + // and the initial wait on SERIAL0 + return; + } + if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { + ::printf("SoloGimbal connected to %s:%u\n", target_address, (unsigned)target_port); + mavlink.connected = true; + } + if (!mavlink.connected) { + return; + } + + if (param_send_last_ms && now - param_send_last_ms > 100) { + param_send(&gimbal_params[param_send_idx]); + if (++param_send_idx == ARRAY_SIZE(gimbal_params)) { + printf("Finished sending parameters\n"); + param_send_last_ms = 0; + } + } + + // check for incoming MAVLink messages + uint8_t buf[100]; + ssize_t ret; + + while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { + for (uint8_t i=0; ivalue = pkt.param_value; + param_send(p); + } + + break; + } + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + mavlink_param_request_list_t pkt; + mavlink_msg_param_request_list_decode(&msg, &pkt); + if (pkt.target_system == 0 && pkt.target_component == MAV_COMP_ID_GIMBAL) { + // start param send + param_send_idx = 0; + param_send_last_ms = AP_HAL::millis(); + } + printf("SoloGimbal sending %u parameters\n", (unsigned)ARRAY_SIZE(gimbal_params)); + break; + } + default: + debug("got unexpected msg %u\n", msg.msgid); + break; + } + } + } + } + + if (!seen_heartbeat) { + return; + } + mavlink_message_t msg; + uint16_t len; + + if (now - last_heartbeat_ms >= 1000) { + mavlink_heartbeat_t heartbeat; + heartbeat.type = MAV_TYPE_GIMBAL; + heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; + heartbeat.base_mode = 0; + heartbeat.system_status = 0; + heartbeat.mavlink_version = 0; + heartbeat.custom_mode = 0; + + len = mavlink_msg_heartbeat_encode_status(vehicle_system_id, + gimbal_component_id, + &mavlink.status, + &msg, &heartbeat); + + mav_socket.send(&msg.magic, len); + last_heartbeat_ms = now; + } + + /* + send a GIMBAL_REPORT message + */ + uint32_t now_us = AP_HAL::micros(); + if (now_us - last_report_us > reporting_period_ms*1000UL) { + last_report_us = now_us; + + uint32_t delta_time_us; + Vector3f delta_angle; + Vector3f delta_velocity; + gimbal.get_deltas(delta_angle, delta_velocity, delta_time_us); + + Vector3f joint_angles; + gimbal.get_joint_angles(joint_angles); + + mavlink_gimbal_report_t gimbal_report; + gimbal_report.target_system = vehicle_system_id; + gimbal_report.target_component = vehicle_component_id; + gimbal_report.delta_time = delta_time_us * 1e-6; + gimbal_report.delta_angle_x = delta_angle.x; + gimbal_report.delta_angle_y = delta_angle.y; + gimbal_report.delta_angle_z = delta_angle.z; + gimbal_report.delta_velocity_x = delta_velocity.x; + gimbal_report.delta_velocity_y = delta_velocity.y; + gimbal_report.delta_velocity_z = delta_velocity.z; + gimbal_report.joint_roll = joint_angles.x; + gimbal_report.joint_el = joint_angles.y; + gimbal_report.joint_az = joint_angles.z; + + len = mavlink_msg_gimbal_report_encode_status(vehicle_system_id, + gimbal_component_id, + &mavlink.status, + &msg, &gimbal_report); + + uint8_t msgbuf[len]; + len = mavlink_msg_to_send_buffer(msgbuf, &msg); + if (len > 0) { + mav_socket.send(msgbuf, len); + } + + delta_velocity.zero(); + delta_angle.zero(); + } +} + +} // namespace SITL + +#endif // AP_SIM_SOLOGIMBAL_ENABLED diff --git a/libraries/SITL/SIM_SoloGimbal.h b/libraries/SITL/SIM_SoloGimbal.h new file mode 100644 index 00000000000000..d3fdb6d2d2188a --- /dev/null +++ b/libraries/SITL/SIM_SoloGimbal.h @@ -0,0 +1,88 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + gimbal simulator class + +./Tools/autotest/sim_vehicle.py -D -G -v ArduCopter --mavlink-gimbal +param set MNT1_TYPE 2 +param set RC6_OPTION 213 # MOUNT1_PITCH +rc 6 1818 # for neutral pitch input +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_SOLOGIMBAL_ENABLED + +#include "SIM_Gimbal.h" + +#include +#include +#include + +namespace SITL { + +class SoloGimbal { +public: + + SoloGimbal() {} + void update(const Aircraft &aicraft); + +private: + + const char *target_address = "127.0.0.1"; + const uint16_t target_port = 5762; + + // physic simulation of gimbal: + Gimbal gimbal; + + // reporting variables. gimbal pushes these to vehicle code over + // MAVLink at approx 100Hz + + // reporting period in ms + const float reporting_period_ms = 10; + + uint32_t last_report_us; + uint32_t last_heartbeat_ms; + bool seen_heartbeat; + bool seen_gimbal_control; + uint8_t vehicle_system_id; + uint8_t vehicle_component_id; + + SocketAPM_native mav_socket{false}; + struct { + // socket to telem2 on aircraft + bool connected; + mavlink_message_t rxmsg; + mavlink_status_t status; + uint8_t seq; + } mavlink; + + uint32_t param_send_last_ms; + uint8_t param_send_idx; + + // component ID we send from: + const uint8_t gimbal_component_id = 154; // MAV_COMP_ID_GIMBAL + + void send_report(void); + void param_send(const struct gimbal_param *p); + struct gimbal_param *param_find(const char *name); +}; + +} // namespace SITL + +#endif // AP_SIM_SOLOGIMBAL_ENABLED + diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index ed4c1bcaedd53d..b3f9ecf6a4d89f 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -214,14 +214,24 @@ void VectorNav::update(void) } char receive_buf[50]; - const ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf)); - if (n > 0) { - if (strncmp(receive_buf, "$VNRRG,01", 9) == 0) { + ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf)); + if (n <= 0) { + return; + } + + // avoid parsing the NMEA stream here by making assumptions about + // how we receive configuration strings. Generally we can just + // echo back the configuration string to make the driver happy. + if (n >= 9) { + // intercept device-version query, respond with simulated version: + const char *ver_query_string = "$VNRRG,01"; + if (strncmp(receive_buf, ver_query_string, strlen(ver_query_string)) == 0) { nmea_printf("$VNRRG,01,VN-300-SITL"); - } else { - nmea_printf("$%s", receive_buf); + // consume the query so we don't "respond" twice: + memmove(&receive_buf[0], &receive_buf[strlen(ver_query_string)], n - strlen(ver_query_string)); + n -= strlen(ver_query_string); } } - + write_to_autopilot(receive_buf, n); } diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index bd5d07e799c50f..8ec2d7ff42c48c 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -51,6 +51,10 @@ #define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_SLUNGPAYLOAD_ENABLED +#define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_TSYS03_ENABLED #define AP_SIM_TSYS03_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif @@ -121,3 +125,12 @@ #ifndef AP_SIM_GLIDER_ENABLED #define AP_SIM_GLIDER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif + +#ifndef AP_SIM_SOLOGIMBAL_ENABLED +#define AP_SIM_SOLOGIMBAL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && HAL_MAVLINK_BINDINGS_ENABLED) +#endif + +#ifndef AP_SIM_GIMBAL_ENABLED +#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) +#endif + diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 49c344b6e7fb0d..681d5d3dbf440c 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -924,11 +924,21 @@ const AP_Param::GroupInfo SIM::var_ins[] = { #if INS_MAX_INSTANCES > 2 AP_GROUPINFO("GYR3_RND", 10, SIM, gyro_noise[2], 0), #endif + // @Param: ACC1_RND + // @DisplayName: Accel 1 motor noise factor + // @Description: scaling factor for simulated vibration from motors + // @User: Advanced AP_GROUPINFO("ACC1_RND", 11, SIM, accel_noise[0], 0), #if INS_MAX_INSTANCES > 1 + // @Param: ACC2_RND + // @DisplayName: Accel 2 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC2_RND", 12, SIM, accel_noise[1], 0), #endif #if INS_MAX_INSTANCES > 2 + // @Param: ACC3_RND + // @DisplayName: Accel 3 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC3_RND", 13, SIM, accel_noise[2], 0), #endif // @Param: GYR1_SCALE @@ -1106,6 +1116,9 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Vector3Parameter: 1 AP_GROUPINFO("GYR4_SCALE", 36, SIM, gyro_scale[3], 0), + // @Param: ACC4_RND + // @DisplayName: Accel 4 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC4_RND", 37, SIM, accel_noise[3], 0), AP_GROUPINFO("GYR4_RND", 38, SIM, gyro_noise[3], 0), @@ -1156,6 +1169,9 @@ const AP_Param::GroupInfo SIM::var_ins[] = { // @Vector3Parameter: 1 AP_GROUPINFO("GYR5_SCALE", 43, SIM, gyro_scale[4], 0), + // @Param: ACC5_RND + // @DisplayName: Accel 5 motor noise factor + // @CopyFieldsFrom: SIM_ACC1_RND AP_GROUPINFO("ACC5_RND", 44, SIM, accel_noise[4], 0), AP_GROUPINFO("GYR5_RND", 45, SIM, gyro_noise[4], 0), @@ -1233,6 +1249,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider), #endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + // @Group: SLUP_ + // @Path: ./SIM_SlungPayload.cpp + AP_SUBGROUPINFO(slung_payload_sim, "SLUP_", 4, SIM::ModelParm, SlungPayloadSim), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3b197fe8469e52..d3ad9ebbd962aa 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -27,6 +27,7 @@ #include "SIM_FETtecOneWireESC.h" #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" +#include "SIM_SlungPayload.h" #include "SIM_GPS.h" #include "SIM_DroneCANDevice.h" #include "SIM_ADSB_Sagetech_MXS.h" @@ -322,6 +323,9 @@ class SIM { #endif #if AP_SIM_GLIDER_ENABLED Glider *glider_ptr; +#endif +#if AP_SIM_SLUNGPAYLOAD_ENABLED + SlungPayloadSim slung_payload_sim; #endif }; ModelParm models; diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc index 2465ace6c8cb01..43d8a9ed909e18 160000 --- a/modules/DroneCAN/dronecan_dsdlc +++ b/modules/DroneCAN/dronecan_dsdlc @@ -1 +1 @@ -Subproject commit 2465ace6c8cb0148e3ff5865aa9e4dd17d691a71 +Subproject commit 43d8a9ed909e18f169c001a0f418edc72269b36b