Skip to content

Commit

Permalink
Merge branch 'master' into ducle/patch-airspeed-calibration
Browse files Browse the repository at this point in the history
  • Loading branch information
mduclehcm authored Feb 7, 2025
2 parents 01b61c4 + 2257009 commit 54147bc
Show file tree
Hide file tree
Showing 66 changed files with 5,549 additions and 5,393 deletions.
2 changes: 2 additions & 0 deletions .github/workflows/cygwin_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -74,6 +75,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/qurt_build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -74,6 +75,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_replay.yml
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -81,6 +82,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_blimp.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -88,6 +89,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_copter.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -87,6 +88,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_periph.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/bootloaders/**'
- 'Tools/CHDK-Script/**'
Expand Down Expand Up @@ -86,6 +87,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/bootloaders/**'
- 'Tools/CHDK-Script/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_plane.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -87,6 +88,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_rover.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -87,6 +88,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_sub.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -88,6 +89,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_sitl_tracker.yml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down Expand Up @@ -88,6 +89,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# remove non SITL directories
- 'Tools/AP_Bootloader/**'
- 'Tools/AP_Periph/**'
Expand Down
2 changes: 2 additions & 0 deletions .github/workflows/test_unit_tests.yml
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# Remove change on other workflows
- '.github/workflows/test_environment.yml'
pull_request:
Expand Down Expand Up @@ -88,6 +89,7 @@ on:
# remove non SITL HAL
- 'libraries/AP_HAL_ChibiOS/**'
- 'libraries/AP_HAL_ESP32/**'
- 'libraries/AP_HAL_QURT/**'
# Remove change on other workflows
- '.github/workflows/test_environment.yml'
workflow_dispatch:
Expand Down
4 changes: 3 additions & 1 deletion ArduCopter/Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 0 additions & 9 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -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();
Expand Down
90 changes: 53 additions & 37 deletions ArduCopter/RC_Channel_Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
12 changes: 12 additions & 0 deletions ArduCopter/RC_Channel_Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions ArduCopter/config.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading

0 comments on commit 54147bc

Please sign in to comment.