ardupilot/Rover/system.cpp

331 lines
8.9 KiB
C++
Raw Normal View History

/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
We will determine later if we are actually on the ground and process a
ground start in that case.
*****************************************************************************/
2015-05-13 00:16:45 -03:00
#include "Rover.h"
static void failsafe_check_static()
{
rover.failsafe_check();
}
void Rover::init_ardupilot()
{
2018-02-28 08:23:09 -04:00
#if STATS_ENABLED == ENABLED
// initialise stats module
g2.stats.init();
2018-02-28 08:23:09 -04:00
#endif
BoardConfig.init();
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
can_mgr.init();
#endif
2019-02-07 19:45:23 -04:00
// init gripper
2022-09-20 04:37:48 -03:00
#if AP_GRIPPER_ENABLED
2019-02-07 19:45:23 -04:00
g2.gripper.init();
#endif
// initialise notify system
2018-07-25 22:11:10 -03:00
notify.init();
notify_mode(control_mode);
battery.init();
#if AP_RPM_ENABLED
2018-12-30 16:43:39 -04:00
// Initialise RPM sensor
rpm_sensor.init();
#endif
2018-12-30 16:43:39 -04:00
rssi.init();
g2.windvane.init(serial_manager);
2018-09-25 10:09:47 -03:00
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
2016-05-16 00:33:43 -03:00
// setup telem slots with serial ports
gcs().setup_uarts();
#if OSD_ENABLED == ENABLED
osd.init();
#endif
#if LOGGING_ENABLED == ENABLED
2015-05-13 00:16:45 -03:00
log_init();
#endif
// initialise compass
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();
2022-01-09 21:37:11 -04:00
#if AP_AIRSPEED_ENABLED
airspeed.set_log_bit(MASK_LOG_IMU);
#endif
2017-07-13 08:36:44 -03:00
// initialise rangefinder
2021-05-04 14:54:39 -03:00
rangefinder.set_log_rfnd_bit(MASK_LOG_RANGEFINDER);
2019-04-05 06:14:23 -03:00
rangefinder.init(ROTATION_NONE);
2021-03-25 05:47:29 -03:00
#if HAL_PROXIMITY_ENABLED
2017-08-16 07:57:42 -03:00
// init proximity sensor
g2.proximity.init();
2021-03-25 05:47:29 -03:00
#endif
2017-08-16 07:57:42 -03:00
#if AP_BEACON_ENABLED
// init beacons used for non-gps position estimation
g2.beacon.init();
#endif
// and baro for EKF
barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate();
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
gps.init(serial_manager);
2013-12-21 07:27:06 -04:00
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
2017-07-10 06:43:55 -03:00
init_rc_in(); // sets up rc channels deadzone
g2.motors.init(get_frame_type()); // init motors including setting servo out channels ranges
SRV_Channels::enable_aux_servos();
2017-07-11 23:02:51 -03:00
// init wheel encoders
g2.wheel_encoder.init();
2021-07-01 03:15:10 -03:00
#if HAL_TORQEEDO_ENABLED
// init torqeedo motor driver
g2.torqeedo.init();
#endif
2022-09-07 03:49:51 -03:00
#if AP_OPTICALFLOW_ENABLED
// initialise optical flow sensor
optflow.init(MASK_LOG_OPTFLOW);
#endif // AP_OPTICALFLOW_ENABLED
relay.init();
#if HAL_MOUNT_ENABLED
// initialise camera mount
camera_mount.init();
2015-01-28 00:56:36 -04:00
#endif
#if AP_CAMERA_ENABLED
// initialise camera
camera.init();
#endif
#if AC_PRECLAND_ENABLED
// initialise precision landing
init_precland();
#endif
/*
setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised.
*/
2015-05-12 04:00:25 -03:00
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// initialize SmartRTL
g2.smart_rtl.init();
2015-01-28 00:56:36 -04:00
2019-05-10 02:59:52 -03:00
// initialise object avoidance
g2.oa.init();
startup_ground();
Mode *initial_mode = mode_from_mode_num((enum Mode::Number)g.initial_mode.get());
2017-07-18 23:19:08 -03:00
if (initial_mode == nullptr) {
initial_mode = &mode_initializing;
}
set_mode(*initial_mode, ModeReason::INITIALISED);
2017-07-18 23:19:08 -03:00
// initialise rc channels
2021-09-13 21:06:54 -03:00
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
rc().convert_options(RC_Channel::AUX_FUNC::SAVE_TRIM, RC_Channel::AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC);
rc().init();
2019-05-25 15:42:18 -03:00
rover.g2.sailboat.init();
// boat should loiter after completing a mission to avoid drifting off
if (is_boat()) {
rover.g2.mis_done_behave.set_default(ModeAuto::Mis_Done_Behave::MIS_DONE_BEHAVE_LOITER);
}
// flag that initialisation has completed
initialised = true;
}
//*********************************************************************************
// This function does all the calibrations, etc. that we need during a ground start
//*********************************************************************************
void Rover::startup_ground(void)
{
set_mode(mode_initializing, ModeReason::INITIALISED);
// IMU ground start
//------------------------
//
startup_INS_ground();
// initialise mission library
mode_auto.mission.init();
// initialise AP_Logger library
#if LOGGING_ENABLED == ENABLED
logger.setVehicle_Startup_Writer(
FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
);
#endif
#if AP_SCRIPTING_ENABLED
g2.scripting.init();
#endif // AP_SCRIPTING_ENABLED
2019-03-01 02:40:40 -04:00
2015-01-28 00:56:36 -04:00
// we don't want writes to the serial port to cause us to pause
// so set serial ports non-blocking once we are ready to drive
serial_manager.set_blocking_writes_all(false);
}
// update the ahrs flyforward setting which can allow
// the vehicle's movements to be used to estimate heading
void Rover::update_ahrs_flyforward()
{
bool flyforward = false;
// boats never use movement to estimate heading
if (!is_boat()) {
// throttle threshold is 15% or 1/2 cruise throttle
bool throttle_over_thresh = g2.motors.get_throttle() > MIN(g.throttle_cruise * 0.50f, 15.0f);
// desired speed threshold of 1m/s
bool desired_speed_over_thresh = g2.attitude_control.speed_control_active() && (g2.attitude_control.get_desired_speed() > 0.5f);
if (throttle_over_thresh || (is_positive(g2.motors.get_throttle()) && desired_speed_over_thresh)) {
uint32_t now = AP_HAL::millis();
// if throttle over threshold start timer
if (flyforward_start_ms == 0) {
flyforward_start_ms = now;
}
// if throttle over threshold for 2 seconds set flyforward to true
flyforward = (now - flyforward_start_ms > 2000);
} else {
// reset timer
flyforward_start_ms = 0;
}
}
ahrs.set_fly_forward(flyforward);
}
bool Rover::set_mode(Mode &new_mode, ModeReason reason)
{
2017-07-18 23:19:08 -03:00
if (control_mode == &new_mode) {
// don't switch modes if we are already in the correct mode.
2017-07-18 23:19:08 -03:00
return true;
}
2017-07-18 23:19:08 -03:00
Mode &old_mode = *control_mode;
if (!new_mode.enter()) {
2017-07-24 14:05:59 -03:00
// Log error that we failed to enter desired flight mode
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE,
LogErrorCode(new_mode.mode_number()));
2017-07-24 14:05:59 -03:00
gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
2017-07-18 23:19:08 -03:00
return false;
}
2017-07-18 23:19:08 -03:00
control_mode = &new_mode;
#if AP_FENCE_ENABLED
2017-08-16 07:02:56 -03:00
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
2022-03-04 12:41:00 -04:00
fence.manual_recovery_start();
#endif
2017-08-16 07:02:56 -03:00
2022-06-02 05:28:26 -03:00
#if AP_CAMERA_ENABLED
camera.set_is_auto_mode(control_mode->mode_number() == Mode::Number::AUTO);
#endif
2017-07-18 23:19:08 -03:00
old_mode.exit();
control_mode_reason = reason;
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
2019-06-26 15:21:05 -03:00
gcs().send_message(MSG_HEARTBEAT);
notify_mode(control_mode);
2017-07-18 23:19:08 -03:00
return true;
}
bool Rover::set_mode(const uint8_t new_mode, ModeReason reason)
{
static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
Mode *mode = rover.mode_from_mode_num((enum Mode::Number)new_mode);
if (mode == nullptr) {
notify_no_such_mode(new_mode);
return false;
}
return rover.set_mode(*mode, reason);
}
void Rover::startup_INS_ground(void)
{
gcs().send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
2017-08-22 06:30:40 -03:00
hal.scheduler->delay(100);
2013-01-13 01:03:54 -04:00
ahrs.init();
// say to EKF that rover only move by going forward
ahrs.set_fly_forward(true);
ahrs.set_vehicle_class(AP_AHRS::VehicleClass::GROUND);
2013-10-20 19:09:57 -03:00
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
}
// update notify with mode change
void Rover::notify_mode(const Mode *mode)
{
AP_Notify::flags.autopilot_mode = mode->is_autopilot_mode();
notify.flags.flight_mode = mode->mode_number();
notify.set_flight_mode_str(mode->name4());
}
/*
check a digital pin for high,low (1/0)
*/
uint8_t Rover::check_digital_pin(uint8_t pin)
{
// ensure we are in input mode
hal.gpio->pinMode(pin, HAL_GPIO_INPUT);
// enable pullup
hal.gpio->write(pin, 1);
return hal.gpio->read(pin);
}
2014-01-14 00:10:13 -04:00
/*
should we log a message type now?
*/
bool Rover::should_log(uint32_t mask)
2014-01-14 00:10:13 -04:00
{
return logger.should_log(mask);
2014-01-14 00:10:13 -04:00
}
// returns true if vehicle is a boat
// this affects whether the vehicle tries to maintain position after reaching waypoints
bool Rover::is_boat() const
{
return ((enum frame_class)g2.frame_class.get() == FRAME_BOAT);
}
2019-11-03 20:41:40 -04:00
#include <AP_Avoidance/AP_Avoidance.h>
#include <AP_ADSB/AP_ADSB.h>
#if HAL_ADSB_ENABLED
2019-11-03 20:41:40 -04:00
// dummy method to avoid linking AP_Avoidance
AP_Avoidance *AP::ap_avoidance() { return nullptr; }
#endif