2019-12-18 18:35:32 -04:00
|
|
|
#include "AP_Vehicle.h"
|
|
|
|
|
2020-06-09 17:14:25 -03:00
|
|
|
#include <AP_BLHeli/AP_BLHeli.h>
|
2020-01-16 06:40:52 -04:00
|
|
|
#include <AP_Common/AP_FWVersion.h>
|
2020-07-15 23:52:11 -03:00
|
|
|
#include <AP_Arming/AP_Arming.h>
|
2020-09-10 09:54:10 -03:00
|
|
|
#include <AP_Frsky_Telem/AP_Frsky_Parameters.h>
|
2022-03-03 23:29:48 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2020-12-05 14:46:16 -04:00
|
|
|
#include <AP_Mission/AP_Mission.h>
|
|
|
|
#include <AP_OSD/AP_OSD.h>
|
2022-04-15 04:40:24 -03:00
|
|
|
#include <AP_RPM/AP_RPM.h>
|
2022-03-03 23:29:48 -04:00
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2022-07-17 21:51:11 -03:00
|
|
|
#include <AP_Motors/AP_Motors.h>
|
2022-08-11 05:02:03 -03:00
|
|
|
#include <AP_CheckFirmware/AP_CheckFirmware.h>
|
2021-04-14 06:22:27 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
|
|
#include <AP_HAL_ChibiOS/sdcard.h>
|
2022-10-04 04:12:20 -03:00
|
|
|
#include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
|
2021-04-14 06:22:27 -03:00
|
|
|
#endif
|
2020-01-16 06:40:52 -04:00
|
|
|
|
2021-11-13 23:18:10 -04:00
|
|
|
#define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio)
|
2019-12-18 18:35:32 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
2nd group of parameters
|
|
|
|
*/
|
|
|
|
const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
|
|
|
|
#if HAL_RUNCAM_ENABLED
|
|
|
|
// @Group: CAM_RC_
|
|
|
|
// @Path: ../AP_Camera/AP_RunCam.cpp
|
|
|
|
AP_SUBGROUPINFO(runcam, "CAM_RC_", 1, AP_Vehicle, AP_RunCam),
|
|
|
|
#endif
|
|
|
|
|
2019-12-30 14:15:48 -04:00
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
// @Group: FFT_
|
|
|
|
// @Path: ../AP_GyroFFT/AP_GyroFFT.cpp
|
|
|
|
AP_SUBGROUPINFO(gyro_fft, "FFT_", 2, AP_Vehicle, AP_GyroFFT),
|
|
|
|
#endif
|
2020-04-06 01:12:11 -03:00
|
|
|
|
|
|
|
#if HAL_VISUALODOM_ENABLED
|
|
|
|
// @Group: VISO
|
|
|
|
// @Path: ../AP_VisualOdom/AP_VisualOdom.cpp
|
|
|
|
AP_SUBGROUPINFO(visual_odom, "VISO", 3, AP_Vehicle, AP_VisualOdom),
|
|
|
|
#endif
|
2020-05-28 17:50:07 -03:00
|
|
|
// @Group: VTX_
|
2021-02-17 15:58:21 -04:00
|
|
|
// @Path: ../AP_VideoTX/AP_VideoTX.cpp
|
2020-05-28 17:50:07 -03:00
|
|
|
AP_SUBGROUPINFO(vtx, "VTX_", 4, AP_Vehicle, AP_VideoTX),
|
|
|
|
|
2020-08-04 17:39:40 -03:00
|
|
|
#if HAL_MSP_ENABLED
|
|
|
|
// @Group: MSP
|
|
|
|
// @Path: ../AP_MSP/AP_MSP.cpp
|
|
|
|
AP_SUBGROUPINFO(msp, "MSP", 5, AP_Vehicle, AP_MSP),
|
|
|
|
#endif
|
|
|
|
|
2020-09-10 09:54:10 -03:00
|
|
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
|
|
// @Group: FRSKY_
|
|
|
|
// @Path: ../AP_Frsky_Telem/AP_Frsky_Parameters.cpp
|
|
|
|
AP_SUBGROUPINFO(frsky_parameters, "FRSKY_", 6, AP_Vehicle, AP_Frsky_Parameters),
|
|
|
|
#endif
|
|
|
|
|
2021-09-24 00:10:47 -03:00
|
|
|
#if HAL_GENERATOR_ENABLED
|
2020-10-22 14:17:26 -03:00
|
|
|
// @Group: GEN_
|
|
|
|
// @Path: ../AP_Generator/AP_Generator.cpp
|
|
|
|
AP_SUBGROUPINFO(generator, "GEN_", 7, AP_Vehicle, AP_Generator),
|
|
|
|
#endif
|
|
|
|
|
2020-12-27 22:06:14 -04:00
|
|
|
#if HAL_EXTERNAL_AHRS_ENABLED
|
|
|
|
// @Group: EAHRS
|
|
|
|
// @Path: ../AP_ExternalAHRS/AP_ExternalAHRS.cpp
|
|
|
|
AP_SUBGROUPINFO(externalAHRS, "EAHRS", 8, AP_Vehicle, AP_ExternalAHRS),
|
|
|
|
#endif
|
2021-02-01 12:26:35 -04:00
|
|
|
|
2021-10-21 03:37:35 -03:00
|
|
|
#if HAL_EFI_ENABLED
|
|
|
|
// @Group: EFI
|
|
|
|
// @Path: ../AP_EFI/AP_EFI.cpp
|
|
|
|
AP_SUBGROUPINFO(efi, "EFI", 9, AP_Vehicle, AP_EFI),
|
|
|
|
#endif
|
|
|
|
|
2022-01-03 17:17:01 -04:00
|
|
|
#if AP_AIRSPEED_ENABLED
|
|
|
|
// @Group: ARSPD
|
|
|
|
// @Path: ../AP_Airspeed/AP_Airspeed.cpp
|
|
|
|
AP_SUBGROUPINFO(airspeed, "ARSPD", 10, AP_Vehicle, AP_Airspeed),
|
|
|
|
#endif
|
|
|
|
|
2021-11-05 13:13:03 -03:00
|
|
|
// @Group: CUST_ROT
|
|
|
|
// @Path: ../AP_CustomRotations/AP_CustomRotations.cpp
|
|
|
|
AP_SUBGROUPINFO(custom_rotations, "CUST_ROT", 11, AP_Vehicle, AP_CustomRotations),
|
|
|
|
|
2022-06-06 01:33:54 -03:00
|
|
|
#if HAL_WITH_ESC_TELEM
|
|
|
|
// @Group: ESC_TLM
|
|
|
|
// @Path: ../AP_ESC_Telem/AP_ESC_Telem.cpp
|
|
|
|
AP_SUBGROUPINFO(esc_telem, "ESC_TLM", 12, AP_Vehicle, AP_ESC_Telem),
|
|
|
|
#endif
|
|
|
|
|
2021-08-14 18:29:21 -03:00
|
|
|
#if AP_AIS_ENABLED
|
|
|
|
// @Group: AIS_
|
|
|
|
// @Path: ../AP_AIS/AP_AIS.cpp
|
|
|
|
AP_SUBGROUPINFO(ais, "AIS_", 13, AP_Vehicle, AP_AIS),
|
|
|
|
#endif
|
|
|
|
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2022-03-04 12:41:48 -04:00
|
|
|
// @Group: FENCE_
|
|
|
|
// @Path: ../AC_Fence/AC_Fence.cpp
|
|
|
|
AP_SUBGROUPINFO(fence, "FENCE_", 14, AP_Vehicle, AC_Fence),
|
|
|
|
#endif
|
|
|
|
|
2022-07-28 05:42:22 -03:00
|
|
|
#if AP_OPENDRONEID_ENABLED
|
|
|
|
// @Group: DID_
|
|
|
|
// @Path: ../AP_OpenDroneID/AP_OpenDroneID.cpp
|
|
|
|
AP_SUBGROUPINFO(opendroneid, "DID_", 15, AP_Vehicle, AP_OpenDroneID),
|
|
|
|
#endif
|
2019-12-18 18:35:32 -04:00
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
|
|
|
// reference to the vehicle. using AP::vehicle() here does not work on clang
|
2021-06-20 03:18:34 -03:00
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) || APM_BUILD_TYPE(APM_BUILD_AP_Periph)
|
2019-12-18 18:35:32 -04:00
|
|
|
AP_Vehicle& vehicle = *AP_Vehicle::get_singleton();
|
|
|
|
#else
|
|
|
|
extern AP_Vehicle& vehicle;
|
|
|
|
#endif
|
|
|
|
|
2020-01-16 06:31:20 -04:00
|
|
|
/*
|
|
|
|
setup is called when the sketch starts
|
|
|
|
*/
|
|
|
|
void AP_Vehicle::setup()
|
|
|
|
{
|
|
|
|
// load the default values of variables listed in var_info[]
|
|
|
|
AP_Param::setup_sketch_defaults();
|
|
|
|
|
|
|
|
// initialise serial port
|
|
|
|
serial_manager.init_console();
|
|
|
|
|
2022-03-21 06:41:06 -03:00
|
|
|
DEV_PRINTF("\n\nInit %s"
|
2020-01-16 06:31:20 -04:00
|
|
|
"\n\nFree RAM: %u\n",
|
|
|
|
AP::fwversion().fw_string,
|
|
|
|
(unsigned)hal.util->available_memory());
|
|
|
|
|
2022-08-11 05:02:03 -03:00
|
|
|
#if AP_CHECK_FIRMWARE_ENABLED
|
|
|
|
check_firmware_print();
|
|
|
|
#endif
|
|
|
|
|
2020-01-16 06:31:20 -04:00
|
|
|
load_parameters();
|
|
|
|
|
2021-04-14 06:22:27 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
|
|
if (AP_BoardConfig::get_sdcard_slowdown() != 0) {
|
|
|
|
// user wants the SDcard slower, we need to remount
|
|
|
|
sdcard_stop();
|
|
|
|
sdcard_retry();
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2020-01-16 06:31:20 -04:00
|
|
|
// initialise the main loop scheduler
|
|
|
|
const AP_Scheduler::Task *tasks;
|
|
|
|
uint8_t task_count;
|
|
|
|
uint32_t log_bit;
|
|
|
|
get_scheduler_tasks(tasks, task_count, log_bit);
|
|
|
|
AP::scheduler().init(tasks, task_count, log_bit);
|
2020-01-28 19:06:37 -04:00
|
|
|
|
|
|
|
// time per loop - this gets updated in the main loop() based on
|
|
|
|
// actual loop rate
|
|
|
|
G_Dt = scheduler.get_loop_period_s();
|
|
|
|
|
2020-02-11 21:01:17 -04:00
|
|
|
// this is here for Plane; its failsafe_check method requires the
|
|
|
|
// RC channels to be set as early as possible for maximum
|
|
|
|
// survivability.
|
|
|
|
set_control_channels();
|
|
|
|
|
|
|
|
// initialise serial manager as early as sensible to get
|
|
|
|
// diagnostic output during boot process. We have to initialise
|
|
|
|
// the GCS singleton first as it sets the global mavlink system ID
|
|
|
|
// which may get used very early on.
|
|
|
|
gcs().init();
|
|
|
|
|
|
|
|
// initialise serial ports
|
|
|
|
serial_manager.init();
|
|
|
|
gcs().setup_console();
|
|
|
|
|
|
|
|
// Register scheduler_delay_cb, which will run anytime you have
|
|
|
|
// more than 5ms remaining in your call to hal.scheduler->delay
|
|
|
|
hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
|
|
|
|
|
2020-09-05 01:18:07 -03:00
|
|
|
#if HAL_MSP_ENABLED
|
|
|
|
// call MSP init before init_ardupilot to allow for MSP sensors
|
|
|
|
msp.init();
|
|
|
|
#endif
|
|
|
|
|
2020-12-27 22:06:14 -04:00
|
|
|
#if HAL_EXTERNAL_AHRS_ENABLED
|
|
|
|
// call externalAHRS init before init_ardupilot to allow for external sensors
|
|
|
|
externalAHRS.init();
|
|
|
|
#endif
|
2021-02-01 12:26:35 -04:00
|
|
|
|
2020-01-28 19:06:37 -04:00
|
|
|
// init_ardupilot is where the vehicle does most of its initialisation.
|
|
|
|
init_ardupilot();
|
2020-02-24 22:42:33 -04:00
|
|
|
|
2022-01-03 17:17:01 -04:00
|
|
|
#if AP_AIRSPEED_ENABLED
|
|
|
|
airspeed.init();
|
|
|
|
if (airspeed.enabled()) {
|
|
|
|
airspeed.calibrate(true);
|
|
|
|
}
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
|
|
|
else {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No airspeed sensor present or enabled");
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
#endif // AP_AIRSPEED_ENABLED
|
|
|
|
|
2021-04-14 18:51:20 -03:00
|
|
|
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
|
2021-03-02 18:18:28 -04:00
|
|
|
SRV_Channels::init();
|
2021-04-14 18:51:20 -03:00
|
|
|
#endif
|
|
|
|
|
2019-12-30 14:15:48 -04:00
|
|
|
// gyro FFT needs to be initialized really late
|
|
|
|
#if HAL_GYROFFT_ENABLED
|
2021-06-07 03:13:14 -03:00
|
|
|
gyro_fft.init(AP::scheduler().get_loop_rate_hz());
|
2019-12-30 14:15:48 -04:00
|
|
|
#endif
|
2020-02-17 21:59:18 -04:00
|
|
|
#if HAL_RUNCAM_ENABLED
|
|
|
|
runcam.init();
|
|
|
|
#endif
|
|
|
|
#if HAL_HOTT_TELEM_ENABLED
|
|
|
|
hott_telem.init();
|
|
|
|
#endif
|
2020-04-06 01:12:11 -03:00
|
|
|
#if HAL_VISUALODOM_ENABLED
|
|
|
|
// init library used for visual position estimation
|
|
|
|
visual_odom.init();
|
|
|
|
#endif
|
2021-01-29 15:55:49 -04:00
|
|
|
|
2020-05-28 17:50:07 -03:00
|
|
|
vtx.init();
|
2020-02-24 22:42:33 -04:00
|
|
|
|
2021-01-29 15:55:49 -04:00
|
|
|
#if HAL_SMARTAUDIO_ENABLED
|
|
|
|
smartaudio.init();
|
|
|
|
#endif
|
|
|
|
|
2022-06-18 14:50:31 -03:00
|
|
|
#if AP_TRAMP_ENABLED
|
|
|
|
tramp.init();
|
|
|
|
#endif
|
|
|
|
|
2020-02-24 22:42:33 -04:00
|
|
|
#if AP_PARAM_KEY_DUMP
|
|
|
|
AP_Param::show_all(hal.console, true);
|
|
|
|
#endif
|
2020-11-05 19:35:35 -04:00
|
|
|
|
|
|
|
send_watchdog_reset_statustext();
|
2020-10-22 14:17:26 -03:00
|
|
|
|
2021-09-24 00:10:47 -03:00
|
|
|
#if HAL_GENERATOR_ENABLED
|
2020-10-22 14:17:26 -03:00
|
|
|
generator.init();
|
|
|
|
#endif
|
2021-10-21 03:37:35 -03:00
|
|
|
|
2022-07-28 05:42:22 -03:00
|
|
|
#if AP_OPENDRONEID_ENABLED
|
|
|
|
opendroneid.init();
|
|
|
|
#endif
|
|
|
|
|
2021-10-21 03:37:35 -03:00
|
|
|
// init EFI monitoring
|
|
|
|
#if HAL_EFI_ENABLED
|
|
|
|
efi.init();
|
|
|
|
#endif
|
|
|
|
|
2021-08-14 18:29:21 -03:00
|
|
|
#if AP_AIS_ENABLED
|
|
|
|
ais.init();
|
|
|
|
#endif
|
|
|
|
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2022-03-04 12:41:48 -04:00
|
|
|
fence.init();
|
|
|
|
#endif
|
|
|
|
|
2021-11-05 13:13:03 -03:00
|
|
|
custom_rotations.init();
|
|
|
|
|
2021-08-11 11:23:26 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "ArduPilot Ready");
|
2020-01-16 06:31:20 -04:00
|
|
|
}
|
|
|
|
|
2020-01-27 21:36:10 -04:00
|
|
|
void AP_Vehicle::loop()
|
|
|
|
{
|
|
|
|
scheduler.loop();
|
|
|
|
G_Dt = scheduler.get_loop_period_s();
|
2020-11-30 23:52:29 -04:00
|
|
|
|
|
|
|
if (!done_safety_init) {
|
|
|
|
/*
|
|
|
|
disable safety if requested. This is delayed till after the
|
|
|
|
first loop has run to ensure that all servos have received
|
|
|
|
an update for their initial values. Otherwise we may end up
|
|
|
|
briefly driving a servo to a position out of the configured
|
|
|
|
range which could damage hardware
|
|
|
|
*/
|
|
|
|
done_safety_init = true;
|
|
|
|
BoardConfig.init_safety();
|
2021-08-11 11:23:26 -03:00
|
|
|
|
|
|
|
// send RC output mode info if available
|
|
|
|
char banner_msg[50];
|
|
|
|
if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", banner_msg);
|
|
|
|
}
|
2020-11-30 23:52:29 -04:00
|
|
|
}
|
2021-08-28 13:18:07 -03:00
|
|
|
const uint32_t new_internal_errors = AP::internalerror().errors();
|
|
|
|
if(_last_internal_errors != new_internal_errors) {
|
|
|
|
AP::logger().Write_Error(LogErrorSubsystem::INTERNAL_ERROR, LogErrorCode::INTERNAL_ERRORS_DETECTED);
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Internal Errors %x", (unsigned)new_internal_errors);
|
|
|
|
_last_internal_errors = new_internal_errors;
|
|
|
|
}
|
2020-01-27 21:36:10 -04:00
|
|
|
}
|
|
|
|
|
2019-12-18 18:35:32 -04:00
|
|
|
/*
|
2021-11-13 23:18:10 -04:00
|
|
|
scheduler table - all regular tasks apart from the fast_loop()
|
|
|
|
should be listed here.
|
|
|
|
|
|
|
|
All entries in this table must be ordered by priority.
|
|
|
|
|
|
|
|
This table is interleaved with the table presnet in each of the
|
|
|
|
vehicles to determine the order in which tasks are run. Convenience
|
|
|
|
methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
|
|
|
|
entries in this structure:
|
|
|
|
|
|
|
|
SCHED_TASK arguments:
|
|
|
|
- name of static function to call
|
|
|
|
- rate (in Hertz) at which the function should be called
|
|
|
|
- expected time (in MicroSeconds) that the function should take to run
|
|
|
|
- priority (0 through 255, lower number meaning higher priority)
|
|
|
|
|
|
|
|
SCHED_TASK_CLASS arguments:
|
|
|
|
- class name of method to be called
|
|
|
|
- instance on which to call the method
|
|
|
|
- method to call on that instance
|
|
|
|
- rate (in Hertz) at which the method should be called
|
|
|
|
- expected time (in MicroSeconds) that the method should take to run
|
|
|
|
- priority (0 through 255, lower number meaning higher priority)
|
|
|
|
|
2019-12-18 18:35:32 -04:00
|
|
|
*/
|
|
|
|
const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
|
2022-04-01 16:56:39 -03:00
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
FAST_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, sample_gyros),
|
|
|
|
#endif
|
2022-01-03 17:17:01 -04:00
|
|
|
#if AP_AIRSPEED_ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_Airspeed, &vehicle.airspeed, update, 10, 100, 41), // NOTE: the priority number here should be right before Plane's calc_airspeed_errors
|
|
|
|
#endif
|
2022-07-15 23:02:45 -03:00
|
|
|
#if COMPASS_CAL_ENABLED
|
|
|
|
SCHED_TASK_CLASS(Compass, &vehicle.compass, cal_update, 100, 200, 75),
|
|
|
|
#endif
|
2019-12-18 18:35:32 -04:00
|
|
|
#if HAL_RUNCAM_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_RunCam, &vehicle.runcam, update, 50, 50, 200),
|
2019-12-30 14:15:48 -04:00
|
|
|
#endif
|
|
|
|
#if HAL_GYROFFT_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205),
|
|
|
|
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210),
|
2019-12-18 18:35:32 -04:00
|
|
|
#endif
|
2021-11-14 04:41:30 -04:00
|
|
|
SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215),
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100, 220),
|
2022-06-18 14:50:31 -03:00
|
|
|
#if AP_TRAMP_ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_Tramp, &vehicle.tramp, update, 50, 50, 225),
|
|
|
|
#endif
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20, 225),
|
2021-02-23 18:06:37 -04:00
|
|
|
#if HAL_WITH_ESC_TELEM
|
2022-05-05 21:15:39 -03:00
|
|
|
SCHED_TASK_CLASS(AP_ESC_Telem, &vehicle.esc_telem, update, 100, 50, 230),
|
2021-02-23 18:06:37 -04:00
|
|
|
#endif
|
2021-09-24 00:10:47 -03:00
|
|
|
#if HAL_GENERATOR_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK_CLASS(AP_Generator, &vehicle.generator, update, 10, 50, 235),
|
2020-10-22 14:17:26 -03:00
|
|
|
#endif
|
2022-07-28 05:42:22 -03:00
|
|
|
#if AP_OPENDRONEID_ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_OpenDroneID, &vehicle.opendroneid, update, 10, 50, 236),
|
|
|
|
#endif
|
2020-12-05 14:46:16 -04:00
|
|
|
#if OSD_ENABLED
|
2021-11-13 23:18:10 -04:00
|
|
|
SCHED_TASK(publish_osd_info, 1, 10, 240),
|
2020-12-05 14:46:16 -04:00
|
|
|
#endif
|
2022-03-18 04:02:30 -03:00
|
|
|
#if HAL_INS_ACCELCAL_ENABLED
|
2022-07-15 23:02:45 -03:00
|
|
|
SCHED_TASK(accel_cal_update, 10, 100, 245),
|
2022-03-18 04:02:30 -03:00
|
|
|
#endif
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2022-03-04 12:41:48 -04:00
|
|
|
SCHED_TASK_CLASS(AC_Fence, &vehicle.fence, update, 10, 100, 248),
|
|
|
|
#endif
|
2021-08-14 18:29:21 -03:00
|
|
|
#if AP_AIS_ENABLED
|
|
|
|
SCHED_TASK_CLASS(AP_AIS, &vehicle.ais, update, 5, 100, 249),
|
|
|
|
#endif
|
2021-10-21 03:37:35 -03:00
|
|
|
#if HAL_EFI_ENABLED
|
2022-08-28 18:19:26 -03:00
|
|
|
SCHED_TASK_CLASS(AP_EFI, &vehicle.efi, update, 50, 200, 250),
|
2022-10-04 04:12:20 -03:00
|
|
|
#endif
|
|
|
|
#if HAL_INS_ACCELCAL_ENABLED
|
|
|
|
SCHED_TASK(one_Hz_update, 1, 100, 252),
|
2021-10-21 03:37:35 -03:00
|
|
|
#endif
|
2022-05-10 04:53:32 -03:00
|
|
|
SCHED_TASK(update_arming, 1, 50, 253),
|
2019-12-18 18:35:32 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
|
|
|
|
{
|
|
|
|
tasks = scheduler_tasks;
|
|
|
|
num_tasks = ARRAY_SIZE(scheduler_tasks);
|
|
|
|
}
|
|
|
|
|
2020-01-15 23:04:41 -04:00
|
|
|
/*
|
|
|
|
* a delay() callback that processes MAVLink packets. We set this as the
|
|
|
|
* callback in long running library initialisation routines to allow
|
|
|
|
* MAVLink to process packets while waiting for the initialisation to
|
|
|
|
* complete
|
|
|
|
*/
|
|
|
|
void AP_Vehicle::scheduler_delay_callback()
|
|
|
|
{
|
2020-03-11 05:47:44 -03:00
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
|
|
// compass.init() delays, so we end up here.
|
|
|
|
return;
|
|
|
|
#endif
|
|
|
|
|
2020-01-15 23:04:41 -04:00
|
|
|
static uint32_t last_1hz, last_50hz, last_5s;
|
|
|
|
|
|
|
|
AP_Logger &logger = AP::logger();
|
|
|
|
|
|
|
|
// don't allow potentially expensive logging calls:
|
|
|
|
logger.EnableWrites(false);
|
|
|
|
|
|
|
|
const uint32_t tnow = AP_HAL::millis();
|
|
|
|
if (tnow - last_1hz > 1000) {
|
|
|
|
last_1hz = tnow;
|
|
|
|
gcs().send_message(MSG_HEARTBEAT);
|
|
|
|
gcs().send_message(MSG_SYS_STATUS);
|
|
|
|
}
|
|
|
|
if (tnow - last_50hz > 20) {
|
|
|
|
last_50hz = tnow;
|
|
|
|
gcs().update_receive();
|
|
|
|
gcs().update_send();
|
|
|
|
_singleton->notify.update();
|
|
|
|
}
|
|
|
|
if (tnow - last_5s > 5000) {
|
|
|
|
last_5s = tnow;
|
2020-10-28 14:47:37 -03:00
|
|
|
if (AP_BoardConfig::in_config_error()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "Config Error: fix problem then reboot");
|
|
|
|
} else {
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Initialising ArduPilot");
|
|
|
|
}
|
2020-01-15 23:04:41 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
logger.EnableWrites(true);
|
|
|
|
}
|
|
|
|
|
2020-03-25 08:07:50 -03:00
|
|
|
// if there's been a watchdog reset, notify the world via a statustext:
|
|
|
|
void AP_Vehicle::send_watchdog_reset_statustext()
|
|
|
|
{
|
|
|
|
if (!hal.util->was_watchdog_reset()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
const AP_HAL::Util::PersistentData &pd = hal.util->last_persistent_data;
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL,
|
2020-04-28 03:33:45 -03:00
|
|
|
"WDG: T%d SL%u FL%u FT%u FA%x FTP%u FLR%x FICSR%u MM%u MC%u IE%u IEC%u TN:%.4s",
|
2020-03-25 08:07:50 -03:00
|
|
|
pd.scheduler_task,
|
|
|
|
pd.semaphore_line,
|
|
|
|
pd.fault_line,
|
|
|
|
pd.fault_type,
|
|
|
|
(unsigned)pd.fault_addr,
|
|
|
|
pd.fault_thd_prio,
|
|
|
|
(unsigned)pd.fault_lr,
|
|
|
|
(unsigned)pd.fault_icsr,
|
|
|
|
pd.last_mavlink_msgid,
|
|
|
|
pd.last_mavlink_cmd,
|
|
|
|
(unsigned)pd.internal_errors,
|
2020-04-28 03:33:45 -03:00
|
|
|
(unsigned)pd.internal_error_count,
|
|
|
|
pd.thread_name4
|
2020-03-25 08:07:50 -03:00
|
|
|
);
|
|
|
|
}
|
2020-07-15 23:52:11 -03:00
|
|
|
|
|
|
|
bool AP_Vehicle::is_crashed() const
|
|
|
|
{
|
|
|
|
if (AP::arming().is_armed()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return AP::arming().last_disarm_method() == AP_Arming::Method::CRASH;
|
|
|
|
}
|
2020-03-25 08:07:50 -03:00
|
|
|
|
2022-06-10 17:42:38 -03:00
|
|
|
// update the harmonic notch filter for throttle based notch
|
|
|
|
void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch)
|
|
|
|
{
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
|
|
|
|
const float ref_freq = notch.params.center_freq_hz();
|
|
|
|
const float ref = notch.params.reference();
|
|
|
|
const float min_ratio = notch.params.freq_min_ratio();
|
|
|
|
|
|
|
|
const AP_Motors* motors = AP::motors();
|
|
|
|
const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0;
|
|
|
|
float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref));
|
|
|
|
|
|
|
|
notch.update_freq_hz(throttle_freq);
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2022-04-15 04:40:24 -03:00
|
|
|
// update the harmonic notch filter center frequency dynamically
|
|
|
|
void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch)
|
|
|
|
{
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
|
|
|
|
if (!notch.params.enabled()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
const float ref_freq = notch.params.center_freq_hz();
|
|
|
|
const float ref = notch.params.reference();
|
|
|
|
if (is_zero(ref)) {
|
|
|
|
notch.update_freq_hz(ref_freq);
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2022-09-19 12:08:47 -03:00
|
|
|
const AP_Motors* motors = AP::motors();
|
2022-10-05 21:17:07 -03:00
|
|
|
if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
|
2022-09-19 12:08:47 -03:00
|
|
|
notch.set_inactive(true);
|
|
|
|
} else {
|
|
|
|
notch.set_inactive(false);
|
|
|
|
}
|
|
|
|
|
2022-04-15 04:40:24 -03:00
|
|
|
switch (notch.params.tracking_mode()) {
|
|
|
|
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
|
|
|
|
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
|
2022-06-10 17:42:38 -03:00
|
|
|
update_throttle_notch(notch);
|
2022-04-15 04:40:24 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
|
|
|
|
case HarmonicNotchDynamicMode::UpdateRPM2: {
|
|
|
|
const auto *rpm_sensor = AP::rpm();
|
|
|
|
uint8_t sensor = (notch.params.tracking_mode()==HarmonicNotchDynamicMode::UpdateRPM?0:1);
|
|
|
|
float rpm;
|
|
|
|
if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) {
|
|
|
|
// set the harmonic notch filter frequency from the main rotor rpm
|
|
|
|
notch.update_freq_hz(MAX(ref_freq, rpm * ref * (1.0/60)));
|
|
|
|
} else {
|
|
|
|
notch.update_freq_hz(ref_freq);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
#if HAL_WITH_ESC_TELEM
|
|
|
|
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
|
|
|
|
// set the harmonic notch filter frequency scaled on measured frequency
|
|
|
|
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
|
|
|
|
float notches[INS_MAX_NOTCHES];
|
2022-08-06 17:30:26 -03:00
|
|
|
// ESC telemetry will return 0 for missing data, but only after 1s
|
2022-08-18 04:20:58 -03:00
|
|
|
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches);
|
2022-08-20 13:50:33 -03:00
|
|
|
for (uint8_t i = 0; i < num_notches; i++) {
|
2022-08-06 17:30:26 -03:00
|
|
|
if (!is_zero(notches[i])) {
|
|
|
|
notches[i] = MAX(ref_freq, notches[i]);
|
|
|
|
}
|
2022-04-15 04:40:24 -03:00
|
|
|
}
|
|
|
|
if (num_notches > 0) {
|
2022-08-20 13:50:33 -03:00
|
|
|
notch.update_frequencies_hz(num_notches, notches);
|
2022-04-15 04:40:24 -03:00
|
|
|
} else { // throttle fallback
|
2022-06-10 17:42:38 -03:00
|
|
|
update_throttle_notch(notch);
|
2022-04-15 04:40:24 -03:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
#endif
|
|
|
|
#if HAL_GYROFFT_ENABLED
|
|
|
|
case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
|
|
|
|
// set the harmonic notch filter frequency scaled on measured frequency
|
|
|
|
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
|
|
|
|
float notches[INS_MAX_NOTCHES];
|
|
|
|
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches);
|
|
|
|
|
|
|
|
notch.update_frequencies_hz(peaks, notches);
|
|
|
|
} else {
|
|
|
|
notch.update_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz());
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
#endif
|
|
|
|
case HarmonicNotchDynamicMode::Fixed: // static
|
|
|
|
default:
|
|
|
|
notch.update_freq_hz(ref_freq);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
#endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2021-03-08 17:05:24 -04:00
|
|
|
// run notch update at either loop rate or 200Hz
|
|
|
|
void AP_Vehicle::update_dynamic_notch_at_specified_rate()
|
|
|
|
{
|
2022-04-15 04:40:24 -03:00
|
|
|
for (auto ¬ch : ins.harmonic_notches) {
|
|
|
|
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::LoopRateUpdate)) {
|
|
|
|
update_dynamic_notch(notch);
|
2022-04-13 00:36:18 -03:00
|
|
|
} else {
|
|
|
|
// decimated update at 200Hz
|
|
|
|
const uint32_t now = AP_HAL::millis();
|
2022-04-15 04:40:24 -03:00
|
|
|
const uint8_t i = ¬ch - &ins.harmonic_notches[0];
|
2022-04-13 00:36:18 -03:00
|
|
|
if (now - _last_notch_update_ms[i] > 5) {
|
|
|
|
_last_notch_update_ms[i] = now;
|
2022-04-15 04:40:24 -03:00
|
|
|
update_dynamic_notch(notch);
|
2022-04-13 00:36:18 -03:00
|
|
|
}
|
|
|
|
}
|
2021-03-08 17:05:24 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-08-24 23:04:50 -03:00
|
|
|
void AP_Vehicle::notify_no_such_mode(uint8_t mode_number)
|
|
|
|
{
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No such mode %u", mode_number);
|
|
|
|
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode_number));
|
|
|
|
}
|
|
|
|
|
2020-09-27 22:21:52 -03:00
|
|
|
// reboot the vehicle in an orderly manner, doing various cleanups and
|
|
|
|
// flashing LEDs as appropriate
|
|
|
|
void AP_Vehicle::reboot(bool hold_in_bootloader)
|
|
|
|
{
|
|
|
|
if (should_zero_rc_outputs_on_reboot()) {
|
|
|
|
SRV_Channels::zero_rc_outputs();
|
|
|
|
}
|
|
|
|
|
|
|
|
// Notify might want to blink some LEDs:
|
|
|
|
AP_Notify::flags.firmware_update = 1;
|
|
|
|
notify.update();
|
|
|
|
|
|
|
|
// force safety on
|
|
|
|
hal.rcout->force_safety_on();
|
|
|
|
|
|
|
|
// flush pending parameter writes
|
|
|
|
AP_Param::flush();
|
|
|
|
|
|
|
|
// do not process incoming mavlink messages while we delay:
|
|
|
|
hal.scheduler->register_delay_callback(nullptr, 5);
|
|
|
|
|
2021-02-19 09:46:38 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
// need to ensure the ack goes out:
|
|
|
|
hal.serial(0)->flush();
|
|
|
|
#endif
|
|
|
|
|
2020-09-27 22:21:52 -03:00
|
|
|
// delay to give the ACK a chance to get out, the LEDs to flash,
|
|
|
|
// the IO board safety to be forced on, the parameters to flush, ...
|
|
|
|
hal.scheduler->delay(200);
|
|
|
|
|
|
|
|
hal.scheduler->reboot(hold_in_bootloader);
|
|
|
|
}
|
|
|
|
|
2020-12-05 14:46:16 -04:00
|
|
|
#if OSD_ENABLED
|
|
|
|
void AP_Vehicle::publish_osd_info()
|
|
|
|
{
|
|
|
|
AP_Mission *mission = AP::mission();
|
|
|
|
if (mission == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
AP_OSD *osd = AP::osd();
|
|
|
|
if (osd == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
AP_OSD::NavInfo nav_info;
|
|
|
|
if(!get_wp_distance_m(nav_info.wp_distance)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
float wp_bearing_deg;
|
|
|
|
if (!get_wp_bearing_deg(wp_bearing_deg)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
nav_info.wp_bearing = (int32_t)wp_bearing_deg * 100; // OSD expects cd
|
|
|
|
if (!get_wp_crosstrack_error_m(nav_info.wp_xtrack_error)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
nav_info.wp_number = mission->get_current_nav_index();
|
|
|
|
osd->set_nav_info(nav_info);
|
|
|
|
}
|
2022-09-23 09:27:26 -03:00
|
|
|
#endif
|
2021-07-28 20:29:37 -03:00
|
|
|
|
|
|
|
void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
|
|
|
|
{
|
|
|
|
roll = ahrs.roll;
|
|
|
|
pitch = ahrs.pitch;
|
|
|
|
}
|
|
|
|
|
2022-03-18 04:02:30 -03:00
|
|
|
#if HAL_INS_ACCELCAL_ENABLED
|
|
|
|
|
2021-08-16 23:51:35 -03:00
|
|
|
#ifndef HAL_CAL_ALWAYS_REBOOT
|
|
|
|
// allow for forced reboot after accelcal
|
|
|
|
#define HAL_CAL_ALWAYS_REBOOT 0
|
|
|
|
#endif
|
|
|
|
|
|
|
|
/*
|
|
|
|
update accel cal
|
|
|
|
*/
|
|
|
|
void AP_Vehicle::accel_cal_update()
|
|
|
|
{
|
|
|
|
if (hal.util->get_soft_armed()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
ins.acal_update();
|
|
|
|
// check if new trim values, and set them
|
|
|
|
Vector3f trim_rad;
|
|
|
|
if (ins.get_new_trim(trim_rad)) {
|
|
|
|
ahrs.set_trim(trim_rad);
|
|
|
|
}
|
|
|
|
|
|
|
|
#if HAL_CAL_ALWAYS_REBOOT
|
|
|
|
if (ins.accel_cal_requires_reboot() &&
|
|
|
|
!hal.util->get_soft_armed()) {
|
|
|
|
hal.scheduler->delay(1000);
|
|
|
|
hal.scheduler->reboot(false);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
2022-03-18 04:02:30 -03:00
|
|
|
#endif // HAL_INS_ACCELCAL_ENABLED
|
2021-08-16 23:51:35 -03:00
|
|
|
|
2022-05-10 04:53:32 -03:00
|
|
|
// call the arming library's update function
|
|
|
|
void AP_Vehicle::update_arming()
|
|
|
|
{
|
|
|
|
AP::arming().update();
|
|
|
|
}
|
|
|
|
|
2022-10-04 04:12:20 -03:00
|
|
|
/*
|
|
|
|
one Hz checks common to all vehicles
|
|
|
|
*/
|
|
|
|
void AP_Vehicle::one_Hz_update(void)
|
|
|
|
{
|
|
|
|
one_Hz_counter++;
|
|
|
|
|
|
|
|
/*
|
|
|
|
every 10s check if using a 2M firmware on a 1M board
|
|
|
|
*/
|
|
|
|
if (one_Hz_counter % 10U == 0) {
|
|
|
|
#if defined(BOARD_CHECK_F427_USE_1M) && (BOARD_FLASH_SIZE>1024)
|
|
|
|
if (!hal.util->get_soft_armed() && check_limit_flash_1M()) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, BOARD_CHECK_F427_USE_1M);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
every 30s check if using a 1M firmware on a 2M board
|
|
|
|
*/
|
|
|
|
if (one_Hz_counter % 30U == 0) {
|
|
|
|
#if defined(BOARD_CHECK_F427_USE_1M) && (BOARD_FLASH_SIZE<=1024)
|
|
|
|
if (!hal.util->get_soft_armed() && !check_limit_flash_1M()) {
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, BOARD_CHECK_F427_USE_2M);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-10-08 19:36:46 -03:00
|
|
|
AP_Vehicle *AP_Vehicle::_singleton = nullptr;
|
|
|
|
|
|
|
|
AP_Vehicle *AP_Vehicle::get_singleton()
|
|
|
|
{
|
|
|
|
return _singleton;
|
|
|
|
}
|
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
|
|
|
|
AP_Vehicle *vehicle()
|
|
|
|
{
|
|
|
|
return AP_Vehicle::get_singleton();
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
2022-04-15 04:40:24 -03:00
|
|
|
|