diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 43c6d2bb759f4..03448c07b3f5f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -164,7 +164,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50, 24), #endif SCHED_TASK(auto_disarm_check, 10, 50, 27), - SCHED_TASK(auto_trim, 10, 75, 30), +#if AP_COPTER_AHRS_AUTO_TRIM_ENABLED + SCHED_TASK_CLASS(RC_Channels_Copter, &copter.g2.rc_channels, auto_trim_run, 10, 75, 30), +#endif #if AP_RANGEFINDER_ENABLED SCHED_TASK(read_rangefinder, 20, 100, 33), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index eaa65fb82024d..75461d9ccc217 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -496,10 +496,6 @@ class Copter : public AP_Vehicle { // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; - // Used to exit the roll and pitch auto trim function - uint8_t auto_trim_counter; - bool auto_trim_started = false; - // Camera #if AP_CAMERA_ENABLED AP_Camera camera{MASK_LOG_CAMERA}; @@ -997,11 +993,6 @@ class Copter : public AP_Vehicle { // takeoff_check.cpp void takeoff_check(); - // RC_Channel.cpp - void save_trim(); - void auto_trim(); - void auto_trim_cancel(); - // system.cpp void init_ardupilot() override; void startup_INS_ground(); diff --git a/ArduCopter/RC_Channel_Copter.cpp b/ArduCopter/RC_Channel_Copter.cpp index ee2fe1c82be8e..b09ae572e5187 100644 --- a/ArduCopter/RC_Channel_Copter.cpp +++ b/ArduCopter/RC_Channel_Copter.cpp @@ -213,7 +213,7 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) if ((ch_flag == AuxSwitchPos::HIGH) && (copter.flightmode->allows_save_trim()) && (copter.channel_throttle->get_control_in() == 0)) { - copter.save_trim(); + copter.g2.rc_channels.save_trim(); } break; @@ -631,6 +631,12 @@ bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) break; #endif +#if AP_COPTER_AHRS_AUTO_TRIM_ENABLED + case AUX_FUNC::AHRS_AUTO_TRIM: + copter.g2.rc_channels.do_aux_function_ahrs_auto_trim(ch_flag); + break; +#endif // AP_COPTER_AHRS_AUTO_TRIM_ENABLED + case AUX_FUNC::SIMPLE_HEADING_RESET: if (ch_flag == AuxSwitchPos::HIGH) { copter.init_simple_bearing(); @@ -706,68 +712,78 @@ void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos c } } +// note that this is a method on the RC_Channels object, not the +// individual channel // save_trim - adds roll and pitch trims from the radio to ahrs -void Copter::save_trim() +void RC_Channels_Copter::save_trim() { // save roll and pitch trim - float roll_trim = ToRad((float)channel_roll->get_control_in()*0.01f); - float pitch_trim = ToRad((float)channel_pitch->get_control_in()*0.01f); - ahrs.add_trim(roll_trim, pitch_trim); + float roll_trim = ToRad((float)get_roll_channel().get_control_in()*0.01f); + float pitch_trim = ToRad((float)get_pitch_channel().get_control_in()*0.01f); + AP::ahrs().add_trim(roll_trim, pitch_trim); LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); } +#if AP_COPTER_AHRS_AUTO_TRIM_ENABLED +// start/stop ahrs auto trim +void RC_Channels_Copter::do_aux_function_ahrs_auto_trim(const RC_Channel::AuxSwitchPos ch_flag) +{ + switch (ch_flag) { + case RC_Channel::AuxSwitchPos::HIGH: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim running"); + // flash the leds + AP_Notify::flags.save_trim = true; + auto_trim.running = true; + break; + case RC_Channel::AuxSwitchPos::MIDDLE: + break; + case RC_Channel::AuxSwitchPos::LOW: + if (auto_trim.running) { + AP_Notify::flags.save_trim = false; + save_trim(); + } + break; + } +} + // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions // meant to be called continuously while the pilot attempts to keep the copter level -void Copter::auto_trim_cancel() +void RC_Channels_Copter::auto_trim_cancel() { - auto_trim_counter = 0; + auto_trim.running = false; AP_Notify::flags.save_trim = false; gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled"); + // restore original trims } -void Copter::auto_trim() +void RC_Channels_Copter::auto_trim_run() { - if (auto_trim_counter > 0) { - if (copter.flightmode != &copter.mode_stabilize || - !copter.motors->armed()) { - auto_trim_cancel(); + if (!auto_trim.running) { return; } - // flash the leds - AP_Notify::flags.save_trim = true; - - if (!auto_trim_started) { - if (ap.land_complete) { - // haven't taken off yet - return; - } - auto_trim_started = true; + // only trim in certain modes: + if (copter.flightmode != &copter.mode_stabilize && + copter.flightmode != &copter.mode_althold) { + auto_trim_cancel(); + return; } - if (ap.land_complete) { - // landed again. + // must be started and stopped mid-air: + if (copter.ap.land_complete_maybe) { auto_trim_cancel(); return; } - auto_trim_counter--; - // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f); + float roll_trim_adjustment = ToRad((float)get_roll_channel().get_control_in() / 4000.0f); // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f); - - // add trim to ahrs object - // save to eeprom on last iteration - ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); + float pitch_trim_adjustment = ToRad((float)get_pitch_channel().get_control_in() / 4000.0f); - // on last iteration restore leds and accel gains to normal - if (auto_trim_counter == 0) { - AP_Notify::flags.save_trim = false; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved"); - } - } + // add trim to ahrs object, but do not save to permanent storage: + AP::ahrs().add_trim(roll_trim_adjustment, pitch_trim_adjustment, false); } + +#endif // AP_COPTER_AHRS_AUTO_TRIM_ENABLED diff --git a/ArduCopter/RC_Channel_Copter.h b/ArduCopter/RC_Channel_Copter.h index 07bf285ab59a6..5a4be076d5671 100644 --- a/ArduCopter/RC_Channel_Copter.h +++ b/ArduCopter/RC_Channel_Copter.h @@ -46,6 +46,18 @@ class RC_Channels_Copter : public RC_Channels // returns true if throttle arming checks should be run bool arming_check_throttle() const override; + // support for trimming AHRS using RC stick inputs, enabled via an + // aux function +#if AP_COPTER_AHRS_AUTO_TRIM_ENABLED + void do_aux_function_ahrs_auto_trim(const RC_Channel::AuxSwitchPos ch_flag); + struct { + bool running; + } auto_trim; + void auto_trim_run(); + void auto_trim_cancel(); +#endif // AP_COPTER_AHRS_AUTO_TRIM_ENABLED + void save_trim(); + protected: int8_t flight_mode_channel_number() const override; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 0da560ea09099..8e2bf0e8838f4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -578,6 +578,10 @@ # define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0 #endif +#ifndef AP_COPTER_AHRS_AUTO_TRIM_ENABLED +# define AP_COPTER_AHRS_AUTO_TRIM_ENABLED 1 +#endif + #ifndef CH_MODE_DEFAULT # define CH_MODE_DEFAULT 5 #endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 0b2193107ffca..552baa6faee9c 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -2,7 +2,6 @@ #define ARM_DELAY 20 // called at 10hz so 2 seconds #define DISARM_DELAY 20 // called at 10hz so 2 seconds -#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second static uint32_t auto_disarm_begin; @@ -39,7 +38,7 @@ void Copter::arm_motors_check() if (yaw_in > 4000) { // increase the arming counter to a maximum of 1 beyond the auto trim counter - if (arming_counter <= AUTO_TRIM_DELAY) { + if (arming_counter < ARM_DELAY) { arming_counter++; } @@ -51,15 +50,6 @@ void Copter::arm_motors_check() } } - // arm the motors and configure for flight - if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && flightmode->mode_number() == Mode::Number::STABILIZE) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); - auto_trim_counter = 250; - auto_trim_started = false; - // ensure auto-disarm doesn't trigger immediately - auto_disarm_begin = millis(); - } - // full left and rudder disarming is enabled } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { if (!flightmode->has_manual_throttle() && !ap.land_complete) { diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 596c81e547759..fb3499e532193 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -239,10 +239,6 @@ class Blimp : public AP_Vehicle // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; - // Used to exit the roll and pitch auto trim function - uint8_t auto_trim_counter; - bool auto_trim_started = false; - // last valid RC input time uint32_t last_radio_update_ms; @@ -412,11 +408,6 @@ class Blimp : public AP_Vehicle bool rangefinder_alt_ok(); bool rangefinder_up_ok(); - // RC_Channel.cpp - void save_trim(); - void auto_trim(); - void auto_trim_cancel(); - // system.cpp void init_ardupilot() override; void startup_INS_ground(); diff --git a/Blimp/RC_Channel_Blimp.cpp b/Blimp/RC_Channel_Blimp.cpp index 5db884f88a25e..20e68927d38cc 100644 --- a/Blimp/RC_Channel_Blimp.cpp +++ b/Blimp/RC_Channel_Blimp.cpp @@ -106,14 +106,6 @@ bool RC_Channel_Blimp::do_aux_function(const AuxFuncTrigger &trigger) switch (ch_option) { - case AUX_FUNC::SAVE_TRIM: - if ((ch_flag == AuxSwitchPos::HIGH) && - (blimp.control_mode <= Mode::Number::MANUAL) && - (blimp.channel_up->get_control_in() == 0)) { - blimp.save_trim(); - } - break; - case AUX_FUNC::LOITER: do_aux_function_change_mode(Mode::Number::LOITER, ch_flag); break; @@ -127,69 +119,3 @@ bool RC_Channel_Blimp::do_aux_function(const AuxFuncTrigger &trigger) } return true; } - -// save_trim - adds roll and pitch trims from the radio to ahrs -void Blimp::save_trim() -{ - // save roll and pitch trim - float roll_trim = ToRad((float)channel_right->get_control_in()*0.01f); - float pitch_trim = ToRad((float)channel_front->get_control_in()*0.01f); - ahrs.add_trim(roll_trim, pitch_trim); - LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); - gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); -} - -// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions -// meant to be called continuously while the pilot attempts to keep the blimp level -void Blimp::auto_trim_cancel() -{ - auto_trim_counter = 0; - AP_Notify::flags.save_trim = false; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled"); -} - -void Blimp::auto_trim() -{ - if (auto_trim_counter > 0) { - if (blimp.flightmode != &blimp.mode_manual || - !blimp.motors->armed()) { - auto_trim_cancel(); - return; - } - - // flash the leds - AP_Notify::flags.save_trim = true; - - if (!auto_trim_started) { - if (ap.land_complete) { - // haven't taken off yet - return; - } - auto_trim_started = true; - } - - if (ap.land_complete) { - // landed again. - auto_trim_cancel(); - return; - } - - auto_trim_counter--; - - // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f); - - // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f); - - // add trim to ahrs object - // save to eeprom on last iteration - ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); - - // on last iteration restore leds and accel gains to normal - if (auto_trim_counter == 0) { - AP_Notify::flags.save_trim = false; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved"); - } - } -} diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 90fb3308a925b..d2e6cfe7d2bd2 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -2,7 +2,6 @@ #define ARM_DELAY 20 // called at 10hz so 2 seconds #define DISARM_DELAY 20 // called at 10hz so 2 seconds -#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second // arm_motors_check - checks for pilot input to arm or disarm the blimp @@ -29,7 +28,7 @@ void Blimp::arm_motors_check() if (yaw_in > 900) { // increase the arming counter to a maximum of 1 beyond the auto trim counter - if (arming_counter <= AUTO_TRIM_DELAY) { + if (arming_counter < ARM_DELAY) { arming_counter++; } @@ -41,13 +40,6 @@ void Blimp::arm_motors_check() } } - // arm the motors and configure for flight - if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); - auto_trim_counter = 250; - auto_trim_started = false; - } - // full left and rudder disarming is enabled } else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { if (!flightmode->has_manual_throttle() && !ap.land_complete) { diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index c83fe967a3f50..dadc1ed9161ac 100755 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -300,6 +300,7 @@ AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_FlywooH743Pro 1181 AP_HW_CBU_StampH743_LC 1182 AP_HW_NarinH7 1183 +AP_HW_BrahmaF4 1184 AP_HW_JFB200 1200 diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index df78bc533fd11..388495c4f8283 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -287,7 +287,7 @@ def configure_env(self, cfg, env): '-Wno-gnu-variable-sized-type-not-at-end', '-Werror=implicit-fallthrough', '-cl-single-precision-constant', - '-Wno-vla-cxx-extension', + '-Wno-vla-extension', ] else: env.CFLAGS += [ @@ -420,7 +420,7 @@ def configure_env(self, cfg, env): '-Wno-gnu-designator', '-Wno-mismatched-tags', - '-Wno-vla-cxx-extension', + '-Wno-vla-extension', '-Wno-gnu-variable-sized-type-not-at-end', '-Werror=implicit-fallthrough', '-cl-single-precision-constant', diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 994b47a053fa1..677db76d2dd61 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -12542,6 +12542,8 @@ def CompassLearnCopyFromEKF(self): 'COMPASS_USE3': 0, }) self.assert_parameter_value("COMPASS_OFS_X", 20, epsilon=30) + # set the parameter so it gets reset at context pop time: + self.set_parameter("COMPASS_OFS_X", 20) new_compass_ofs_x = 200 self.set_parameters({ "SIM_MAG1_OFS_X": new_compass_ofs_x, @@ -12574,6 +12576,89 @@ def CompassLearnCopyFromEKF(self): self.reboot_sitl() self.assert_parameter_value("COMPASS_OFS_X", new_compass_ofs_x, epsilon=30) + def AHRSAutoTrim(self): + '''calibrate AHRS trim using RC input''' + self.progress("Making earth frame same as body frame") # because I'm lazy + self.takeoff(5, mode='GUIDED') + self.guided_achieve_heading(0) + self.do_land() + + self.set_parameters({ + 'RC9_OPTION': 182, + }) + + param_last_check_time = 0 + for mode in ['STABILIZE', 'ALT_HOLD']: + self.set_parameters({ + 'AHRS_TRIM_X': 0.1, + 'AHRS_TRIM_Y': -0.1, + }) + self.takeoff(mode=mode) + self.set_rc(9, 2000) + tstart = self.get_sim_time() + while True: + now = self.get_sim_time_cached() + if now - tstart > 30: + raise ValueError(f"Failed to reduce trims in {mode}!") + lpn = self.assert_receive_message('LOCAL_POSITION_NED') + delta = 40 + roll_input = 1500 + if lpn.vx > 0: + roll_input -= delta + elif lpn.vx < 0: + roll_input += delta + + pitch_input = 1500 + if lpn.vy > 0: + pitch_input += delta + elif lpn.vy < 0: + pitch_input -= delta + self.set_rc_from_map({ + 1: roll_input, + 2: pitch_input, + }, quiet=True) + + # check parameters once/second: + if now - param_last_check_time > 1: + param_last_check_time = now + trim_x = self.get_parameter('AHRS_TRIM_X', verbose=False) + trim_y = self.get_parameter('AHRS_TRIM_Y', verbose=False) + self.progress(f"trim_x={trim_x} trim_y={trim_y}") + if abs(trim_x) < 0.02 and abs(trim_y) < 0.02: + self.progress("Good AHRS trims") + if abs(lpn.vx) > 1 or abs(lpn.vy) > 1: + raise NotAchievedException("Velocity after trimming?!") + break + self.context_collect('STATUSTEXT') + self.set_rc(9, 1000) + self.wait_statustext('Trim saved', check_context=True) + self.context_stop_collecting('STATUSTEXT') + self.do_land() + self.set_rc_default() + + self.progress("Landing should cancel the autotrim") + self.takeoff(10, mode='STABILIZE') + self.context_collect('STATUSTEXT') + self.set_rc(9, 2000) + self.wait_statustext('AutoTrim running', check_context=True) + self.do_land() + self.wait_statustext('AutoTrim cancelled', check_context=True) + self.set_rc(9, 1000) + + self.progress("Changing mode to LOITER") + self.takeoff(10, mode='STABILIZE') + self.context_collect('STATUSTEXT') + self.set_rc(9, 2000) + self.wait_statustext('AutoTrim running', check_context=True) + self.change_mode('LOITER') + self.wait_statustext('AutoTrim cancelled', check_context=True) + self.do_land() + self.set_rc(9, 1000) + + def do_land(self): + self.change_mode('LAND') + self.wait_disarmed() + def tests2b(self): # this block currently around 9.5mins here '''return list of all tests''' ret = ([ @@ -12685,6 +12770,7 @@ def tests2b(self): # this block currently around 9.5mins here self.TestTetherStuck, self.ScriptingFlipMode, self.CompassLearnCopyFromEKF, + self.AHRSAutoTrim, ]) return ret diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index fef6519a7dd82..c4be3f75075e8 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -2306,6 +2306,47 @@ def GCSFence(self): self.test_gcs_fence_via_mavproxy(target_system=target_system, target_component=target_component) + def GCSFenceInvalidPoint(self): + '''Test fetch non-existant fencepoint''' + target_system = 1 + target_component = 1 + + here = self.mav.location() + self.upload_fences_from_locations([ + (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, [ + # east + self.offset_location_ne(here, -50, 20), # bl + self.offset_location_ne(here, 50, 20), # br + self.offset_location_ne(here, 50, 40), # tr + ]), + ]) + for i in 0, 1, 2: + self.assert_fetch_mission_item_int(target_system, target_component, i, mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.progress("Requesting invalid point") + self.mav.mav.mission_request_int_send( + target_system, + target_component, + 3, + mavutil.mavlink.MAV_MISSION_TYPE_FENCE + ) + self.context_push() + self.context_collect('MISSION_COUNT') + m = self.assert_receive_mission_ack( + mavutil.mavlink.MAV_MISSION_TYPE_FENCE, + want_type=mavutil.mavlink.MAV_MISSION_INVALID_SEQUENCE, + ) + self.delay_sim_time(0.1) + found = False + for m in self.context_collection('MISSION_COUNT'): + if m.mission_type != mavutil.mavlink.MAV_MISSION_TYPE_FENCE: + continue + if m.count != 3: + raise NotAchievedException("Unexpected count in MISSION_COUNT") + found = True + self.context_pop() + if not found: + raise NotAchievedException("Did not see correction for fencepoint count") + # explode the write_type_to_storage method # FIXME: test converting invalid fences / minimally valid fences / normal fences # FIXME: show that uploading smaller items take up less space @@ -2533,6 +2574,7 @@ def assert_receive_mission_ack(self, mission_type, if m.type != want_type: raise NotAchievedException("Expected ack type %u got %u" % (want_type, m.type)) + return m def assert_filepath_content(self, filepath, want): with open(filepath) as f: @@ -7015,6 +7057,7 @@ def tests(self): self.Offboard, self.MAVProxyParam, self.GCSFence, + self.GCSFenceInvalidPoint, self.GCSMission, self.GCSRally, self.MotorTest, diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 5331d39c81bf5..0df65882cc8b6 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -290,6 +290,7 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): feature_define_whitelist.add(r'AP_MOTORS_FRAME_.*_ENABLED') feature_define_whitelist.add('AP_COPTER_ADVANCED_FAILSAFE_ENABLED') feature_define_whitelist.add('AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED') + feature_define_whitelist.add('AP_COPTER_AHRS_AUTO_TRIM_ENABLED') if target.lower() != "plane": # only on Plane: diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index daef87434f966..562f9ab6d848c 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5612,7 +5612,7 @@ def rc_defaults(self): 16: 1500, } - def set_rc_from_map(self, _map, timeout=20): + def set_rc_from_map(self, _map, timeout=20, quiet=False): map_copy = _map.copy() for v in map_copy.values(): if not isinstance(v, int): @@ -5639,7 +5639,8 @@ def set_rc_from_map(self, _map, timeout=20): bad_channels += " (ch=%u want=%u got=%u)" % (chan, map_copy[chan], chan_pwm) break if len(bad_channels) == 0: - self.progress("RC values good") + if not quiet: + self.progress("RC values good") break self.progress("RC values bad:%s" % bad_channels) if not self.rc_thread.is_alive(): @@ -9311,6 +9312,19 @@ def upload_using_mission_protocol(self, mission_type, items): (mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name),) self.progress("Upload of all %u items succeeded" % len(items)) + def assert_fetch_mission_item_int(self, target_system, target_component, seq, mission_type): + self.mav.mav.mission_request_int_send(target_system, + target_component, + seq, + mission_type) + m = self.assert_receive_message( + 'MISSION_ITEM_INT', + condition=f'MISSION_ITEM_INT.mission_type=={mission_type}', + ) + if m is None: + raise NotAchievedException("Did not receive MISSION_ITEM_INT") + return m + def download_using_mission_protocol(self, mission_type, verbose=False, timeout=10): '''mavlink2 required''' target_system = 1 @@ -9362,16 +9376,7 @@ def download_using_mission_protocol(self, mission_type, verbose=False, timeout=1 return items self.progress("Requesting item %u (remaining=%u)" % (next_to_request, len(remaining_to_receive))) - self.mav.mav.mission_request_int_send(target_system, - target_component, - next_to_request, - mission_type) - m = self.mav.recv_match(type='MISSION_ITEM_INT', - blocking=True, - timeout=5, - condition='MISSION_ITEM_INT.mission_type==%u' % mission_type) - if m is None: - raise NotAchievedException("Did not receive MISSION_ITEM_INT") + m = self.assert_fetch_mission_item_int(target_system, target_component, next_to_request, mission_type) if m.target_system != self.mav.source_system: raise NotAchievedException("Wrong target system (want=%u got=%u)" % (self.mav.source_system, m.target_system)) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index d6d85d58f0a89..664d65dc70a6d 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -158,6 +158,7 @@ def config_option(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('Copter', 'COPTER_ADVANCED_FAILSAFE', 'AP_COPTER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 + Feature('Copter', 'COPTER_AHRS_AUTO_TRIM', 'AP_COPTER_AHRS_AUTO_TRIM_ENABLED', 'Enable Copter in-flight AHRS Trim calibration', 0, None), # noqa Feature('Rover', 'ROVER_ADVANCED_FAILSAFE', 'AP_ROVER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 76be68dd361ef..6d2b79aeef747 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -278,6 +278,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_ROVER_ADVANCED_FAILSAFE_ENABLED', r'Rover::afs_fs_check'), ('AP_COPTER_ADVANCED_FAILSAFE_ENABLED', r'Copter::afs_fs_check'), + ('AP_COPTER_AHRS_AUTO_TRIM_ENABLED', r'RC_Channels_Copter::auto_trim_run'), ('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'), ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'), diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index aa443483e82b2..73cc963645ce5 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -20,7 +20,8 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_VIEWPRO_DATALEN_MAX (AP_MOUNT_VIEWPRO_PACKETLEN_MAX-AP_MOUNT_VIEWPRO_PACKETLEN_MIN) // max bytes for data portion of packet #define AP_MOUNT_VIEWPRO_HEALTH_TIMEOUT_MS 1000 // state will become unhealthy if no attitude is received within this timeout #define AP_MOUNT_VIEWPRO_UPDATE_INTERVAL_MS 100 // resend angle or rate targets to gimbal at this interval -#define AP_MOUNT_VIEWPRO_ZOOM_SPEED 0x07 // hard-coded zoom speed (fast) +#define AP_MOUNT_VIEWPRO_EO_ZOOM_SPEED 0x07 // hard-coded zoom speed (fast) +#define AP_MOUNT_VIEWPRO_IR_ZOOM_SPEED 1 #define AP_MOUNT_VIEWPRO_ZOOM_MAX 10 // hard-coded absolute zoom times max #define AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT (65536.0 / 360.0) // scalar to convert degrees to the viewpro angle scaling #define AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG (360.0 / 65536.0) // scalar to convert viewpro angle scaling to degrees @@ -349,9 +350,19 @@ void AP_Mount_Viewpro::process_packet() GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_text_prefix, _recording ? "ON" : "OFF"); } - // get optical zoom times - _zoom_times = UINT16_VALUE(_msg_buff[_msg_buff_data_start+39], _msg_buff[_msg_buff_data_start+40]) * 0.1; - + switch (_image_sensor) { + default: + case ImageSensor::EO1: + case ImageSensor::EO1_IR_PIP: + //optical zoom times + _zoom_times = UINT16_VALUE(_msg_buff[_msg_buff_data_start+39], _msg_buff[_msg_buff_data_start+40]) * 0.1; + break; + case ImageSensor::IR: + case ImageSensor::IR_EO1_PIP: + //ir zoom times + _zoom_times = ((_msg_buff[_msg_buff_data_start+29] >> 3) & 0x0F) + 1; + break; + } // get laser rangefinder distance _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+33], _msg_buff[_msg_buff_data_start+34]) * 0.1; break; @@ -737,13 +748,32 @@ bool AP_Mount_Viewpro::set_zoom(ZoomType zoom_type, float zoom_value) // zoom rate if (zoom_type == ZoomType::RATE) { + uint8_t zoom_speed = 1; CameraCommand zoom_cmd = CameraCommand::STOP_FOCUS_AND_ZOOM; - if (zoom_value < 0) { - zoom_cmd = CameraCommand::ZOOM_OUT; - } else if (zoom_value > 0) { - zoom_cmd = CameraCommand::ZOOM_IN; + + switch (_image_sensor) { + default: + case ImageSensor::EO1: + case ImageSensor::EO1_IR_PIP: + if (zoom_value < 0) { + zoom_cmd = CameraCommand::ZOOM_OUT; + } else if (zoom_value > 0) { + zoom_cmd = CameraCommand::ZOOM_IN; + } + zoom_speed = AP_MOUNT_VIEWPRO_EO_ZOOM_SPEED; + break; + case ImageSensor::IR: + case ImageSensor::IR_EO1_PIP: + if (zoom_value < 0) { + zoom_cmd = CameraCommand::IR_ZOOM_IN; + } else if (zoom_value > 0) { + zoom_cmd = CameraCommand::IR_ZOOM_OUT; + } + zoom_speed = AP_MOUNT_VIEWPRO_IR_ZOOM_SPEED; + break; } - return send_camera_command(_image_sensor, zoom_cmd, AP_MOUNT_VIEWPRO_ZOOM_SPEED); + + return send_camera_command(_image_sensor, zoom_cmd, zoom_speed); } // zoom percentage diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 1100586cd1ce1..214394a32416a 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -160,7 +160,9 @@ class AP_Mount_Viewpro : public AP_Mount_Backend_Serial START_RECORD = 0x14, STOP_RECORD = 0x15, AUTO_FOCUS = 0x19, - MANUAL_FOCUS = 0x1A + MANUAL_FOCUS = 0x1A, + IR_ZOOM_OUT = 0x1B, + IR_ZOOM_IN = 0x1C }; // C1 rangefinder commands diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index c7af789559e34..9e92377e0bb9c 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -155,9 +155,19 @@ void MissionItemProtocol::handle_mission_request_int(GCS_MAVLINK &_link, } mavlink_mission_item_int_t ret_packet; - const MAV_MISSION_RESULT result_code = get_item(_link, msg, packet, ret_packet); - + const MAV_MISSION_RESULT result_code = get_item(packet.seq, ret_packet); if (result_code != MAV_MISSION_ACCEPTED) { + if (result_code == MAV_MISSION_INVALID_SEQUENCE) { + // try to educate the GCS on the actual size of the mission: + const mavlink_channel_t chan = _link.get_chan(); + if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) { + mavlink_msg_mission_count_send(chan, + msg.sysid, + msg.compid, + item_count(), + mission_type()); + } + } // send failure message send_mission_ack(_link, msg, result_code); return; @@ -179,15 +189,8 @@ void MissionItemProtocol::handle_mission_request(GCS_MAVLINK &_link, return; } - // convert into a MISSION_REQUEST_INT and reuse its handling code - mavlink_mission_request_int_t request_int; - request_int.target_system = packet.target_system; - request_int.target_component = packet.target_component; - request_int.seq = packet.seq; - request_int.mission_type = packet.mission_type; - mavlink_mission_item_int_t item_int; - MAV_MISSION_RESULT ret = get_item(_link, msg, request_int, item_int); + MAV_MISSION_RESULT ret = get_item(packet.seq, item_int); if (ret != MAV_MISSION_ACCEPTED) { send_mission_ack(_link, msg, ret); return; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h index 37f37fd559716..ff8a6ae73adbf 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -102,9 +102,7 @@ class MissionItemProtocol bool mission_item_warning_sent = false; // support for GCS getting waypoints etc from us: - virtual MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, + virtual MAV_MISSION_RESULT get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) = 0; void init_send_requests(GCS_MAVLINK &_link, diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp index c50001cc12dd7..df945641c4d21 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -74,17 +74,13 @@ bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq, return true; } -MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) +MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) { - const auto num_stored_items = _fence.polyfence().num_stored_items(); - if (packet.seq > num_stored_items) { + if (seq >= _fence.polyfence().num_stored_items()) { return MAV_MISSION_INVALID_SEQUENCE; } - if (!get_item_as_mission_item(packet.seq, ret_packet)) { + if (!get_item_as_mission_item(seq, ret_packet)) { return MAV_MISSION_ERROR; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h index 58a984f60a821..e724891a6622e 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h @@ -42,10 +42,7 @@ class MissionItemProtocol_Fence : public MissionItemProtocol { MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; - MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; + MAV_MISSION_RESULT get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; void free_upload_resources() override; MAV_MISSION_RESULT allocate_receive_resources(const uint16_t count) override WARN_IF_UNUSED; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp index 0af495d7ff81a..ad330039c33f9 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -150,12 +150,13 @@ bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq, return true; } -MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) +MAV_MISSION_RESULT MissionItemProtocol_Rally::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) { - if (!get_item_as_mission_item(packet.seq, ret_packet)) { + if (seq >= item_count()) { + return MAV_MISSION_INVALID_SEQUENCE; + } + + if (!get_item_as_mission_item(seq, ret_packet)) { return MAV_MISSION_INVALID_SEQUENCE; } return MAV_MISSION_ACCEPTED; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h index add6efa6ca53e..b2aaf1ff03af2 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h @@ -39,10 +39,7 @@ class MissionItemProtocol_Rally : public MissionItemProtocol { MAV_MISSION_RESULT replace_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t&) override WARN_IF_UNUSED; - MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; + MAV_MISSION_RESULT get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; }; diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp index 7ce5787d46459..a1987c457cd12 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -64,29 +64,17 @@ MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_l return MAV_MISSION_ACCEPTED; } -MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) +MAV_MISSION_RESULT MissionItemProtocol_Waypoints::get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) { - if (packet.seq != 0 && // always allow HOME to be read - packet.seq >= mission.num_commands()) { - // try to educate the GCS on the actual size of the mission: - const mavlink_channel_t chan = _link.get_chan(); - if (HAVE_PAYLOAD_SPACE(chan, MISSION_COUNT)) { - mavlink_msg_mission_count_send(chan, - msg.sysid, - msg.compid, - mission.num_commands(), - MAV_MISSION_TYPE_MISSION); - } - return MAV_MISSION_ERROR; + if (seq != 0 && // always allow HOME to be read + seq >= mission.num_commands()) { + return MAV_MISSION_INVALID_SEQUENCE; } AP_Mission::Mission_Command cmd; // retrieve mission from eeprom - if (!mission.read_cmd_from_storage(packet.seq, cmd)) { + if (!mission.read_cmd_from_storage(seq, cmd)) { return MAV_MISSION_ERROR; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h index 5a7ee4e86ea36..b5592b776447d 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.h @@ -45,14 +45,8 @@ class MissionItemProtocol_Waypoints : public MissionItemProtocol { // item to the end of the list of stored items. MAV_MISSION_RESULT append_item(const mavlink_mission_item_int_t &) override WARN_IF_UNUSED; - // get_item() fills in ret_packet based on packet; _link is the - // link the request was received on, and msg is the undecoded - // request. Note that msg may not actually decode to a - // request_int_t! - MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, - const mavlink_message_t &msg, - const mavlink_mission_request_int_t &packet, - mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; + // support for GCS getting waypoints etc from us: + MAV_MISSION_RESULT get_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; // item_count() returns the number of stored items uint16_t item_count() const override; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 6bd477cdaf185..0e1fe150c7b69 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -261,7 +261,7 @@ class RC_Channel { ICE_START_STOP = 179, // AP_ICEngine start stop AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains QUICKTUNE = 181, //quicktune 3 position switch - + AHRS_AUTO_TRIM = 182, // in-flight trim Copter AHRS using manual levelling // inputs from 200 will eventually used to replace RCMAP ROLL = 201, // roll input