diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index d38d8af77516d..590b40e0acd93 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -279,8 +279,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_NAV_CONTROLLER_OUTPUT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif }; static const ap_message STREAM_POSITION_msgs[] = { MSG_LOCATION, diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5af9342850878..2d620deb8305c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -237,6 +237,8 @@ class Copter : public AP_Vehicle { friend class _AutoTakeoff; + friend class PayloadPlace; + Copter(void); private: diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 770748e9644b1..ee174cc933f35 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -508,8 +508,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, #if AP_FENCE_ENABLED MSG_FENCE_STATUS, @@ -569,7 +571,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 3254dfde78946..40acda0f05c59 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -154,9 +154,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) auto_yaw.set_mode_to_default(false); #if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled - if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - copter.camera_mount.set_mode_to_default(); - } + copter.camera_mount.clear_roi_target(); #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7a7b6900c2fa7..8b2010c0d242f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -638,3 +638,7 @@ #ifndef AC_CUSTOMCONTROL_MULTI_ENABLED #define AC_CUSTOMCONTROL_MULTI_ENABLED FRAME_CONFIG == MULTICOPTER_FRAME && AP_CUSTOMCONTROL_ENABLED #endif + +#ifndef AC_PAYLOAD_PLACE_ENABLED +#define AC_PAYLOAD_PLACE_ENABLED 1 +#endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d5c55be60fea1..7f1175552537b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -76,18 +76,6 @@ enum class AirMode { AIRMODE_ENABLED, }; -enum PayloadPlaceStateType { - PayloadPlaceStateType_FlyToLocation, - PayloadPlaceStateType_Descent_Start, - PayloadPlaceStateType_Descent, - PayloadPlaceStateType_Release, - PayloadPlaceStateType_Releasing, - PayloadPlaceStateType_Delay, - PayloadPlaceStateType_Ascent_Start, - PayloadPlaceStateType_Ascent, - PayloadPlaceStateType_Done, -}; - // bit options for DEV_OPTIONS parameter enum DevOptions { DevOptionADSBMAVLink = 1, diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 888bb87025fe5..3313ab35509bb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -25,6 +25,10 @@ Mode::Mode(void) : G_Dt(copter.G_Dt) { }; +#if AC_PAYLOAD_PLACE_ENABLED +PayloadPlace Mode::payload_place; +#endif + // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 11cbf3afedce0..3de08e4766a69 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -29,7 +29,41 @@ class _AutoTakeoff { Vector3p complete_pos; // target takeoff position as offset from ekf origin in cm }; +#if AC_PAYLOAD_PLACE_ENABLED +class PayloadPlace { +public: + void run(); + void start_descent(); + bool verify(); + + enum class State : uint8_t { + FlyToLocation, + Descent_Start, + Descent, + Release, + Releasing, + Delay, + Ascent_Start, + Ascent, + Done, + }; + + // these are set by the Mission code: + State state = State::Descent_Start; // records state of payload place + float descent_max_cm; + +private: + + uint32_t descent_established_time_ms; // milliseconds + uint32_t place_start_time_ms; // milliseconds + float descent_thrust_level; + float descent_start_altitude_cm; + float descent_speed_cms; +}; +#endif + class Mode { + friend class PayloadPlace; public: @@ -167,6 +201,11 @@ class Mode { land_run_vertical_control(pause_descent); } +#if AC_PAYLOAD_PLACE_ENABLED + // payload place flight behaviour: + static PayloadPlace payload_place; +#endif + // run normal or precision landing (if enabled) // pause_descent is true if vehicle should not descend void land_run_normal_or_precland(bool pause_descent = false); @@ -432,10 +471,11 @@ class ModeAltHold : public Mode { }; - class ModeAuto : public Mode { public: + friend class PayloadPlace; // in case wp_run is accidentally required + // inherit constructor using Mode::Mode; Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; } @@ -461,7 +501,9 @@ class ModeAuto : public Mode { NAVGUIDED, LOITER, LOITER_TO_ALT, +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED NAV_PAYLOAD_PLACE, +#endif NAV_SCRIPT_TIME, NAV_ATTITUDE_TIME, }; @@ -556,12 +598,6 @@ class ModeAuto : public Mode { Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const; - void payload_place_run(); - bool payload_place_run_should_run(); - void payload_place_run_hover(); - void payload_place_run_descent(); - void payload_place_run_release(); - SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run bool shift_alt_to_current_alt(Location& target_loc) const; @@ -649,16 +685,6 @@ class ModeAuto : public Mode { }; State state = State::FlyToLocation; - struct { - PayloadPlaceStateType state = PayloadPlaceStateType_Descent_Start; // records state of payload place - uint32_t descent_established_time_ms; // milliseconds - uint32_t place_start_time_ms; // milliseconds - float descent_thrust_level; - float descent_start_altitude_cm; - float descent_speed_cms; - float descent_max_cm; - } nav_payload_place; - bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f60a512e81355..b79b008c2e6b6 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -149,9 +149,11 @@ void ModeAuto::run() loiter_to_alt_run(); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case SubMode::NAV_PAYLOAD_PLACE: - payload_place_run(); + payload_place.run(); break; +#endif case SubMode::NAV_ATTITUDE_TIME: nav_attitude_time_run(); @@ -539,9 +541,13 @@ bool ModeAuto::is_taking_off() const return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete); } +#if AC_PAYLOAD_PLACE_ENABLED // auto_payload_place_start - initialises controller to implement a placing -void ModeAuto::payload_place_start() +void PayloadPlace::start_descent() { + auto *pos_control = copter.pos_control; + auto *wp_nav = copter.wp_nav; + // set horizontal speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -561,13 +567,11 @@ void ModeAuto::payload_place_start() } // initialise yaw - auto_yaw.set_mode(AutoYaw::Mode::HOLD); + copter.flightmode->auto_yaw.set_mode(Mode::AutoYaw::Mode::HOLD); - // set submode - set_submode(SubMode::NAV_PAYLOAD_PLACE); - - nav_payload_place.state = PayloadPlaceStateType_Descent_Start; + state = PayloadPlace::State::Descent_Start; } +#endif // returns true if pilot's yaw input should be used to adjust vehicle's heading bool ModeAuto::use_pilot_yaw(void) const @@ -657,9 +661,11 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_nav_delay(cmd); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint do_payload_place(cmd); break; +#endif #if AP_SCRIPTING_ENABLED case MAV_CMD_NAV_SCRIPT_TIME: @@ -865,9 +871,11 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) cmd_complete = verify_land(); break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: - cmd_complete = verify_payload_place(); + cmd_complete = payload_place.verify(); break; +#endif case MAV_CMD_NAV_LOITER_UNLIM: cmd_complete = verify_loiter_unlimited(); @@ -1158,122 +1166,130 @@ void ModeAuto::nav_attitude_time_run() pos_control->update_z_controller(); } +#if AC_PAYLOAD_PLACE_ENABLED // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more -void ModeAuto::payload_place_run() +void PayloadPlace::run() { const char* prefix_str = "PayloadPlace:"; - if (!payload_place_run_should_run()) { - zero_throttle_and_relax_ac(); + if (copter.flightmode->is_disarmed_or_landed()) { + copter.flightmode->make_safe_ground_handling(); return; } // set motors to full range - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed // Vertical thrust is taken from the attitude controller before angle boost is added + auto *attitude_control = copter.attitude_control; const float thrust_level = attitude_control->get_throttle_in(); const uint32_t now_ms = AP_HAL::millis(); // if we discover we've landed then immediately release the load: if (copter.ap.land_complete || copter.ap.land_complete_maybe) { - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + switch (state) { + case State::FlyToLocation: // this is handled in wp_run() break; - case PayloadPlaceStateType_Descent_Start: + case State::Descent_Start: // do nothing on this loop break; - case PayloadPlaceStateType_Descent: + case State::Descent: gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Release; + state = State::Release; break; - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: + case State::Done: break; } } #if AP_GRIPPER_ENABLED == ENABLED // if pilot releases load manually: - if (g2.gripper.valid() && g2.gripper.released()) { - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - case PayloadPlaceStateType_Descent_Start: - set_submode(SubMode::NAV_PAYLOAD_PLACE); + if (AP::gripper() != nullptr && + AP::gripper()->valid() && AP::gripper()->released()) { + switch (state) { + case State::FlyToLocation: + case State::Descent_Start: gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Done; + state = State::Done; break; - case PayloadPlaceStateType_Descent: + case State::Descent: gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); - nav_payload_place.state = PayloadPlaceStateType_Release; + state = State::Release; break; - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: + case State::Done: break; } } #endif - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + auto &inertial_nav = copter.inertial_nav; + auto &g2 = copter.g2; + const auto &g = copter.g; + const auto &wp_nav = copter.wp_nav; + const auto &pos_control = copter.pos_control; + + switch (state) { + case State::FlyToLocation: if (copter.wp_nav->reached_wp_destination()) { - payload_place_start(); + start_descent(); } break; - case PayloadPlaceStateType_Descent_Start: - nav_payload_place.descent_established_time_ms = now_ms; - nav_payload_place.descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); + case State::Descent_Start: + descent_established_time_ms = now_ms; + descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety - nav_payload_place.descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down()); - nav_payload_place.descent_thrust_level = 1.0; - nav_payload_place.state = PayloadPlaceStateType_Descent; + descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down()); + descent_thrust_level = 1.0; + state = State::Descent; FALLTHROUGH; - case PayloadPlaceStateType_Descent: + case State::Descent: // check maximum decent distance - if (!is_zero(nav_payload_place.descent_max_cm) && - nav_payload_place.descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > nav_payload_place.descent_max_cm) { - nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + if (!is_zero(descent_max_cm) && + descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > descent_max_cm) { + state = State::Ascent_Start; gcs().send_text(MAV_SEVERITY_WARNING, "%s Reached maximum descent", prefix_str); break; } // calibrate the decent thrust after aircraft has reached constant decent rate and release if threshold is reached - if (pos_control->get_vel_desired_cms().z > -0.95 * nav_payload_place.descent_speed_cms) { + if (pos_control->get_vel_desired_cms().z > -0.95 * descent_speed_cms) { // decent rate has not reached descent_speed_cms - nav_payload_place.descent_established_time_ms = now_ms; + descent_established_time_ms = now_ms; break; - } else if (now_ms - nav_payload_place.descent_established_time_ms < descent_thrust_cal_duration_ms) { + } else if (now_ms - descent_established_time_ms < descent_thrust_cal_duration_ms) { // record minimum thrust for descent_thrust_cal_duration_ms - nav_payload_place.descent_thrust_level = MIN(nav_payload_place.descent_thrust_level, thrust_level); - nav_payload_place.place_start_time_ms = now_ms; + descent_thrust_level = MIN(descent_thrust_level, thrust_level); + place_start_time_ms = now_ms; break; - } else if (thrust_level > g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level) { + } else if (thrust_level > g2.pldp_thrust_placed_fraction * descent_thrust_level) { // thrust is above minimum threshold - nav_payload_place.place_start_time_ms = now_ms; + place_start_time_ms = now_ms; break; } else if (is_positive(g2.pldp_range_finder_minimum_m)) { if (!copter.rangefinder_state.enabled) { // abort payload place because rangefinder is not enabled - nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + state = State::Ascent_Start; gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MIN set and rangefinder not enabled", prefix_str); break; } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_minimum_m * 100.0)) { // range finder altitude is above minimum - nav_payload_place.place_start_time_ms = now_ms; + place_start_time_ms = now_ms; break; } } @@ -1287,57 +1303,61 @@ void ModeAuto::payload_place_run() // payload touchdown must be detected for 0.5 seconds - if (now_ms - nav_payload_place.place_start_time_ms > placed_check_duration_ms) { - nav_payload_place.state = PayloadPlaceStateType_Release; - gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast(g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level)); + if (now_ms - place_start_time_ms > placed_check_duration_ms) { + state = State::Release; + gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast(g2.pldp_thrust_placed_fraction * descent_thrust_level)); } break; - case PayloadPlaceStateType_Release: + case State::Release: // Reinitialise vertical position controller to remove discontinuity due to touch down of payload pos_control->init_z_controller_no_descent(); #if AP_GRIPPER_ENABLED == ENABLED if (g2.gripper.valid()) { gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str); g2.gripper.release(); - nav_payload_place.state = PayloadPlaceStateType_Releasing; + state = State::Releasing; } else { - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; } #else - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; #endif break; - case PayloadPlaceStateType_Releasing: + case State::Releasing: #if AP_GRIPPER_ENABLED == ENABLED if (g2.gripper.valid() && !g2.gripper.released()) { break; } #endif - nav_payload_place.state = PayloadPlaceStateType_Delay; + state = State::Delay; FALLTHROUGH; - case PayloadPlaceStateType_Delay: + case State::Delay: // If we get here we have finished releasing the gripper - if (now_ms - nav_payload_place.place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) { + if (now_ms - place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) { break; } FALLTHROUGH; - case PayloadPlaceStateType_Ascent_Start: { - auto_takeoff.start(nav_payload_place.descent_start_altitude_cm, false); - nav_payload_place.state = PayloadPlaceStateType_Ascent; - } - break; + case State::Ascent_Start: + state = State::Ascent; + FALLTHROUGH; - case PayloadPlaceStateType_Ascent: - if (auto_takeoff.complete) { - nav_payload_place.state = PayloadPlaceStateType_Done; + case State::Ascent: { + // Ascent complete when we are less than 10% of the stopping + // distance from the target altitude stopping distance from + // vel_threshold_fraction * max velocity + const float vel_threshold_fraction = 0.1; + const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); + bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= descent_start_altitude_cm - stop_distance; + if (reached_altitude) { + state = State::Done; } break; - - case PayloadPlaceStateType_Done: + } + case State::Done: break; default: // this should never happen @@ -1345,61 +1365,35 @@ void ModeAuto::payload_place_run() break; } - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: + switch (state) { + case State::FlyToLocation: // this should never happen - return wp_run(); - case PayloadPlaceStateType_Descent_Start: - case PayloadPlaceStateType_Descent: - return payload_place_run_descent(); - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - return payload_place_run_hover(); - case PayloadPlaceStateType_Ascent: - case PayloadPlaceStateType_Done: - return takeoff_run(); - } -} - -bool ModeAuto::payload_place_run_should_run() -{ - // must be armed - if (!motors->armed()) { - return false; - } - // must be auto-armed - if (!copter.ap.auto_armed) { - return false; - } - // must not be landed - if (copter.ap.land_complete && (nav_payload_place.state == PayloadPlaceStateType_FlyToLocation || nav_payload_place.state == PayloadPlaceStateType_Descent_Start)) { - return false; - } - // interlock must be enabled (i.e. unsafe) - if (!motors->get_interlock()) { - return false; + return copter.mode_auto.wp_run(); + case State::Descent_Start: + case State::Descent: + copter.flightmode->land_run_horizontal_control(); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(-descent_speed_cms, true); + pos_control->update_z_controller(); + return; + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + copter.flightmode->land_run_horizontal_control(); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(0.0, false); + pos_control->update_z_controller(); + return; + case State::Ascent: + case State::Done: + float vel = 0.0; + copter.flightmode->land_run_horizontal_control(); + pos_control->input_pos_vel_accel_z(descent_start_altitude_cm, vel, 0.0); + break; } - - return true; -} - -void ModeAuto::payload_place_run_hover() -{ - land_run_horizontal_control(); - // update altitude target and call position controller - pos_control->land_at_climb_rate_cm(0.0, false); - pos_control->update_z_controller(); -} - -void ModeAuto::payload_place_run_descent() -{ - land_run_horizontal_control(); - // update altitude target and call position controller - pos_control->land_at_climb_rate_cm(-nav_payload_place.descent_speed_cms, true); - pos_control->update_z_controller(); } +#endif // sets the target_loc's alt to the vehicle's current alt but does not change target_loc's frame // in the case of terrain altitudes either the terrain database or the rangefinder may be used @@ -1522,11 +1516,13 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TIME: +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: { const Location dest_loc = loc_from_cmd(current_cmd, default_loc); const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); return wp_nav->set_wp_destination_next_loc(next_dest_loc); } +#endif case MAV_CMD_NAV_SPLINE_WAYPOINT: { // get spline's location and next location from command and send to wp_nav Location next_dest_loc, next_next_dest_loc; @@ -1927,13 +1923,14 @@ void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) } #endif +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED // do_payload_place - initiate placing procedure void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) { // if location provided we fly to that location at current altitude if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { // set state to fly to location - nav_payload_place.state = PayloadPlaceStateType_FlyToLocation; + payload_place.state = PayloadPlace::State::FlyToLocation; // convert cmd to location class Location target_loc(cmd.content.location); @@ -1949,14 +1946,16 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) copter.failsafe_terrain_on_event(); return; } - // set submode - set_submode(SubMode::NAV_PAYLOAD_PLACE); } else { // initialise placing controller - payload_place_start(); + payload_place.start_descent(); } - nav_payload_place.descent_max_cm = cmd.p1; + payload_place.descent_max_cm = cmd.p1; + + // set submode + set_submode(SubMode::NAV_PAYLOAD_PLACE); } +#endif // AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED // do_RTL - start Return-to-Launch void ModeAuto::do_RTL(void) @@ -2026,25 +2025,27 @@ bool ModeAuto::verify_land() return retval; } +#if AC_PAYLOAD_PLACE_ENABLED // verify_payload_place - returns true if placing has been completed -bool ModeAuto::verify_payload_place() -{ - switch (nav_payload_place.state) { - case PayloadPlaceStateType_FlyToLocation: - case PayloadPlaceStateType_Descent_Start: - case PayloadPlaceStateType_Descent: - case PayloadPlaceStateType_Release: - case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Delay: - case PayloadPlaceStateType_Ascent_Start: - case PayloadPlaceStateType_Ascent: +bool PayloadPlace::verify() +{ + switch (state) { + case State::FlyToLocation: + case State::Descent_Start: + case State::Descent: + case State::Release: + case State::Releasing: + case State::Delay: + case State::Ascent_Start: + case State::Ascent: return false; - case PayloadPlaceStateType_Done: + case State::Done: return true; } // should never get here return true; } +#endif bool ModeAuto::verify_loiter_unlimited() { diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 39a9461ea63e7..3c59d8a31522b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -49,7 +49,7 @@ float Plane::calc_speed_scaler(void) // no speed estimate and not armed, use a unit scaling speed_scaler = 1; } - if (!plane.ahrs.airspeed_sensor_enabled() && + if (!plane.ahrs.using_airspeed_sensor() && (plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is suppressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates return MIN(speed_scaler, 1.0f) ; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2cc891dafe074..71df773b681fd 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -623,8 +623,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, #if AP_FENCE_ENABLED MSG_FENCE_STATUS, @@ -690,7 +692,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -804,7 +808,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_DENIED; } - if (is_zero(packet.param4)) { + if (isnan(packet.param4) || is_zero(packet.param4)) { requested_position.loiter_ccw = 0; } else { requested_position.loiter_ccw = 1; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b8d6e21db733c..c4c84cf7511d3 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1096,6 +1096,7 @@ class Plane : public AP_Vehicle { void set_servos_idle(void); void set_servos(); void set_servos_controlled(void); + void set_takeoff_expected(void); void set_servos_old_elevons(void); void set_servos_flaps(void); void set_landing_gear(void); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index fbb1b3c9fe27f..839d76b92dc5d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1475,7 +1475,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) // assistance due to Q_ASSIST_SPEED // if option bit is enabled only allow assist with real airspeed sensor if ((have_airspeed && aspeed < assist_speed) && - (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.airspeed_sensor_enabled())) { + (!option_is_set(QuadPlane::OPTION::DISABLE_SYNTHETIC_AIRSPEED_ASSIST) || ahrs.using_airspeed_sensor())) { in_angle_assist = false; angle_error_start_ms = 0; return true; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index cb7e68ff226f3..68ee2cb9986f4 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -196,7 +196,7 @@ void Plane::read_radio() && channel_throttle->get_control_in() > 50 && stickmixing) { float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index ced42525788e8..4237fa9bbfca9 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -134,7 +134,7 @@ bool Plane::suppress_throttle(void) // require 5m/s. This prevents throttle up due to spiky GPS // groundspeed with bad GPS reception #if AP_AIRSPEED_ENABLED - if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { + if ((!ahrs.using_airspeed_sensor()) || airspeed.get_airspeed() >= 5) { // we're moving at more than 5 m/s throttle_suppressed = false; return false; @@ -485,28 +485,22 @@ void Plane::set_servos_controlled(void) min_throttle = 0; } - bool flight_stage_determines_max_throttle = false; - if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || - flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING - ) { - flight_stage_determines_max_throttle = true; - } + const bool use_takeoff_throttle_max = #if HAL_QUADPLANE_ENABLED - if (quadplane.in_transition()) { - flight_stage_determines_max_throttle = true; - } + quadplane.in_transition() || #endif - if (flight_stage_determines_max_throttle) { + (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || + (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); + + if (use_takeoff_throttle_max) { if (aparm.takeoff_throttle_max != 0) { - max_throttle = aparm.takeoff_throttle_max; - } else { - max_throttle = aparm.throttle_max; + max_throttle = aparm.takeoff_throttle_max.get(); } } else if (landing.is_flaring()) { min_throttle = 0; } - // conpensate for battery voltage drop + // compensate for battery voltage drop throttle_voltage_comp(min_throttle, max_throttle); // apply watt limiter @@ -516,14 +510,16 @@ void Plane::set_servos_controlled(void) constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); if (!arming.is_armed_and_safety_off()) { + // Always set 0 scaled even if overriding to zero pwm. + // This ensures slew limits and other functions using the scaled value pick up in the correct place + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0); + if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); - } else { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0); } } else if (suppress_throttle()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default @@ -573,6 +569,13 @@ void Plane::set_servos_controlled(void) #endif // HAL_QUADPLANE_ENABLED } +} + +/* + Warn AHRS that we might take off soon + */ +void Plane::set_takeoff_expected(void) +{ // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); if (!is_flying() && arming.is_armed()) { @@ -605,7 +608,7 @@ void Plane::set_servos_flaps(void) if (control_mode->does_auto_throttle()) { int16_t flapSpeedSource = 0; - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { flapSpeedSource = target_airspeed_cm * 0.01f; } else { flapSpeedSource = aparm.throttle_cruise; @@ -821,6 +824,7 @@ void Plane::set_servos(void) if (control_mode != &mode_manual) { set_servos_controlled(); + set_takeoff_expected(); } // setup flap outputs diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 5574ad2c9b0c1..ce2264f8c800c 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -185,7 +185,7 @@ void Plane::takeoff_calc_pitch(void) return; } - if (ahrs.airspeed_sensor_enabled()) { + if (ahrs.using_airspeed_sensor()) { int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd(); calc_nav_pitch(); if (nav_pitch_cd < takeoff_pitch_min_cd) { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 15d4c4621a663..9e8d7322852a3 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -351,8 +351,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, #if AP_FENCE_ENABLED MSG_FENCE_STATUS, diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 87ad912976b5c..d947c8ed0e938 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -394,9 +394,7 @@ void ModeAuto::set_auto_yaw_roi(const Location &roi_location) set_auto_yaw_mode(get_default_auto_yaw_mode(false)); #if HAL_MOUNT_ENABLED // switch off the camera tracking if enabled - if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - sub.camera_mount.set_mode_to_default(); - } + sub.camera_mount.clear_roi_target(); #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED diff --git a/BUILD.md b/BUILD.md index 9124d7acb7d3d..ae32c0a9107e7 100644 --- a/BUILD.md +++ b/BUILD.md @@ -88,6 +88,7 @@ list some basic and more used commands as example. ./waf rover # Ground-based rovers and surface boats ./waf sub # ROV and other submarines ./waf antennatracker # Antenna trackers + ./waf AP_Periph # AP Peripheral ``` diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 21096c5bfd015..c96624649184a 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -320,8 +320,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, // MISSION_CURRENT MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, #if AP_FENCE_ENABLED MSG_FENCE_STATUS, @@ -374,7 +376,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index a7f01fd54b336..7e7cc4f320f17 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -149,6 +149,7 @@ int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const return rover.g2.motors.get_throttle(); } +#if AP_RANGEFINDER_ENABLED void GCS_MAVLINK_Rover::send_rangefinder() const { float distance = 0; @@ -178,6 +179,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const distance, voltage); } +#endif // AP_RANGEFINDER_ENABLED /* send PID tuning message @@ -528,8 +530,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_CURRENT_WAYPOINT, MSG_GPS_RAW, MSG_GPS_RTK, +#if GPS_MAX_RECEIVERS > 1 MSG_GPS2_RAW, MSG_GPS2_RTK, +#endif MSG_NAV_CONTROLLER_OUTPUT, #if AP_FENCE_ENABLED MSG_FENCE_STATUS, @@ -588,7 +592,9 @@ static const ap_message STREAM_PARAMS_msgs[] = { }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, +#if AP_AIS_ENABLED MSG_AIS_VESSEL, +#endif }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index f628bfa530bc4..050109f5ccc3b 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -58,7 +58,9 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK int16_t vfr_hud_throttle() const override; +#if AP_RANGEFINDER_ENABLED void send_rangefinder() const override; +#endif #if HAL_HIGH_LATENCY2_ENABLED uint8_t high_latency_tgt_heading() const override; diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 569f34c684ab6..013f1b21c83b9 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -267,10 +267,14 @@ AP_HW_VIMDRONES_MOSAIC_H 1406 AP_HW_VIMDRONES_PERIPH 1407 # IDs 5000-5099 reserved for Carbonix +# IDs 5100-5199 reserved for SYPAQ Systems # IDs 5200-5209 reserved for Airvolute -AP_HW_AIRVOLUTE_DCS2 5200 +AP_HW_AIRVOLUTE_DCS2 5200 + +# IDs 5210-5219 reserved for Aocoda-RC +AP_HW_AOCODA-RC-H743DUAL 5210 +AP_HW_AOCODA-RC-F405V3 5211 -# IDs 5100-5199 reserved for SYPAQ Systems # IDs 6000-6099 reserved for SpektreWorks # OpenDroneID enabled boards. Use 10000 + the base board ID diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index a4cdf01a460d2..b06df26492126 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -14,15 +14,15 @@ from pymavlink import mavextra from pymavlink import mavutil -from common import AutoTest -from common import NotAchievedException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7) -class AutoTestTracker(AutoTest): +class AutoTestTracker(vehicle_test_suite.TestSuite): def log_name(self): return "AntennaTracker" diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 7710cff11eec9..a582a33008956 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -20,10 +20,11 @@ from pysim import util from pysim import vehicleinfo -from common import AutoTest -from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException -from common import Test -from common import MAV_POS_TARGET_TYPE_MASK +import vehicle_test_suite + +from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException +from vehicle_test_suite import Test +from vehicle_test_suite import MAV_POS_TARGET_TYPE_MASK from pymavlink.rotmat import Vector3 @@ -40,7 +41,7 @@ # switch 6 = Stabilize -class AutoTestCopter(AutoTest): +class AutoTestCopter(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return ["AUTO", "AUTOTUNE", "BRAKE", "CIRCLE", "FLIP", "LAND", "RTL", "SMART_RTL", "AVOID_ADSB", "FOLLOW"] @@ -9874,7 +9875,7 @@ def Sprayer(self): def tests1a(self): '''return list of all tests''' - ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/common.py + ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/vehicle_test_suite.py ret.extend([ self.NavDelayTakeoffAbsTime, self.NavDelayAbsTime, @@ -10005,6 +10006,7 @@ def tests1e(self): self.MAV_CMD_NAV_LOITER_UNLIM, self.MAV_CMD_NAV_RETURN_TO_LAUNCH, self.MAV_CMD_NAV_VTOL_LAND, + self.clear_roi, ]) return ret @@ -10310,6 +10312,45 @@ def MAV_CMD_NAV_VTOL_LAND(self): command(mavutil.mavlink.MAV_CMD_NAV_LAND) self.wait_mode('LAND') + def clear_roi(self): + '''ensure three commands that clear ROI are equivalent''' + + self.upload_simple_relhome_mission([ + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 20), # directly North, i.e. 0 degrees + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, 20), # directly North, i.e. 0 degrees + ]) + + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + home_loc = self.mav.location() + + cmd_ids = [ + mavutil.mavlink.MAV_CMD_DO_SET_ROI, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE, + ] + for command in self.run_cmd, self.run_cmd_int: + for cmd_id in cmd_ids: + self.wait_waypoint(2, 2) + + # Set an ROI at the Home location, expect to point at Home + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, p5=home_loc.lat, p6=home_loc.lng, p7=home_loc.alt) + self.wait_heading(180) + + # Clear the ROI, expect to point at the next Waypoint + self.progress("Clear ROI using %s(%d)" % (command.__name__, cmd_id)) + command(cmd_id) + self.wait_heading(0) + + self.wait_waypoint(4, 4) + self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=2) + + self.land_and_disarm() + def start_flying_simple_rehome_mission(self, items): '''uploads items, changes mode to auto, waits ready to arm and arms vehicle. If the first item it a takeoff you can expect the diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index abd652a309511..d3382aece5c78 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -15,15 +15,17 @@ from pymavlink import mavextra from pymavlink import mavutil -from common import AutoTest -from common import AutoTestTimeoutException -from common import NotAchievedException -from common import PreconditionFailedException -from common import WaitModeTimeout -from common import OldpymavlinkException -from common import Test - from pymavlink.rotmat import Vector3 + +import vehicle_test_suite + +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import OldpymavlinkException +from vehicle_test_suite import PreconditionFailedException +from vehicle_test_suite import Test +from vehicle_test_suite import WaitModeTimeout + from pysim import vehicleinfo from pysim import util @@ -35,7 +37,7 @@ WIND = "0,180,0.2" # speed,direction,variance -class AutoTestPlane(AutoTest): +class AutoTestPlane(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return [] diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 230bfbd1838f3..fcc622ddfba76 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -11,9 +11,9 @@ from pymavlink import mavutil -from common import AutoTest -from common import NotAchievedException -from common import AutoTestTimeoutException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import AutoTestTimeoutException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -33,7 +33,7 @@ class Joystick(): Lateral = 6 -class AutoTestSub(AutoTest): +class AutoTestSub(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return [] @@ -422,7 +422,6 @@ def disabled_tests(self): ret = super(AutoTestSub, self).disabled_tests() ret.update({ "ConfigErrorLoop": "Sub does not instantiate AP_Stats. Also see https://github.com/ArduPilot/ardupilot/issues/10247", # noqa - "MAV_CMD_DO_CHANGE_SPEED": "Doesn't work", }) return ret @@ -458,21 +457,18 @@ def MAV_CMD_MISSION_START(self): def MAV_CMD_DO_CHANGE_SPEED(self): '''ensure vehicle changes speeds when DO_CHANGE_SPEED received''' items = [ - (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 0), + (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, -1), (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), ] - items = [] - for i in range(0, 2000, 10): - items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, i, 0, 0)) self.upload_simple_relhome_mission(items) - self.wait_ready_to_arm() self.arm_vehicle() self.run_cmd(mavutil.mavlink.MAV_CMD_MISSION_START) + self.progress("SENT MISSION START") for run_cmd in self.run_cmd, self.run_cmd_int: - for speed in [1, 2, 3, 1]: + for speed in [1, 1.5, 0.5]: run_cmd(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, p2=speed) - self.wait_groundspeed(speed-0.02, speed+0.02, minimum_duration=2) + self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=2, timeout=60) self.disarm_vehicle() def _MAV_CMD_CONDITION_YAW(self, run_cmd): diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 84c5884e070db..8a2d82e2c883d 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -36,7 +36,7 @@ from pysim import util from pymavlink.generator import mavtemplate -from common import Test +from vehicle_test_suite import Test tester = None diff --git a/Tools/autotest/balancebot.py b/Tools/autotest/balancebot.py index 0bc8963e8b262..de99e7542ec3e 100644 --- a/Tools/autotest/balancebot.py +++ b/Tools/autotest/balancebot.py @@ -10,9 +10,9 @@ import os from rover import AutoTestRover -from common import AutoTest -from common import NotAchievedException +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) @@ -105,7 +105,7 @@ def tests(self): '''note that while AutoTestBalanceBot inherits from Rover we don't inherit Rover's tests!''' - ret = AutoTest.tests(self) + ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.DriveRTL, diff --git a/Tools/autotest/blimp.py b/Tools/autotest/blimp.py index affeec2cecd87..05a8d5fd98056 100644 --- a/Tools/autotest/blimp.py +++ b/Tools/autotest/blimp.py @@ -10,7 +10,7 @@ from pymavlink import mavutil -from common import AutoTest +from vehicle_test_suite import TestSuite # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) @@ -25,7 +25,7 @@ # switch 6 = Manual -class AutoTestBlimp(AutoTest): +class AutoTestBlimp(TestSuite): @staticmethod def get_not_armable_mode_list(): return [] diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 0dddcea057b83..0485556eb13ef 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -5,9 +5,11 @@ ''' from __future__ import print_function + from arducopter import AutoTestCopter -from common import AutoTest -from common import NotAchievedException, AutoTestTimeoutException + +import vehicle_test_suite +from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException from pymavlink import mavutil from pysim import vehicleinfo @@ -801,7 +803,7 @@ def TurbineStart(self, timeout=200): def tests(self): '''return list of all tests''' - ret = AutoTest.tests(self) + ret = vehicle_test_suite.TestSuite.tests(self) ret.extend([ self.AVCMission, self.RotorRunup, diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 48444a89183f4..8715aa3e6a996 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -13,9 +13,9 @@ from pymavlink import mavutil from pymavlink.rotmat import Vector3 -from common import AutoTest -from common import Test -from common import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException +import vehicle_test_suite +from vehicle_test_suite import Test +from vehicle_test_suite import AutoTestTimeoutException, NotAchievedException, PreconditionFailedException import operator @@ -26,7 +26,7 @@ SITL_START_LOCATION = mavutil.location(-27.274439, 151.290064, 343, 8.7) -class AutoTestQuadPlane(AutoTest): +class AutoTestQuadPlane(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 891d3773e24de..3f97fd9e2eaca 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -14,12 +14,13 @@ import sys import time -from common import AutoTest +import vehicle_test_suite + from pysim import util -from common import AutoTestTimeoutException -from common import NotAchievedException -from common import PreconditionFailedException +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import NotAchievedException +from vehicle_test_suite import PreconditionFailedException from pymavlink import mavextra from pymavlink import mavutil @@ -33,7 +34,7 @@ 246) -class AutoTestRover(AutoTest): +class AutoTestRover(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): return ["RTL", "SMART_RTL"] diff --git a/Tools/autotest/run_mission.py b/Tools/autotest/run_mission.py index 99a0bc519868f..53721d76ee2c1 100755 --- a/Tools/autotest/run_mission.py +++ b/Tools/autotest/run_mission.py @@ -6,7 +6,7 @@ AP_FLAKE8_CLEAN ''' -import common +import vehicle_test_suite import os import sys import argparse @@ -14,7 +14,7 @@ from pysim import util -class RunMission(common.AutoTest): +class RunMission(vehicle_test_suite.TestSuite): def __init__(self, vehicle_binary, model, mission_filepath, speedup=None, sim_rate_hz=None): super(RunMission, self).__init__(vehicle_binary) self.mission_filepath = mission_filepath diff --git a/Tools/autotest/sailboat.py b/Tools/autotest/sailboat.py index a67fe0a5ad8ea..4c85ae33a4afe 100644 --- a/Tools/autotest/sailboat.py +++ b/Tools/autotest/sailboat.py @@ -11,8 +11,8 @@ from rover import AutoTestRover -from common import AutoTestTimeoutException -from common import PreconditionFailedException +from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import PreconditionFailedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) diff --git a/Tools/autotest/common.py b/Tools/autotest/vehicle_test_suite.py similarity index 99% rename from Tools/autotest/common.py rename to Tools/autotest/vehicle_test_suite.py index e495c82a3b662..1f7145cbe4a54 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -1482,7 +1482,7 @@ def __str__(self): return ret -class AutoTest(ABC): +class TestSuite(ABC): """Base abstract class. It implements the common function for all vehicle types. """ @@ -5937,14 +5937,14 @@ def longitude_scale(lat): @staticmethod def get_distance(loc1, loc2): """Get ground distance between two locations.""" - return AutoTest.get_distance_accurate(loc1, loc2) + return TestSuite.get_distance_accurate(loc1, loc2) # dlat = loc2.lat - loc1.lat # try: # dlong = loc2.lng - loc1.lng # except AttributeError: # dlong = loc2.lon - loc1.lon - # return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5 + # return math.sqrt((dlat*dlat) + (dlong*dlong)*TestSuite.longitude_scale(loc2.lat)) * 1.113195e5 @staticmethod def get_distance_accurate(loc1, loc2): @@ -5981,23 +5981,23 @@ def get_latlon_attr(loc, attrs): @staticmethod def get_lat_attr(loc): '''return any found latitude attribute from loc''' - return AutoTest.get_latlon_attr(loc, ["lat", "latitude"]) + return TestSuite.get_latlon_attr(loc, ["lat", "latitude"]) @staticmethod def get_lon_attr(loc): '''return any found latitude attribute from loc''' - return AutoTest.get_latlon_attr(loc, ["lng", "lon", "longitude"]) + return TestSuite.get_latlon_attr(loc, ["lng", "lon", "longitude"]) @staticmethod def get_distance_int(loc1, loc2): """Get ground distance between two locations in the normal "int" form - lat/lon multiplied by 1e7""" - loc1_lat = AutoTest.get_lat_attr(loc1) - loc2_lat = AutoTest.get_lat_attr(loc2) - loc1_lon = AutoTest.get_lon_attr(loc1) - loc2_lon = AutoTest.get_lon_attr(loc2) + loc1_lat = TestSuite.get_lat_attr(loc1) + loc2_lat = TestSuite.get_lat_attr(loc2) + loc1_lon = TestSuite.get_lon_attr(loc1) + loc2_lon = TestSuite.get_lon_attr(loc2) - return AutoTest.get_distance_accurate( + return TestSuite.get_distance_accurate( mavutil.location(loc1_lat*1e-7, loc1_lon*1e-7), mavutil.location(loc2_lat*1e-7, loc2_lon*1e-7)) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index f1d7531eb7883..4c91b11a5e503 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -136,6 +136,9 @@ def __init__(self, Feature('Copter', 'MODE_FLIP', 'MODE_FLIP_ENABLED', 'Enable Mode Flip', 0, None), Feature('Copter', 'MODE_BRAKE', 'MODE_BRAKE_ENABLED', 'Enable Mode Brake', 0, None), + Feature('Mission', 'AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED', 'AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED', 'Enable handling of NAV_PAYLOAD_PLACE mission items', 0, None), # noqa + Feature('Copter', 'AC_PAYLOAD_PLACE_ENABLED', 'AC_PAYLOAD_PLACE_ENABLED', 'Enable Payload Place flight behaviour', 0, 'AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED'), # noqa + Feature('Compass', 'AK09916', 'AP_COMPASS_AK09916_ENABLED', 'Enable AK09916 compasses', 1, None), Feature('Compass', 'AK8963', 'AP_COMPASS_AK8963_ENABLED', 'Enable AK8963 compasses', 1, None), Feature('Compass', 'BMM150', 'AP_COMPASS_BMM150_ENABLED', 'Enable BMM150 compasses', 1, None), @@ -214,7 +217,7 @@ def __init__(self, Feature('Rangefinder', 'RANGEFINDER_LEDDARONE', 'AP_RANGEFINDER_LEDDARONE_ENABLED', "Enable Rangefinder - LeddarOne", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_LEDDARVU8', 'AP_RANGEFINDER_LEDDARVU8_ENABLED', "Enable Rangefinder - LeddarVU8", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_LIGHTWARE_SERIAL', 'AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED', "Enable Rangefinder - Lightware (serial)", 0, "RANGEFINDER"), # NOQA: E501 - Feature('Rangefinder', 'RANGEFINDER_LUA', 'AP_RANGEFINDER_LUA_ENABLED', "Enable Rangefinder - Lua Scripting", 0, "RANGEFINDER"), # NOQA: E501 + Feature('Rangefinder', 'RANGEFINDER_LUA', 'AP_RANGEFINDER_LUA_ENABLED', "Enable Rangefinder - Lua Scripting", 0, "RANGEFINDER,SCRIPTING"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_LWI2C', 'AP_RANGEFINDER_LWI2C_ENABLED', "Enable Rangefinder - Lightware (i2c)", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_MAVLINK', 'AP_RANGEFINDER_MAVLINK_ENABLED', "Enable Rangefinder - MAVLink", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_MAXBOTIX_SERIAL', 'AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED', "Enable Rangefinder - MaxBotix (serial)", 0, "RANGEFINDER"), # NOQA: E501 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 094c5ff2ceee4..69434688c7f06 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -24,9 +24,14 @@ class ExtractFeatures(object): - def __init__(self, filename, nm="arm-none-eabi-nm"): + class FindString(object): + def __init__(self, string): + self.string = string + + def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): self.filename = filename self.nm = nm + self.strings = strings # feature_name should match the equivalent feature in # build_options.py ('FEATURE_NAME', 'EXPECTED_SYMBOL'). @@ -130,7 +135,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('HAL_RALLY_ENABLED', r'AP_Rally::get_rally_max\b',), ('AC_AVOID_ENABLED', 'AC_Avoid::AC_Avoid',), ('AC_OAPATHPLANNER_ENABLED', 'AP_OAPathPlanner::AP_OAPathPlanner',), - + ('AC_PAYLOAD_PLACE_ENABLED', 'PayloadPlace::start_descent'), + ('AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED', ExtractFeatures.FindString('PayloadPlace')), ('AP_ICENGINE_ENABLED', 'AP_ICEngine::AP_ICEngine',), ('HAL_EFI_ENABLED', 'AP_RPM_EFI::AP_RPM_EFI',), ('AP_EFI_NWPWU_ENABLED', r'AP_EFI_NWPMU::update\b',), @@ -365,36 +371,53 @@ def extract_symbols_from_elf(self, filename): return ret + def extract_strings_from_elf(self, filename): + """Runs strings on filename, returns as a list""" + text_output = self.run_program('EF', [ + self.strings, + filename + ], show_output=False) + return text_output.split("\n") + def extract(self): '''returns two sets - compiled_in and not_compiled_in''' build_options_defines = set([x.define for x in build_options.BUILD_OPTIONS]) symbols = self.extract_symbols_from_elf(self.filename) + strings = self.extract_strings_from_elf(self.filename) remaining_build_options_defines = build_options_defines compiled_in_feature_defines = [] for (feature_define, symbol) in self.features: - some_dict = symbols.dict_for_symbol(symbol) - # look for symbols without arguments - # print("Looking for (%s)" % str(name)) - for s in some_dict.keys(): - m = re.match(symbol, s) - # print("matching %s with %s" % (symbol, s)) - if m is None: - continue - d = m.groupdict() - for key in d.keys(): - d[key] = d[key].upper() - # filter to just the defines present in - # build_options.py - otherwise we end up with (e.g.) - # AP_AIRSPEED_BACKEND_ENABLED, even 'though that - # doesn't exist in the ArduPilot codebase. - some_define = feature_define.format(**d) - if some_define not in build_options_defines: - continue - compiled_in_feature_defines.append(some_define) - remaining_build_options_defines.discard(some_define) + if isinstance(symbol, ExtractFeatures.FindString): + if symbol.string in strings: + some_define = feature_define + if some_define not in build_options_defines: + continue + compiled_in_feature_defines.append(some_define) + remaining_build_options_defines.discard(some_define) + else: + some_dict = symbols.dict_for_symbol(symbol) + # look for symbols without arguments + # print("Looking for (%s)" % str(name)) + for s in some_dict.keys(): + m = re.match(symbol, s) + # print("matching %s with %s" % (symbol, s)) + if m is None: + continue + d = m.groupdict() + for key in d.keys(): + d[key] = d[key].upper() + # filter to just the defines present in + # build_options.py - otherwise we end up with (e.g.) + # AP_AIRSPEED_BACKEND_ENABLED, even 'though that + # doesn't exist in the ArduPilot codebase. + some_define = feature_define.format(**d) + if some_define not in build_options_defines: + continue + compiled_in_feature_defines.append(some_define) + remaining_build_options_defines.discard(some_define) return (compiled_in_feature_defines, remaining_build_options_defines) def create_string(self): diff --git a/Tools/scripts/filter_size_compare_branches_csv.py b/Tools/scripts/filter_size_compare_branches_csv.py index b8d60b5905a05..8030608c5e747 100755 --- a/Tools/scripts/filter_size_compare_branches_csv.py +++ b/Tools/scripts/filter_size_compare_branches_csv.py @@ -13,6 +13,35 @@ def __init__(self, filename, show_binary_identical=True, ): def pretty_print_data(self, data): print(tabulate.tabulate(data)) + def prune_empty_columns(self, data): + if len(data) == 0: + return data + + # populate an array indicating whether here are any non-empty + # values in each column: + line_length = len(data[0]) + column_has_content = [False] * line_length + for line in data[1:]: # ASSUMPTION we have a header row! + for i in range(line_length): + if line[i] != "": + column_has_content[i] = True + + # if all columns are full then we don't need to any more: + all_empty = False + if not any(column_has_content): + return data + + new_data = [] + for line in data: + new_line = [] + for i in range(line_length): + if not column_has_content[i]: + continue + new_line.append(line[i]) + new_data.append(new_line) + + return new_data + def run(self): csvt = csv.reader(open(self.filename,'r')) data = list(csvt) @@ -29,6 +58,8 @@ def run(self): new_data.append(line) data = new_data + data = self.prune_empty_columns(data) + self.pretty_print_data(data) if __name__ == '__main__': diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 0cc30e59cb626..14b14b618fb27 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -556,7 +556,7 @@ void AP_AutoTune::update_rmax(void) if (level == 0) { // this level means to keep current values of RMAX and TCONST - target_rmax = constrain_float(current.rmax_pos, 75, 720); + target_rmax = constrain_float(current.rmax_pos, 20, 720); target_tau = constrain_float(current.tau, 0.1, 2); } else { target_rmax = tuning_table[level-1].rmax; diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index cf702b9da423f..4e061a465ec5d 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -806,11 +806,19 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const } /* - return true if a real airspeed sensor is enabled + return true if the current AHRS airspeed estimate is directly derived from an airspeed sensor */ -bool AP_AHRS::airspeed_sensor_enabled(void) const +bool AP_AHRS::using_airspeed_sensor() const { - if (!AP_AHRS_Backend::airspeed_sensor_enabled()) { + return state.airspeed_estimate_type == AirspeedEstimateType::AIRSPEED_SENSOR; +} + +/* + Return true if a airspeed sensor should be used for the AHRS airspeed estimate + */ +bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const +{ + if (!airspeed_sensor_enabled(airspeed_index)) { return false; } nav_filter_status filter_status; @@ -831,9 +839,11 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const // if we have an estimate bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const { +#if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED) + const uint8_t idx = get_active_airspeed_index(); +#endif #if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED - if (airspeed_sensor_enabled()) { - uint8_t idx = get_active_airspeed_index(); + if (_should_use_airspeed_sensor(idx)) { airspeed_ret = AP::airspeed()->get_airspeed(idx); if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) { @@ -866,7 +876,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED case EKFType::DCM: airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); + return dcm.airspeed_estimate(idx, airspeed_ret); #endif #if AP_AHRS_SIM_ENABLED @@ -879,7 +889,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs case EKFType::TWO: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); + return dcm.airspeed_estimate(idx, airspeed_ret); #else return false; #endif @@ -893,8 +903,12 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: +#if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); + return dcm.airspeed_estimate(idx, airspeed_ret); +#else + return false; +#endif #endif } @@ -919,7 +933,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED // fallback to DCM airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); + return dcm.airspeed_estimate(idx, airspeed_ret); #endif return false; @@ -3079,7 +3093,7 @@ uint8_t AP_AHRS::get_active_airspeed_index() const #if HAL_NAVEKF3_AVAILABLE if (active_EKF_type() == EKFType::THREE) { uint8_t ret = EKF3.getActiveAirspeed(); - if (ret != 255 && airspeed->healthy(ret)) { + if (ret != 255 && airspeed->healthy(ret) && airspeed->use(ret)) { return ret; } } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 8ab6bca0457cb..5ea8ae2ed953d 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -151,6 +151,9 @@ class AP_AHRS { // if we have an estimate bool airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &type) const; + // return true if the current AHRS airspeed estimate (from airspeed_estimate method) is directly derived from an airspeed sensor + bool using_airspeed_sensor() const; + // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate bool airspeed_estimate_true(float &airspeed_ret) const; @@ -163,12 +166,13 @@ class AP_AHRS { // returns false if the data is unavailable bool airspeed_health_data(float &innovation, float &innovationVariance, uint32_t &age_ms) const; - // return true if airspeed comes from an airspeed sensor, as - // opposed to an IMU estimate - bool airspeed_sensor_enabled(void) const; + // return true if a airspeed sensor is enabled + bool airspeed_sensor_enabled(void) const { + // FIXME: make this a method on the active backend + return AP_AHRS_Backend::airspeed_sensor_enabled(); + } - // return true if airspeed comes from a specific airspeed sensor, as - // opposed to an IMU estimate + // return true if a airspeed from a specific airspeed sensor is enabled bool airspeed_sensor_enabled(uint8_t airspeed_index) const { // FIXME: make this a method on the active backend return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); @@ -909,6 +913,9 @@ class AP_AHRS { // get current location estimate bool _get_location(Location &loc) const; + + // return true if a airspeed sensor should be used for the AHRS airspeed estimate + bool _should_use_airspeed_sensor(uint8_t airspeed_index) const; /* update state structure diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index c137aa2c146ef..1aef15a6efda3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -47,6 +47,9 @@ bool AP_BattMonitor_Backend::capacity_remaining_pct(uint8_t &percentage) const if (!has_current() || !_state.healthy) { return false; } + if (isnan(_state.consumed_mah) || _params._pack_capacity <= 0) { + return false; + } const float mah_remaining = _params._pack_capacity - _state.consumed_mah; percentage = constrain_float(100 * mah_remaining / _params._pack_capacity, 0, UINT8_MAX); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index d4b7aed2a9cab..281831f0d1839 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -61,28 +61,39 @@ void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) } } +/* + match a battery ID to driver serial number + when serial number is negative, all batteries are accepted, otherwise it must match +*/ +bool AP_BattMonitor_DroneCAN::match_battery_id(uint8_t instance, uint8_t battery_id) +{ + const auto serial_num = AP::battery().get_serial_number(instance); + return serial_num < 0 || serial_num == (int32_t)battery_id; +} + AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id) { if (ap_dronecan == nullptr) { return nullptr; } - for (uint8_t i = 0; i < AP::battery()._num_instances; i++) { - if (AP::battery().drivers[i] == nullptr || - AP::battery().get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { + const auto &batt = AP::battery(); + for (uint8_t i = 0; i < batt._num_instances; i++) { + if (batt.drivers[i] == nullptr || + batt.get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { continue; } - AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i]; + AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)batt.drivers[i]; if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id && match_battery_id(i, battery_id)) { return driver; } } // find empty uavcan driver - for (uint8_t i = 0; i < AP::battery()._num_instances; i++) { - if (AP::battery().drivers[i] != nullptr && - AP::battery().get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && + for (uint8_t i = 0; i < batt._num_instances; i++) { + if (batt.drivers[i] != nullptr && + batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && match_battery_id(i, battery_id)) { - AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i]; + AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)batt.drivers[i]; if(batmon->_ap_dronecan != nullptr || batmon->_node_id != 0) { continue; } @@ -142,21 +153,22 @@ void AP_BattMonitor_DroneCAN::handle_battery_info_aux(const ardupilot_equipment_ { WITH_SEMAPHORE(_sem_battmon); uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), msg.voltage_cell.len); - float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage; - float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage; _cycle_count = msg.cycle_count; for (uint8_t i = 0; i < cell_count; i++) { _interim_state.cell_voltages.cells[i] = msg.voltage_cell.data[i] * 1000; } _interim_state.is_powering_off = msg.is_powering_off; - _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000; - _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh; - _interim_state.time_remaining = is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600); - _interim_state.has_time_remaining = true; + if (!isnan(msg.nominal_voltage) && msg.nominal_voltage > 0) { + float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage; + float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage; + _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000; + _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh; + _interim_state.time_remaining = is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600); + _interim_state.has_time_remaining = true; + } _has_cell_voltages = true; - _has_time_remaining = true; _has_battery_info_aux = true; } @@ -206,7 +218,33 @@ void AP_BattMonitor_DroneCAN::handle_battery_info_trampoline(AP_DroneCAN *ap_dro void AP_BattMonitor_DroneCAN::handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg) { - AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); + const auto &batt = AP::battery(); + AP_BattMonitor_DroneCAN *driver = nullptr; + + /* + check for a backend with AllowSplitAuxInfo set, allowing InfoAux + from a different CAN node than the base battery information + */ + for (uint8_t i = 0; i < batt._num_instances; i++) { + const auto *drv = batt.drivers[i]; + if (drv != nullptr && + batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && + drv->option_is_set(AP_BattMonitor_Params::Options::AllowSplitAuxInfo) && + batt.get_serial_number(i) == int32_t(msg.battery_id)) { + driver = (AP_BattMonitor_DroneCAN *)batt.drivers[i]; + if (driver->_ap_dronecan == nullptr) { + /* we have not received the main battery information + yet. Discard InfoAux until we do so we can init the + backend with the right node ID + */ + return; + } + break; + } + } + if (driver == nullptr) { + driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); + } if (driver == nullptr) { return; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 6b6151d4778c7..819ade7316481 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -63,10 +63,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg); void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc); - static bool match_battery_id(uint8_t instance, uint8_t battery_id) { - // when serial number is negative, all batteries are accepted. Else, it must match - return (AP::battery().get_serial_number(instance) < 0) || (AP::battery().get_serial_number(instance) == (int32_t)battery_id); - } + static bool match_battery_id(uint8_t instance, uint8_t battery_id); // MPPT related enums and methods enum class MPPT_FaultFlags : uint8_t { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 01ec3bbded590..f1d6522a6df0b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -150,7 +150,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: OPTIONS // @DisplayName: Battery monitor options // @Description: This sets options to change the behaviour of the battery monitor - // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS + // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node // @User: Advanced AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0), #endif // HAL_BUILD_AP_PERIPH diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 9366965328cd7..fff2719f8f3cf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,7 @@ class AP_BattMonitor_Params { MPPT_Power_Off_At_Boot = (1U<<4), // MPPT Disabled at startup (aka boot), if HW supports it MPPT_Power_On_At_Boot = (1U<<5), // MPPT Enabled at startup (aka boot), if HW supports it. If Power_Off_at_Boot is also set, the behavior is Power_Off_at_Boot GCS_Resting_Voltage = (1U<<6), // send resistance resting voltage to GCS + AllowSplitAuxInfo = (1U<<7), // allow different node to provide aux info for DroneCAN }; BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); } diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index d0a89c490df4c..f1321c93ecf78 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -67,6 +67,7 @@ class AP_BoardConfig { PX4_BOARD_FMUV6 = 39, FMUV6_BOARD_HOLYBRO_6X = 40, FMUV6_BOARD_CUAV_6X = 41, + FMUV6_BOARD_HOLYBRO_6X_REV6 = 42, PX4_BOARD_OLDDRIVERS = 100, }; diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index ed365120a08cf..c54f872bb94a8 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -106,6 +106,7 @@ void AP_BoardConfig::board_setup_drivers(void) case PX4_BOARD_PCNC1: case PX4_BOARD_MINDPXV2: case FMUV6_BOARD_HOLYBRO_6X: + case FMUV6_BOARD_HOLYBRO_6X_REV6: case FMUV6_BOARD_CUAV_6X: break; default: @@ -129,17 +130,15 @@ bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uin return false; } dev->set_read_flag(read_flag); - dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(dev->get_semaphore()); dev->set_speed(AP_HAL::Device::SPEED_LOW); uint8_t v; if (!dev->read_registers(regnum, &v, 1)) { #if SPI_PROBE_DEBUG hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum); #endif - dev->get_semaphore()->give(); return false; } - dev->get_semaphore()->give(); #if SPI_PROBE_DEBUG hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v); #endif @@ -161,7 +160,7 @@ bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum return false; } dev->set_read_flag(read_flag); - dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(dev->get_semaphore()); dev->set_speed(AP_HAL::Device::SPEED_LOW); uint8_t v; // select bank 0 for who am i @@ -170,10 +169,8 @@ bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum #if SPI_PROBE_DEBUG hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum); #endif - dev->get_semaphore()->give(); return false; } - dev->get_semaphore()->give(); #if SPI_PROBE_DEBUG hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v); #endif @@ -195,7 +192,7 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { if (!dev_sem) { return false; } - dev_sem->take_blocking(); + WITH_SEMAPHORE(dev_sem); static const uint8_t CMD_MS56XX_RESET = 0x1E; static const uint8_t CMD_MS56XX_PROM = 0xA0; @@ -209,7 +206,6 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { const uint8_t reg = CMD_MS56XX_PROM + (i << 1); uint8_t val[2]; if (!dev->transfer(®, 1, val, sizeof(val))) { - dev_sem->give(); #if SPI_PROBE_DEBUG hal.console->printf("%s: transfer fail\n", devname); #endif @@ -221,7 +217,6 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { all_zero = false; } } - dev_sem->give(); uint16_t crc_read = prom[7]&0xf; prom[7] &= 0xff00; @@ -257,12 +252,13 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { #define INV2_WHOAMI_ICM20649 0xE1 #define INV3REG_WHOAMI 0x75 -#define INV3REG_456_WHOAMI 0x72 +#define INV3REG_456_WHOAMI 0x72 #define INV3_WHOAMI_ICM42688 0x47 #define INV3_WHOAMI_ICM42670 0x67 - #define INV3_WHOAMI_ICM45686 0xE9 +#define INV3_WHOAMI_IIM42652 0x6f + /* validation of the board type */ @@ -503,7 +499,10 @@ void AP_BoardConfig::detect_fmuv6_variant() state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X); DEV_PRINTF("Detected CUAV 6X\n"); AP_Param::load_defaults_file("@ROMFS/param/CUAV_V6X_defaults.parm", false); + } else if (spi_check_register("iim42652", INV3REG_WHOAMI, INV3_WHOAMI_IIM42652) && + spi_check_register("icm45686", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) { + state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_REV6); + DEV_PRINTF("Detected Holybro 6X_Rev6\n"); } - } #endif diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 8a9539a4c3414..47a4b66cb0703 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -697,6 +697,25 @@ void Compass::init() return; } + /* + on init() if any devid is set then we set suppress_devid_save to + false. This is used to determine if we save device ids during + the init process. + */ + suppress_devid_save = true; + for (uint8_t i=0; i 1 + if (_priority_did_stored_list._priv_instance[i] != 0) { + suppress_devid_save = false; + break; + } +#endif + } + // convert to new custom rotation method // PARAMETER_CONVERSION - Added: Nov-2021 #if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) @@ -741,24 +760,26 @@ void Compass::init() // Load priority list from storage, the changes to priority list // by user only take effect post reboot, after this - for (Priority i(0); idelay(100); @@ -826,6 +839,7 @@ void Compass::init() #endif init_done = true; + suppress_devid_save = false; } #if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV @@ -846,7 +860,11 @@ Compass::Priority Compass::_update_priority_list(int32_t dev_id) // We are not in priority list, let's add at first empty for (Priority i(0); i= _compass_count) { _compass_count = uint8_t(i)+1; @@ -1289,6 +1307,13 @@ void Compass::_detect_backends(void) ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL()); #endif +#if AP_COMPASS_DRONECAN_ENABLED + // probe DroneCAN before I2C and SPI so that DroneCAN compasses + // default to first in the list for a new board + probe_dronecan_compasses(); + CHECK_UNREG_LIMIT_RETURN; +#endif + #ifdef HAL_PROBE_EXTERNAL_I2C_COMPASSES // allow boards to ask for external probing of all i2c compass types in hwdef.dat _probe_external_i2c_compasses(); @@ -1303,6 +1328,21 @@ void Compass::_detect_backends(void) } #endif + // finally look for i2c and spi compasses not found yet + CHECK_UNREG_LIMIT_RETURN; + probe_i2c_spi_compasses(); + + if (_backend_count == 0 || + _compass_count == 0) { + DEV_PRINTF("No Compass backends available\n"); + } +} + +/* + probe i2c and SPI compasses + */ +void Compass::probe_i2c_spi_compasses(void) +{ #if defined(HAL_MAG_PROBE_LIST) // driver probes defined by COMPASS lines in hwdef.dat HAL_MAG_PROBE_LIST; @@ -1446,87 +1486,89 @@ void Compass::_detect_backends(void) break; } #endif - +} #if AP_COMPASS_DRONECAN_ENABLED - if (_driver_enabled(DRIVER_UAVCAN)) { - for (uint8_t i=0; i 0 - if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) { - break; - } -#endif + if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) { + break; } +#endif + } #if COMPASS_MAX_UNREG_DEV > 0 - if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT)) { - // check if there's any uavcan compass in prio slot that's not found - // and replace it if there's a replacement compass - for (Priority i(0); i 0 } +#endif // #if COMPASS_MAX_UNREG_DEV > 0 +} #endif // AP_COMPASS_DRONECAN_ENABLED - if (_backend_count == 0 || - _compass_count == 0) { - DEV_PRINTF("No Compass backends available\n"); - } -} // Check if the devid is a potential replacement compass // Following are the checks done to ensure the compass is a replacement @@ -1570,7 +1612,7 @@ void Compass::remove_unreg_dev_id(uint32_t devid) #if COMPASS_MAX_UNREG_DEV > 0 for (uint8_t i = 0; imag_save_ids) { + // save so the compass always comes up configured in SITL + save_dev_id(_compass_instance[_num_compass]); + } set_rotation(instance, ROTATION_NONE); _num_compass++; } diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 3e89398599276..5b98ecbac774a 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -220,7 +220,7 @@ Published topics: Subscribed topics: * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber - * /tf [tf2_msgs/msg/TFMessage] 1 subscriber + * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber ``` ```bash diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index c97e6d9d11af0..abfa47779e9ab 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -239,7 +239,7 @@ - rt/tf + rt/ap/tf tf2_msgs::msg::dds_::TFMessage_ KEEP_LAST @@ -249,7 +249,7 @@ NO_KEY - rt/tf + rt/ap/tf tf2_msgs::msg::dds_::TFMessage_ diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 027b8f4a07bd1..7337e3dcbefd1 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -303,6 +303,10 @@ #endif #endif +#ifndef AP_BOOTLOADER_FLASHING_ENABLED +#define AP_BOOTLOADER_FLASHING_ENABLED 0 +#endif + #ifndef HAL_HNF_MAX_FILTERS // On an F7 The difference in CPU load between 1 notch and 24 notches is about 2% // The difference in CPU load between 1Khz backend and 2Khz backend is about 10% diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 6e60678d85b3a..f0aecae053c95 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -30,10 +30,6 @@ class ExpandingString; #define HAL_ENABLE_SAVE_PERSISTENT_PARAMS (defined(STM32F7) || defined(STM32H7)) #endif -#ifndef AP_BOOTLOADER_FLASHING_ENABLED -#define AP_BOOTLOADER_FLASHING_ENABLED 0 -#endif - class ChibiOS::Util : public AP_HAL::Util { public: static Util *from(AP_HAL::Util *util) { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat index 2f238089b43e1..708b574cef45f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat @@ -163,4 +163,5 @@ include ../include/no_bootloader_DFU.inc define HAL_DEFAULT_INS_FAST_SAMPLE 1 # no space for bootloader in firmware image: +include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F4BY/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F4BY/hwdef.dat index 0f484d47451cf..d47c7d626e8d9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F4BY/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F4BY/hwdef.dat @@ -162,3 +162,5 @@ PC9 EXTERN_GPIO5 OUTPUT GPIO(5) # IRQ for MPU6000 PB0 EXTI_MPU6000 INPUT PULLUP PB1 DRDY_HMC5883 INPUT PULLUP + +include ../include/minimize_features.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF407/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF407/hwdef.dat index b21c3ec1dd3ea..1a24d497a0816 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF407/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF407/hwdef.dat @@ -175,3 +175,5 @@ PE5 TIM9_CH1 TIM9 ALARM # IRQ for MPU6000 PB0 DRDY_ICM20689 INPUT PULLUP PB1 DRDY_IST8310 INPUT PULLUP + +include ../include/minimize_features.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat index d159f6b2f6515..de334979da0cd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF405S-AIO/hwdef.dat @@ -152,3 +152,5 @@ define DEFAULT_NTF_LED_TYPES 257 # save some flash space include ../include/no_bootloader_DFU.inc + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A-RX2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A-RX2/hwdef.dat index f4b81633c89cd..5df908756f41d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A-RX2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A-RX2/hwdef.dat @@ -25,4 +25,6 @@ PA3 TIM2_CH4 TIM2 PWM(3) GPIO(52) PA2 TIM2_CH3 TIM2 PWM(4) GPIO(53) BIDIR DMA_PRIORITY SPI1* SPI3* TIM1_CH2 TIM2_UP TIM3_UP -DMA_NOSHARE USART2* \ No newline at end of file +DMA_NOSHARE USART2* + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat index 9a5766c43e103..85ee746142dae 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat @@ -169,3 +169,5 @@ define HAL_BUTTON_ENABLED 0 define AP_NOTIFY_OREOLED_ENABLED 0 define HAL_PICCOLO_CAN_ENABLE 0 define BARO_MAX_INSTANCES 1 + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4Mini/hwdef.dat index 6a16d98855fef..45a23fa080259 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4Mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4Mini/hwdef.dat @@ -41,7 +41,6 @@ define STM32_PWM_USE_ADVANCED TRUE # save some flash include ../include/save_some_flash.inc -define AP_BATTERY_SMBUS_ENABLED 0 define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 define HAL_MOUNT_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat index 5ae33ca8d5f2c..4c8b7c627e664 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat @@ -145,6 +145,7 @@ define HAL_PARACHUTE_ENABLED 0 # save FLASH, but leave above when flash issue is fixed include ../include/minimize_fpv_osd.inc +include ../include/no_bootloader_DFU.inc # disable SMBUS battery monitors to save flash undef AP_BATTERY_SMBUS_ENABLED diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat index ddcd61dd36176..effd88691850e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat @@ -156,9 +156,6 @@ define OSD_ENABLED 1 define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin -# disable SMBUS battery monitors to save flash -define AP_BATTERY_SMBUS_ENABLED 0 - # disable parachute and sprayer to save flash define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 @@ -168,6 +165,5 @@ define AP_PARAM_MAX_EMBEDDED_PARAM 1024 # save some flash include ../include/save_some_flash.inc +include ../include/minimize_fpv_osd.inc -# only include ublox GPS driver -include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat index cd7183301b957..1419ab5aa2e05 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat @@ -121,30 +121,31 @@ PC3 ADC1_3V3 ADC1 SCALE(1) PA5 SPI1_SCK SPI1 PB5 SPI1_MOSI SPI1 PG9 SPI1_MISO SPI1 -PI9 IMU1_CS CS +PI9 SP1_CS1 CS # SPI2 - ICM42688 PI1 SPI2_SCK SPI2 PI2 SPI2_MISO SPI2 PI3 SPI2_MOSI SPI2 -PH5 ICM42688_CS CS -PA10 ICM42688_DRDY INPUT +PH5 SP2_CS1 CS +PA10 SP2_DRDY2 INPUT # SPI3 - BMI088 PB2 SPI3_MOSI SPI3 PC10 SPI3_SCK SPI3 PC11 SPI3_MISO SPI3 -PI4 BMI088_A_CS CS -PI8 BMI088_G_CS CS -PI6 BMI088_DRDY_ACC INPUT -PI7 BMI088_DRDY_GYR INPUT - -# SPI4 - BMM150 -PE12 SPI4_SCK SPI4 -PE13 SPI4_MISO SPI4 -PE14 SPI4_MOSI SPI4 -PF3 BMM150_DRDY INPUT -PH15 BMM150_CS CS +PI4 SP3_CS1 CS +PI8 SP3_CS2 CS +PI6 SP3_DRDY1 INPUT +PI7 SP3_DRDY2 INPUT GPIO(93) +define SP3_DRDY2 93 + +# SPI4 - unused +#PE12 SPI4_SCK SPI4 +#PE13 SPI4_MISO SPI4 +#PE14 SPI4_MOSI SPI4 +#PF3 SP4_DRDY1 INPUT +PH15 SP4_CS1 CS # SPI5 - FRAM PF7 SPI5_SCK SPI5 @@ -311,21 +312,26 @@ define HAL_HEATER_MAG_OFFSET_BMM150 AP_HAL::Device::make_bus_id(AP_HAL::Device:: define HAL_HEATER_MAG_OFFSET {HAL_HEATER_MAG_OFFSET_RM3100, HAL_HEATER_MAG_OFFSET_BMM150} # IMU devices for Holybro6X -SPIDEV bmi088_g SPI3 DEVID1 BMI088_G_CS MODE3 10*MHZ 10*MHZ -SPIDEV bmi088_a SPI3 DEVID2 BMI088_A_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_g SPI3 DEVID1 SP3_CS2 MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI3 DEVID2 SP3_CS1 MODE3 10*MHZ 10*MHZ # alternative to bmi088 -SPIDEV icm20649 SPI3 DEVID1 BMI088_A_CS MODE3 10*MHZ 10*MHZ -SPIDEV icm42688 SPI2 DEVID1 ICM42688_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm42670 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ +SPIDEV icm20649 SPI3 DEVID1 SP3_CS1 MODE3 10*MHZ 10*MHZ +SPIDEV icm42688 SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ +SPIDEV icm42670 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ # IMU devices for CUAV-6X. The CUAV board has a BMI088, ICM20649 and # ICM42688 the ICM42688 and BMI088 are on the same SPI buses and CS # pins as the Holybro board, but the orientation of the BMI088 is # different. The ICM20649 is on a different bus -SPIDEV icm20649_2 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ +SPIDEV icm20649_2 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ SPIDEV ramtron SPI5 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ +# IMU devices for Holybro6X-Rev6 +SPIDEV adis16470 SPI3 DEVID1 SP3_CS1 MODE3 1*MHZ 2*MHZ +SPIDEV iim42652 SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ +SPIDEV icm45686 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ + # Holybro6X 3 IMUs IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X) IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X) @@ -337,6 +343,11 @@ IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270 BOARD_MATCH(FMUV6_BOARD_ IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 BOARD_MATCH(FMUV6_BOARD_CUAV_6X) IMU Invensensev2 SPI:icm20649_2 ROTATION_ROLL_180 BOARD_MATCH(FMUV6_BOARD_CUAV_6X) +# Holybro6X-Rev6 3 IMUs +IMU ADIS1647x SPI:adis16470 ROTATION_ROLL_180 SP3_DRDY2 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_REV6) +IMU Invensensev3 SPI:iim42652 ROTATION_ROLL_180_YAW_270 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_REV6) +IMU Invensensev3 SPI:icm45686 ROTATION_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_REV6) + define HAL_DEFAULT_INS_FAST_SAMPLE 7 # enable RAMTROM parameter storage diff --git a/libraries/AP_HAL_ChibiOS/hwdef/ReaperF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/ReaperF745/hwdef.dat index 7851723cf350f..ad2e4b04c33fe 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/ReaperF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/ReaperF745/hwdef.dat @@ -151,6 +151,5 @@ include ../include/minimize_fpv_osd.inc define AP_GRIPPER_ENABLED 0 define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 -define AP_BATTERY_SMBUS_ENABLED 0 include ../include/no_bootloader_DFU.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat index af33c2cbac037..e5a1777c032cc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat @@ -134,3 +134,5 @@ define AP_PARAM_MAX_EMBEDDED_PARAM 1024 # minimal drivers to reduce flash usage include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VRBrain-v51/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/VRBrain-v51/hwdef.dat index 1e552e60200e4..a4e96f0fdd157 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/VRBrain-v51/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/VRBrain-v51/hwdef.dat @@ -164,4 +164,4 @@ define HAL_BATT_CURR_SCALE 17.0 define STM32_PWM_USE_ADVANCED TRUE # save some flash -include ../include/save_some_flash.inc +include ../include/minimize_features.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat index 14c118e2f0597..c3e1fc855edda 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/crazyflie2/hwdef.dat @@ -121,3 +121,6 @@ define MAG_BOARD_ORIENTATION ROTATION_YAW_180 # no ADC pins define HAL_USE_ADC FALSE + +include ../include/minimize_features.inc + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index 5975b34ec331c..d083fd585ee34 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -126,3 +126,8 @@ define AP_AIRSPEED_MS5525_ENABLED 1 define AP_AIRSPEED_SDP3X_ENABLED 1 define AP_AIRSPEED_NMEA_ENABLED 1 # additional checks for vehicle type in .cpp define AP_AIRSPEED_DRONECAN_ENABLED HAL_ENABLE_DRONECAN_DRIVERS + +# don't need payload place mission items on these boards, it's very niche: +define AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED 0 +# don't need the payload place flight behaviour either: +define AC_PAYLOAD_PLACE_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/mini-pix/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/mini-pix/hwdef.dat index 33965ca5ddddc..eef2648885895 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/mini-pix/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/mini-pix/hwdef.dat @@ -148,3 +148,5 @@ define HAL_GPIO_C_LED_PIN 2 define HAL_GPIO_LED_ON 0 define HAL_GPIO_LED_OFF 1 +include ../include/minimize_features.inc + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 5a2b3c87d8e8f..927ddf861d803 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1478,7 +1478,7 @@ def write_SPI_table(self, f): devlist = [] for dev in self.spidev: if len(dev) != 7: - print("Badly formed SPIDEV line %s" % dev) + self.error("Badly formed SPIDEV line %s" % dev) name = '"' + dev[0] + '"' bus = dev[1] devid = dev[2] diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat index b22055469070d..d22798c436d81 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat @@ -147,4 +147,5 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # minimal drivers to reduce flash usage include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc +undef AP_BATTERY_SYNTHETIC_CURRENT_ENABLED define AP_BATTERY_SYNTHETIC_CURRENT_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat index 74e37b49cfa83..f313a8b83f47d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4v3/hwdef.dat @@ -154,6 +154,7 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # minimal drivers to reduce flash usage include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc +undef AP_BATTERY_SYNTHETIC_CURRENT_ENABLED define AP_BATTERY_SYNTHETIC_CURRENT_ENABLED 0 define DEFAULT_NTF_LED_TYPES 257 diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index ec2974faaf548..2e7d3c61d7d70 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -443,3 +443,10 @@ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes) } return last_receive_us; } + +uint32_t UARTDriver::bw_in_bytes_per_second() const +{ + // if connected, assume at least a 10/100Mbps connection + const uint32_t bitrate = (_connected && _ip != nullptr) ? 10E6 : _baudrate; + return bitrate/10; // convert bits to bytes minus overhead +}; \ No newline at end of file diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index d456d91db51ea..fa271cb08add0 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -54,6 +54,8 @@ class UARTDriver : public AP_HAL::UARTDriver { */ uint64_t receive_time_constraint_us(uint16_t nbytes) override; + uint32_t bw_in_bytes_per_second() const override; + private: AP_HAL::OwnPtr _device; bool _console; diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 7b43217e67f1e..9c90928630dea 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -987,5 +987,11 @@ ssize_t UARTDriver::get_system_outqueue_length() const #endif } +uint32_t UARTDriver::bw_in_bytes_per_second() const +{ + // if connected, assume at least a 10/100Mbps connection + const uint32_t bitrate = (_connected && _tcp_client_addr != nullptr) ? 10E6 : _uart_baudrate; + return bitrate/10; // convert bits to bytes minus overhead +}; #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index fb0476550bb21..d47378a8054da 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -46,6 +46,8 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { void set_stop_bits(int n) override; bool set_unbuffered_writes(bool on) override; + uint32_t bw_in_bytes_per_second() const override; + void _timer_tick(void) override; /* diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index ade41a1578ad4..852df82dbe535 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -142,7 +142,7 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Param: OPTIONS // @DisplayName: ICE options - // @Description: Options for ICE control. The Disable ignition in RC failsafe option will cause the ignition to be set off on any R/C failsafe. If Throttle while disarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. If disable while disarmed is set the engine will not start while the vehicle is disarmed. + // @Description: Options for ICE control. The Disable ignition in RC failsafe option will cause the ignition to be set off on any R/C failsafe. If Throttle while disarmed is set then throttle control will be allowed while disarmed for planes when in MANUAL mode. If disable while disarmed is set the engine will not start while the vehicle is disarmed unless overriden by the MAVLink DO_ENGINE_CONTROL command. // @Bitmask: 0:Disable ignition in RC failsafe,1:Disable redline governor,2:Throttle while disarmed,3:Disable while disarmed AP_GROUPINFO("OPTIONS", 15, AP_ICEngine, options, 0), diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index b56a3903869c6..110d3586b64f1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -127,8 +127,8 @@ extern const AP_HAL::HAL& hal; #define INV3BANK_456_IPREG_SYS1_ADDR 0xA400 #define INV3BANK_456_IPREG_SYS2_ADDR 0xA500 -// ICM42688 specific registers -#define INV3REG_42688_INTF_CONFIG1 0x4d +// ICM42xxx specific registers +#define INV3REG_42XXX_INTF_CONFIG1 0x4d // WHOAMI values #define INV3_ID_ICM40605 0x33 @@ -802,6 +802,28 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, (accel_aaf_deltsqr & 0xFF)); // ACCEL_AAF_DELTSQR register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, ((accel_aaf_bitshift<<4) & 0xF0) | ((accel_aaf_deltsqr>>8) & 0x0F)); // ACCEL_AAF_BITSHIFT | ACCEL_AAF_DELTSQR + switch (inv3_type) { + case Invensensev3_Type::ICM42688: + case Invensensev3_Type::ICM42605: + case Invensensev3_Type::IIM42652: + case Invensensev3_Type::ICM42670: { + /* + fix for the "stuck gyro" issue, which affects all IxM42xxx + sensors. This disables the AFSR feature which changes the + noise sensitivity with angular rate. When the switch happens + (at around 100 deg/sec) the gyro gets stuck for around 2ms, + producing constant output which causes a DC gyro bias + */ + const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1); + register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40, true); + break; + } + case Invensensev3_Type::ICM40605: + case Invensensev3_Type::ICM40609: + case Invensensev3_Type::ICM45686: + break; + } + // enable gyro and accel in low-noise modes register_write(INV3REG_PWR_MGMT0, 0x0F); hal.scheduler->delay_microseconds(300); @@ -826,6 +848,10 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void) register_write(INV3REG_70_ACCEL_CONFIG0, 0x05); // AAF is not available for accels, so LPF at 180Hz register_write(INV3REG_70_ACCEL_CONFIG1, 0x01); + + // fix "stuck" gyro issue + const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1); + register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40); } /* @@ -984,7 +1010,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) { WITH_SEMAPHORE(dev->get_semaphore()); - dev->setup_checked_registers(7, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); + dev->setup_checked_registers(8, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); // initially run the bus at low speed dev->set_speed(AP_HAL::Device::SPEED_LOW); @@ -1074,10 +1100,6 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) // disable STC uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68); // I3C_STC_MODE b2 register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x04); - } else if (inv3_type == Invensensev3_Type::ICM42688) { - // fix for the "stuck gyro" issue - const uint8_t v = register_read(INV3REG_42688_INTF_CONFIG1); - register_write(INV3REG_42688_INTF_CONFIG1, (v & 0x3F) | 0x40); } return true; diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 617d8a77aa1bf..a000e7ba91116 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -265,7 +265,7 @@ void AP_Mission::reset() } /// clear - clears out mission -/// returns true if mission was running so it could not be cleared +/// returns false if mission was running so it could not be cleared bool AP_Mission::clear() { // do not allow clearing the mission while it is running unless disarmed @@ -1250,9 +1250,11 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.do_engine_control.allow_disarmed_start = (((uint32_t)packet.param4) & ENGINE_CONTROL_OPTIONS_ALLOW_START_WHILE_DISARMED) != 0; break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm) break; +#endif case MAV_CMD_NAV_SET_YAW_SPEED: cmd.content.set_yaw_speed.angle_deg = packet.param1; // target angle in degrees @@ -1757,9 +1759,11 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c } break; +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (cm->m) break; +#endif case MAV_CMD_NAV_SET_YAW_SPEED: packet.param1 = cmd.content.set_yaw_speed.angle_deg; // target angle in degrees @@ -2617,8 +2621,10 @@ const char *AP_Mission::Mission_Command::type() const case MAV_CMD_DO_GRIPPER: return "Gripper"; #endif +#if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: return "PayloadPlace"; +#endif case MAV_CMD_DO_PARACHUTE: return "Parachute"; case MAV_CMD_DO_SPRAYER: diff --git a/libraries/AP_Mission/AP_Mission_config.h b/libraries/AP_Mission/AP_Mission_config.h index 94be328bcfaa8..fee3f39a3b5e8 100644 --- a/libraries/AP_Mission/AP_Mission_config.h +++ b/libraries/AP_Mission/AP_Mission_config.h @@ -3,3 +3,7 @@ #ifndef AP_MISSION_ENABLED #define AP_MISSION_ENABLED 1 #endif + +#ifndef AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED +#define AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED 1 +#endif diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5f582ffdbbc00..c7f1701fbfe9e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -505,12 +505,6 @@ MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t return MAV_RESULT_ACCEPTED; } -MAV_RESULT AP_Mount::handle_command_do_set_roi_none() -{ - set_mode_to_default(); - return MAV_RESULT_ACCEPTED; -} - MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { @@ -524,8 +518,6 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m return handle_command_do_gimbal_manager_configure(packet, msg); case MAV_CMD_DO_SET_ROI_SYSID: return handle_command_do_set_roi_sysid(packet); - case MAV_CMD_DO_SET_ROI_NONE: - return handle_command_do_set_roi_none(); default: return MAV_RESULT_UNSUPPORTED; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index d349da56b77b5..7e8dc661d6028 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -181,9 +181,6 @@ class AP_Mount // handling of set_roi_sysid message MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet); - // handling of set_roi_none message - MAV_RESULT handle_command_do_set_roi_none(); - // mavlink message handling: MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void handle_param_value(const mavlink_message_t &msg); diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp index 9aa153706ab3e..0e643d8d863ad 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -554,7 +554,7 @@ void AP_OpenDroneID::send_operator_id_message() */ MAV_ODID_HOR_ACC AP_OpenDroneID::create_enum_horizontal_accuracy(float accuracy) const { - // Out of bounds return UKNOWN flag + // Out of bounds return UNKNOWN flag if (accuracy < 0.0 || accuracy >= 18520.0) { return MAV_ODID_HOR_ACC_UNKNOWN; } @@ -595,7 +595,7 @@ MAV_ODID_HOR_ACC AP_OpenDroneID::create_enum_horizontal_accuracy(float accuracy) */ MAV_ODID_VER_ACC AP_OpenDroneID::create_enum_vertical_accuracy(float accuracy) const { - // Out of bounds return UKNOWN flag + // Out of bounds return UNKNOWN flag if (accuracy < 0.0 || accuracy >= 150.0) { return MAV_ODID_VER_ACC_UNKNOWN; } @@ -630,7 +630,7 @@ MAV_ODID_VER_ACC AP_OpenDroneID::create_enum_vertical_accuracy(float accuracy) c */ MAV_ODID_SPEED_ACC AP_OpenDroneID::create_enum_speed_accuracy(float accuracy) const { - // Out of bounds return UKNOWN flag + // Out of bounds return UNKNOWN flag if (accuracy < 0.0 || accuracy >= 10.0) { return MAV_ODID_SPEED_ACC_UNKNOWN; } @@ -657,7 +657,7 @@ MAV_ODID_SPEED_ACC AP_OpenDroneID::create_enum_speed_accuracy(float accuracy) co */ MAV_ODID_TIME_ACC AP_OpenDroneID::create_enum_timestamp_accuracy(float accuracy) const { - // Out of bounds return UKNOWN flag + // Out of bounds return UNKNOWN flag if (accuracy < 0.0 || accuracy >= 1.5) { return MAV_ODID_TIME_ACC_UNKNOWN; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index fac5238b4954a..0675f5c191fe5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -537,7 +537,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::Lua_Scripting: -#if AP_SCRIPTING_ENABLED +#if AP_RANGEFINDER_LUA_ENABLED _add_backend(new AP_RangeFinder_Lua(state[instance], params[instance]), instance); #endif break; diff --git a/libraries/AP_Scripting/applets/Hexsoon LEDs.lua b/libraries/AP_Scripting/applets/Hexsoon LEDs.lua index 142e1e49b7a3a..29b0cad6e94b4 100644 --- a/libraries/AP_Scripting/applets/Hexsoon LEDs.lua +++ b/libraries/AP_Scripting/applets/Hexsoon LEDs.lua @@ -25,12 +25,11 @@ LEDs should now work!, if not try swapping AUX 5 and 6, either by physically swa To get colours to match either change the ordering in "local led_map =" below or swap headers round on the LED distribution board If using 6 les add two extra colours to "local led_map =" e.g: "local led_map = {red, red, red, green, green, green}" --]] --- luacheck: only 0 -- helper colours, red, green, blue values from 0 to 255 local red = {255, 0, 0} local green = {0, 255, 0} -local blue = {0, 0, 255} +-- local blue = {0, 0, 255} -- led map giving the colour for the LEDs plugged in local led_map = {red, red, green, green} diff --git a/libraries/AP_Scripting/examples/README.md b/libraries/AP_Scripting/examples/README.md new file mode 100644 index 0000000000000..3543347b0c8a7 --- /dev/null +++ b/libraries/AP_Scripting/examples/README.md @@ -0,0 +1,5 @@ +#LUA Examples + + +This directory contains some script examples. For simulation the scripts should be placed in the simulation's "scripts" directory located in the root directory from which the sim is run. +Note: if an example uses auxillary files (mission list, other scripts,etc.) the user will have to determine where the files should be placed from the script. If no path name is specified in the file name, then it should be placed one directory level above the "scripts" directory. diff --git a/libraries/AP_Scripting/examples/Serial_Dump.lua b/libraries/AP_Scripting/examples/Serial_Dump.lua index 926a97353e7a7..bfff5392e8c35 100644 --- a/libraries/AP_Scripting/examples/Serial_Dump.lua +++ b/libraries/AP_Scripting/examples/Serial_Dump.lua @@ -1,5 +1,4 @@ -- this script reads data from a serial port and dumps it to a file --- luacheck: only 0 local file_name = 'raw serial dump.txt' local file_name_plain = 'serial dump.txt' @@ -11,6 +10,7 @@ local port = assert(serial:find_serial(0),"Could not find Scripting Serial Port" -- make a file local file = assert(io.open(file_name, "w"),"Could not create file " .. file_name) +file:close() file = assert(io.open(file_name_plain, "w"),"Could not create file " .. file_name) file:close() diff --git a/libraries/AP_Scripting/examples/aux_cached.lua b/libraries/AP_Scripting/examples/aux_cached.lua index 1e514055a97e6..e029c44e985c4 100644 --- a/libraries/AP_Scripting/examples/aux_cached.lua +++ b/libraries/AP_Scripting/examples/aux_cached.lua @@ -1,7 +1,6 @@ --[[ example for getting cached aux function value --]] --- luacheck: only 0 local RATE_HZ = 10 @@ -11,7 +10,6 @@ local MAV_SEVERITY_INFO = 6 local AUX_FUNCTION_NUM = 302 -local last_func_val = nil local last_aux_pos = nil function update() diff --git a/libraries/AP_Scripting/examples/copy_userdata.lua b/libraries/AP_Scripting/examples/copy_userdata.lua index 6e68c97406015..5f186e3f10058 100644 --- a/libraries/AP_Scripting/examples/copy_userdata.lua +++ b/libraries/AP_Scripting/examples/copy_userdata.lua @@ -1,7 +1,6 @@ --[[ An example of using the copy() method on userdata --]] --- luacheck: only 0 local loc1 = Location() @@ -24,5 +23,5 @@ local v2 = v1:copy() v2:x(v2:x()+100) v2:y(v2:y()+300) -local diff = v2 - v1 +diff = v2 - v1 gcs:send_text(0,string.format("vdiff=(%.2f,%.2f)", diff:x(), diff:y())) diff --git a/libraries/AP_Scripting/examples/mission-load.lua b/libraries/AP_Scripting/examples/mission-load.lua index 33d19d66750aa..76f465b1fbb55 100644 --- a/libraries/AP_Scripting/examples/mission-load.lua +++ b/libraries/AP_Scripting/examples/mission-load.lua @@ -1,6 +1,9 @@ -- Example of loading a mission from the SD card using Scripting -- Would be trivial to select a mission based on scripting params or RC switch -- luacheck: only 0 +--Copy this "mission-load.lua" script to the "scripts" directory of the simulation or autopilot's SD card. +--The "mission1.txt" file containing the mission items should be placed in the directory above the "scripts" directory. +--In case of placing it on SD Card, mission1.txt file should be placed in the APM directory root. diff --git a/libraries/AP_Scripting/examples/opendog_demo.lua b/libraries/AP_Scripting/examples/opendog_demo.lua index d518b95c6e0e1..cefe8f17360d1 100644 --- a/libraries/AP_Scripting/examples/opendog_demo.lua +++ b/libraries/AP_Scripting/examples/opendog_demo.lua @@ -1,14 +1,9 @@ -- demo of waving paw of opendog --- luacheck: only 0 -local flipflop = true - -pwm = { 1500, 1500, 2000, - 1500, 1500, 1000, - 1500, 1500, 1500, - 1500, 1500, 1500 } - -local angle = 0.0 +local pwm = { 1500, 1500, 2000, + 1500, 1500, 1000, + 1500, 1500, 1500, + 1500, 1500, 1500 } function update() local t = 0.001 * millis():tofloat() diff --git a/libraries/AP_Scripting/examples/param_get_set_test.lua b/libraries/AP_Scripting/examples/param_get_set_test.lua index 07e84764cc7a0..29baba29b6075 100644 --- a/libraries/AP_Scripting/examples/param_get_set_test.lua +++ b/libraries/AP_Scripting/examples/param_get_set_test.lua @@ -1,5 +1,4 @@ -- This script is a test of param set and get --- luacheck: only 0 local count = 0 @@ -21,7 +20,9 @@ local user_param = Parameter('SCR_USER1') -- this will error out for a bad parameter -- Parameter('FAKE_PARAM') local success, err = pcall(Parameter,'FAKE_PARAM') -gcs:send_text(0, "Lua Caught Error: " .. err) +if not success then + gcs:send_text(0, "Lua Caught Error: " .. err) +end -- this allows this example to catch the otherwise fatal error -- not recommend if error is possible/expected, use separate construction and init diff --git a/libraries/AP_Scripting/examples/rangefinder_test.lua b/libraries/AP_Scripting/examples/rangefinder_test.lua index af5058d8a5834..b6832bf54be35 100644 --- a/libraries/AP_Scripting/examples/rangefinder_test.lua +++ b/libraries/AP_Scripting/examples/rangefinder_test.lua @@ -1,5 +1,4 @@ -- This script checks RangeFinder --- luacheck: only 0 local rotation_downward = 25 local rotation_forward = 0 @@ -8,7 +7,7 @@ function update() local sensor_count = rangefinder:num_sensors() gcs:send_text(0, string.format("%d rangefinder sensors found.", sensor_count)) - for i = 0, rangefinder:num_sensors() do + for _ = 0, rangefinder:num_sensors() do if rangefinder:has_data_orient(rotation_downward) then info(rotation_downward) elseif rangefinder:has_data_orient(rotation_forward) then diff --git a/libraries/AP_Scripting/examples/rover-SaveTurns.lua b/libraries/AP_Scripting/examples/rover-SaveTurns.lua index 58cffa4833d2a..9e530f2331335 100644 --- a/libraries/AP_Scripting/examples/rover-SaveTurns.lua +++ b/libraries/AP_Scripting/examples/rover-SaveTurns.lua @@ -11,7 +11,6 @@ of a vehicle. Use this script AT YOUR OWN RISK. LICENSE - GNU GPLv3 https://www.gnu.org/licenses/gpl-3.0.en.html ------------------------------------------------------------------------------]] --- luacheck: only 0 local SCRIPT_NAME = 'SaveTurns' @@ -33,7 +32,6 @@ local WAYPOINT = 16 -- waypoint command local MAV_SEVERITY_WARNING = 4 local MAV_SEVERITY_INFO = 6 local MSG_NORMAL = 1 -local MSG_DEBUG = 2 local RC_CHAN = rc:find_channel_for_option(RC_OPTION) local last_wp = Location() diff --git a/libraries/AP_Scripting/examples/set-target-location.lua b/libraries/AP_Scripting/examples/set-target-location.lua index d40ca3c4c17b7..b7c2776b5db9f 100644 --- a/libraries/AP_Scripting/examples/set-target-location.lua +++ b/libraries/AP_Scripting/examples/set-target-location.lua @@ -5,7 +5,6 @@ -- a) switches to Guided mode -- b) sets the target location to be 10m above home -- c) switches the vehicle to land once it is within a couple of meters of home --- luacheck: only 0 local wp_radius = 2 local target_alt_above_home = 10 diff --git a/libraries/AP_Scripting/examples/set_target_posvel_circle.lua b/libraries/AP_Scripting/examples/set_target_posvel_circle.lua index 45c12a0ebc14c..87d4d95382634 100644 --- a/libraries/AP_Scripting/examples/set_target_posvel_circle.lua +++ b/libraries/AP_Scripting/examples/set_target_posvel_circle.lua @@ -7,7 +7,6 @@ -- 2) switch to GUIDED mode -- 3) the vehilce will follow a circle in clockwise direction with increasing speed until ramp_up_time_s time has passed. -- 4) switch out of and into the GUIDED mode any time to restart the trajectory from the start. --- luacheck: only 0 -- Edit these variables local rad_xy_m = 10.0 -- circle radius in xy plane in m @@ -26,7 +25,7 @@ gcs:send_text(0,"Script started") gcs:send_text(0,"Trajectory period: " .. tostring(2 * math.rad(180) / omega_radps)) function circle() - local cur_freq = 0 + local cur_freq -- increase target speed lineary with time until ramp_up_time_s is reached if time <= ramp_up_time_s then cur_freq = omega_radps*(time/ramp_up_time_s)^2 @@ -57,9 +56,7 @@ function update() if arming:is_armed() and vehicle:get_mode() == copter_guided_mode_num and -test_start_location:z()>=5 then -- calculate current position and velocity for circle trajectory - local target_pos = Vector3f() - local target_vel = Vector3f() - target_pos, target_vel = circle() + local target_pos, target_vel = circle() -- advance the time time = time + sampling_time_s diff --git a/libraries/AP_Scripting/examples/terrain_warning.lua b/libraries/AP_Scripting/examples/terrain_warning.lua index 4add211232381..553804f35f7b7 100644 --- a/libraries/AP_Scripting/examples/terrain_warning.lua +++ b/libraries/AP_Scripting/examples/terrain_warning.lua @@ -1,5 +1,4 @@ -- height above terrain warning script --- luacheck: only 0 -- min altitude above terrain, script will warn if lower than this local terrain_min_alt = 20 @@ -15,7 +14,6 @@ local warn_ms = 10000 local height_threshold_passed = false -local last_warn = 0 function update() if not arming:is_armed() then diff --git a/libraries/AP_Scripting/examples/wp_test.lua b/libraries/AP_Scripting/examples/wp_test.lua index 7f1e5e889b7f8..a52d483343b85 100644 --- a/libraries/AP_Scripting/examples/wp_test.lua +++ b/libraries/AP_Scripting/examples/wp_test.lua @@ -1,12 +1,9 @@ -- Example script for accessing waypoint info --- luacheck: only 0 local wp_index local wp_distance local wp_bearing local wp_error -local wp_max_distance = 0 -local last_log_ms = millis() function on_wp_change(index, distance) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index e049d31cbbc54..f5892d6858572 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -390,7 +390,7 @@ void AP_TECS::_update_speed(float DT) _vel_dot_lpf = _vel_dot_lpf * (1.0f - alpha) + _vel_dot * alpha; } - bool use_airspeed = _use_synthetic_airspeed_once || _use_synthetic_airspeed.get() || _ahrs.airspeed_sensor_enabled(); + bool use_airspeed = _use_synthetic_airspeed_once || _use_synthetic_airspeed.get() || _ahrs.using_airspeed_sensor(); // Convert equivalent airspeeds to true airspeeds and harmonise limits @@ -923,7 +923,7 @@ void AP_TECS::_update_pitch(void) // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. _SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); - if (!(_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed)) { + if (!(_ahrs.using_airspeed_sensor() || _use_synthetic_airspeed)) { _SKE_weighting = 0.0f; } else if (_flight_stage == AP_FixedWing::FlightStage::VTOL) { // if we are in VTOL mode then control pitch without regard to @@ -1346,7 +1346,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // Note that caller can demand the use of // synthetic airspeed for one loop if needed. This is required // during QuadPlane transition when pitch is constrained - if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) { + if (_ahrs.using_airspeed_sensor() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) { _update_throttle_with_airspeed(); _use_synthetic_airspeed_once = false; _using_airspeed_for_throttle = true; diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp index 5a5a7bc9b9074..4a3a41db4021e 100644 --- a/libraries/AP_Winch/AP_Winch.cpp +++ b/libraries/AP_Winch/AP_Winch.cpp @@ -110,7 +110,7 @@ void AP_Winch::release_length(float length) // display verbose output to user if (backend->option_enabled(Options::VerboseOutput)) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Winch: lowering %4.1fm to %4.1fm", (double)length, (double)config.length_desired); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Winch: %s %4.1fm to %4.1fm", is_negative(length) ? "raising" : "lowering", (double)fabsf(length), (double)config.length_desired); } } diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index 21da2625214b4..d80a8f58760d3 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -344,7 +344,7 @@ void AP_Winch_Daiwa::update_user() if (latest.moving < ARRAY_SIZE(moving_str)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, moving_str[latest.moving]); } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s move state uknown", send_text_prefix); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s move state unknown", send_text_prefix); } update_sent = true; } @@ -357,7 +357,7 @@ void AP_Winch_Daiwa::update_user() if (user_update.clutch < ARRAY_SIZE(clutch_str)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch %s", send_text_prefix, clutch_str[latest.moving]); } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch state uknown", send_text_prefix); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch state unknown", send_text_prefix); } update_sent = true; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f695999aff33d..8a863fff42a55 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -23,6 +23,7 @@ #include #include #include +#include #include "ap_message.h" @@ -338,7 +339,9 @@ class GCS_MAVLINK void send_distance_sensor(); // send_rangefinder sends only if a downward-facing instance is // found. Rover overrides this! +#if AP_RANGEFINDER_ENABLED virtual void send_rangefinder() const; +#endif void send_proximity(); virtual void send_nav_controller_output() const = 0; virtual void send_pid_tuning() = 0; @@ -587,7 +590,7 @@ class GCS_MAVLINK bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const; MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet); - MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); + MAV_RESULT handle_START_RX_PAIR(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet); @@ -626,7 +629,7 @@ class GCS_MAVLINK virtual uint32_t telem_delay() const = 0; MAV_RESULT handle_command_run_prearm_checks(const mavlink_command_long_t &packet); - MAV_RESULT handle_command_flash_bootloader(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_flash_bootloader(const mavlink_command_int_t &packet); // generally this should not be overridden; Plane overrides it to ensure // failsafe isn't triggered during calibration diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a51ca9d2fccc4..fb46ac9bc06ee 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -378,6 +378,7 @@ bool GCS_MAVLINK::send_battery_status() } #endif // AP_BATTERY_ENABLED +#if AP_RANGEFINDER_ENABLED void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const { if (!sensor->has_data()) { @@ -408,11 +409,14 @@ void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, con (const float *)nullptr, // quaternion of sensor orientation for MAV_SENSOR_ROTATION_CUSTOM quality); // Signal quality of the sensor. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. } +#endif // AP_RANGEFINDER_ENABLED + // send any and all distance_sensor messages. This starts by sending // any distance sensors not used by a Proximity sensor, then sends the // proximity sensor ones. void GCS_MAVLINK::send_distance_sensor() { +#if AP_RANGEFINDER_ENABLED RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder == nullptr) { return; @@ -450,12 +454,14 @@ void GCS_MAVLINK::send_distance_sensor() send_distance_sensor(sensor, i); } } +#endif // AP_RANGEFINDER_ENABLED #if HAL_PROXIMITY_ENABLED send_proximity(); #endif } +#if AP_RANGEFINDER_ENABLED void GCS_MAVLINK::send_rangefinder() const { RangeFinder *rangefinder = RangeFinder::get_singleton(); @@ -471,6 +477,7 @@ void GCS_MAVLINK::send_rangefinder() const s->distance(), s->voltage_mv() * 0.001f); } +#endif #if HAL_PROXIMITY_ENABLED void GCS_MAVLINK::send_proximity() @@ -3069,10 +3076,14 @@ float GCS_MAVLINK::vfr_hud_airspeed() const } #endif +#if AP_GPS_ENABLED // because most vehicles don't have airspeed sensors, we return a // different sort of speed estimate in the relevant field for // comparison's sake. return AP::gps().ground_speed(); +#endif + + return 0.0; } float GCS_MAVLINK::vfr_hud_climbrate() const @@ -3276,7 +3287,7 @@ MAV_RESULT GCS_MAVLINK::handle_flight_termination(const mavlink_command_int_t &p /* handle a R/C bind request (for spektrum) */ -MAV_RESULT GCS_MAVLINK::handle_rc_bind(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_START_RX_PAIR(const mavlink_command_int_t &packet) { // initiate bind procedure. We accept the DSM type from either // param1 or param2 due to a past mixup with what parameter is the @@ -3766,24 +3777,24 @@ MAV_RESULT GCS_MAVLINK::handle_can_forward(const mavlink_command_int_t &packet, { return AP::can().handle_can_forward(chan, packet, msg) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; } -#endif /* handle CAN_FRAME messages */ void GCS_MAVLINK::handle_can_frame(const mavlink_message_t &msg) const { -#if HAL_CANMANAGER_ENABLED AP::can().handle_can_frame(msg); -#endif } +#endif // HAL_CANMANAGER_ENABLED void GCS_MAVLINK::handle_distance_sensor(const mavlink_message_t &msg) { +#if AP_RANGEFINDER_ENABLED RangeFinder *rangefinder = AP::rangefinder(); if (rangefinder != nullptr) { rangefinder->handle_msg(msg); } +#endif #if HAL_PROXIMITY_ENABLED AP_Proximity *proximity = AP::proximity(); @@ -3793,45 +3804,43 @@ void GCS_MAVLINK::handle_distance_sensor(const mavlink_message_t &msg) #endif } +#if HAL_PROXIMITY_ENABLED void GCS_MAVLINK::handle_obstacle_distance(const mavlink_message_t &msg) { -#if HAL_PROXIMITY_ENABLED AP_Proximity *proximity = AP::proximity(); if (proximity != nullptr) { proximity->handle_msg(msg); } -#endif } void GCS_MAVLINK::handle_obstacle_distance_3d(const mavlink_message_t &msg) { -#if HAL_PROXIMITY_ENABLED AP_Proximity *proximity = AP::proximity(); if (proximity != nullptr) { proximity->handle_msg(msg); } -#endif } +#endif +#if HAL_ADSB_ENABLED void GCS_MAVLINK::handle_adsb_message(const mavlink_message_t &msg) { -#if HAL_ADSB_ENABLED AP_ADSB *adsb = AP::ADSB(); if (adsb != nullptr) { adsb->handle_message(chan, msg); } -#endif } +#endif +#if OSD_PARAM_ENABLED void GCS_MAVLINK::handle_osd_param_config(const mavlink_message_t &msg) const { -#if OSD_PARAM_ENABLED AP_OSD *osd = AP::osd(); if (osd != nullptr) { osd->handle_msg(msg, *this); } -#endif } +#endif void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg) const { @@ -3981,12 +3990,14 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) break; #endif +#if AP_GPS_ENABLED case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: case MAVLINK_MSG_ID_HIL_GPS: case MAVLINK_MSG_ID_GPS_INJECT_DATA: AP::gps().handle_msg(msg); break; +#endif case MAVLINK_MSG_ID_STATUSTEXT: handle_statustext(msg); @@ -4078,19 +4089,24 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_distance_sensor(msg); break; +#if HAL_PROXIMITY_ENABLED case MAVLINK_MSG_ID_OBSTACLE_DISTANCE: handle_obstacle_distance(msg); break; - + case MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D: handle_obstacle_distance_3d(msg); break; +#endif +#if OSD_PARAM_ENABLED case MAVLINK_MSG_ID_OSD_PARAM_CONFIG: case MAVLINK_MSG_ID_OSD_PARAM_SHOW_CONFIG: handle_osd_param_config(msg); break; +#endif +#if HAL_ADSB_ENABLED case MAVLINK_MSG_ID_ADSB_VEHICLE: case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: @@ -4098,6 +4114,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CONTROL: handle_adsb_message(msg); break; +#endif case MAVLINK_MSG_ID_LANDING_TARGET: handle_landing_target(msg); @@ -4107,10 +4124,12 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_named_value(msg); break; +#if HAL_CANMANAGER_ENABLED case MAVLINK_MSG_ID_CAN_FRAME: case MAVLINK_MSG_ID_CANFD_FRAME: handle_can_frame(msg); break; +#endif case MAVLINK_MSG_ID_CAN_FILTER_MODIFY: #if HAL_CANMANAGER_ENABLED @@ -4291,9 +4310,10 @@ void GCS_MAVLINK::send_sim_state() const } #endif -MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_long_t &packet) +#if AP_BOOTLOADER_FLASHING_ENABLED +MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_int_t &packet) { - if (uint32_t(packet.param5) != 290876) { + if (packet.x != 290876) { gcs().send_text(MAV_SEVERITY_INFO, "Magic not set"); return MAV_RESULT_FAILED; } @@ -4314,6 +4334,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_lo return MAV_RESULT_FAILED; } +#endif // AP_BOOTLOADER_FLASHING_ENABLED MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg) { @@ -4703,10 +4724,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t switch (packet.command) { - case MAV_CMD_START_RX_PAIR: - result = handle_rc_bind(packet); - break; - #if AP_CAMERA_ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: @@ -4767,10 +4784,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t result = handle_command_run_prearm_checks(packet); break; - case MAV_CMD_FLASH_BOOTLOADER: - result = handle_command_flash_bootloader(packet); - break; - default: result = try_command_long_as_command_int(packet, msg); break; @@ -5063,12 +5076,17 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_DO_SET_MODE: return handle_command_do_set_mode(packet); + case MAV_CMD_DO_SET_ROI_NONE: { + const Location zero_loc = Location(); + return handle_command_do_set_roi(zero_loc); + } + case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_ROI_LOCATION: return handle_command_do_set_roi(packet); + #if HAL_MOUNT_ENABLED case MAV_CMD_DO_SET_ROI_SYSID: - case MAV_CMD_DO_SET_ROI_NONE: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: @@ -5098,6 +5116,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_mag_cal(packet); #endif +#if AP_BOOTLOADER_FLASHING_ENABLED + case MAV_CMD_FLASH_BOOTLOADER: + return handle_command_flash_bootloader(packet); +#endif + #if AP_AHRS_ENABLED case MAV_CMD_GET_HOME_POSITION: return handle_command_get_home_position(packet); @@ -5139,6 +5162,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_SET_EKF_SOURCE_SET: return handle_command_set_ekf_source_set(packet); + case MAV_CMD_START_RX_PAIR: + return handle_START_RX_PAIR(packet); + #if AP_FILESYSTEM_FORMAT_ENABLED case MAV_CMD_STORAGE_FORMAT: return handle_command_storage_format(packet, msg); @@ -5464,7 +5490,7 @@ void GCS_MAVLINK::send_generator_status() const } #endif -#if APM_BUILD_TYPE(APM_BUILD_Rover) +#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) void GCS_MAVLINK::send_water_depth() const { if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) { @@ -5513,7 +5539,7 @@ void GCS_MAVLINK::send_water_depth() const } } -#endif +#endif // AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) #if HAL_ADSB_ENABLED void GCS_MAVLINK::send_uavionix_adsb_out_status() const @@ -5706,10 +5732,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif +#if AP_RANGEFINDER_ENABLED case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); send_rangefinder(); break; +#endif case MSG_DISTANCE_SENSOR: send_distance_sensor(); @@ -5752,6 +5780,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(); break; +#if AP_GPS_ENABLED case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); AP::gps().send_mavlink_gps_raw(chan); @@ -5760,18 +5789,19 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(GPS_RTK); AP::gps().send_mavlink_gps_rtk(chan, 0); break; - case MSG_GPS2_RAW: #if GPS_MAX_RECEIVERS > 1 + case MSG_GPS2_RAW: CHECK_PAYLOAD_SIZE(GPS2_RAW); AP::gps().send_mavlink_gps2_raw(chan); -#endif break; - case MSG_GPS2_RTK: +#endif #if GPS_MAX_RECEIVERS > 1 + case MSG_GPS2_RTK: CHECK_PAYLOAD_SIZE(GPS2_RTK); AP::gps().send_mavlink_gps_rtk(chan, 1); -#endif break; +#endif +#endif // AP_GPS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); send_local_position(); @@ -5974,7 +6004,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; case MSG_WATER_DEPTH: -#if APM_BUILD_TYPE(APM_BUILD_Rover) +#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) CHECK_PAYLOAD_SIZE(WATER_DEPTH); send_water_depth(); #endif @@ -5987,16 +6017,15 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #endif // HAL_HIGH_LATENCY2_ENABLED break; - case MSG_AIS_VESSEL: { #if AP_AIS_ENABLED + case MSG_AIS_VESSEL: { AP_AIS *ais = AP_AIS::get_singleton(); if (ais) { ais->send(chan); } -#endif break; } - +#endif case MSG_UAVIONIX_ADSB_OUT_STATUS: #if HAL_ADSB_ENABLED @@ -6005,8 +6034,8 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #endif break; - case MSG_RELAY_STATUS: #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED + case MSG_RELAY_STATUS: ret = send_relay_status(); break; #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 827cb7e3e518b..7af63f16c465e 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -744,6 +744,14 @@ const AP_Param::GroupInfo SIM::var_mag[] = { AP_GROUPINFO("MAG3_SCALING", 30, SIM, mag_scaling[2], 1), AP_GROUPINFO("MAG3_ORIENT", 36, SIM, mag_orient[2], 0), #endif + + // @Param: MAG_SAVE_IDS + // @DisplayName: Save MAG devids on startup + // @Description: This forces saving of compass devids on startup so that simulated compasses start as calibrated + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO("MAG_SAVE_IDS", 37, SIM, mag_save_ids, 1), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 7e8f137d9dc04..4209fe06d1475 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -178,6 +178,7 @@ class SIM { AP_Vector3f mag_offdiag[HAL_COMPASS_MAX_SENSORS]; // off-diagonal corrections AP_Int8 mag_orient[HAL_COMPASS_MAX_SENSORS]; // external compass orientation AP_Int8 mag_fail[HAL_COMPASS_MAX_SENSORS]; // fail magnetometer, 1 for no data, 2 for freeze + AP_Int8 mag_save_ids; AP_Float servo_speed; // servo speed in seconds AP_Float sonar_glitch;// probability between 0-1 that any given sonar sample will read as max distance