ardupilot/APMrover2/system.cpp

389 lines
9.3 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"
#include "version.h"
2015-05-13 00:16:45 -03:00
static void mavlink_delay_cb_static()
{
rover.mavlink_delay_cb();
}
static void failsafe_check_static()
{
rover.failsafe_check();
}
void Rover::init_ardupilot()
{
2015-01-28 00:56:36 -04:00
// initialise console serial port
serial_manager.init_console();
2017-08-11 00:49:03 -03:00
hal.console->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
hal.util->available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
load_parameters();
// initialise stats module
g2.stats.init();
2016-05-30 22:43:42 -03:00
gcs().set_dataflash(&DataFlash);
2014-01-19 21:57:59 -04:00
mavlink_system.sysid = g.sysid_this_mav;
2015-01-28 00:56:36 -04:00
// initialise serial ports
serial_manager.init();
// setup first port early to allow BoardConfig to report errors
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
BoardConfig.init();
#if HAL_WITH_UAVCAN
BoardConfig_CAN.init();
#endif
// initialise notify system
notify.init(false);
AP_Notify::flags.failsafe_battery = false;
2017-07-18 23:19:08 -03:00
notify_mode((enum mode)control_mode->mode_number());
ServoRelayEvents.set_channel_mask(0xFFF0);
battery.init();
rssi.init();
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
2013-09-20 20:21:02 -03:00
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
2013-09-20 20:21:02 -03:00
usb_connected = true;
check_usb_mux();
2016-05-16 00:33:43 -03:00
// setup telem slots with serial ports
gcs().setup_uarts(serial_manager);
2015-01-28 00:56:36 -04:00
// setup frsky telemetry
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.init(serial_manager, FIRMWARE_STRING, MAV_TYPE_GROUND_ROVER);
#endif
#if LOGGING_ENABLED == ENABLED
2015-05-13 00:16:45 -03:00
log_init();
#endif
// initialise compass
init_compass();
2017-07-13 08:36:44 -03:00
// initialise rangefinder
init_rangefinder();
// init beacons used for non-gps position estimation
init_beacon();
2017-06-01 04:39:49 -03:00
// init visual odometry
init_visual_odom();
// and baro for EKF
2016-05-23 22:31:27 -03:00
init_barometer(true);
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
gps.init(serial_manager);
2013-12-21 07:27:06 -04:00
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
2017-07-10 06:43:55 -03:00
set_control_channels(); // setup radio channels and ouputs ranges
init_rc_in(); // sets up rc channels deadzone
g2.motors.init(); // init motors including setting servo out channels ranges
init_rc_out(); // enable output
2017-07-11 23:02:51 -03:00
// init wheel encoders
g2.wheel_encoder.init();
relay.init();
2015-01-28 00:56:36 -04:00
#if MOUNT == ENABLED
// initialise camera mount
2016-01-05 01:23:11 -04:00
camera_mount.init(&DataFlash, serial_manager);
2015-01-28 00:56:36 -04:00
#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);
// give AHRS the range beacon sensor
ahrs.set_beacon(&g2.beacon);
2015-01-28 00:56:36 -04:00
init_capabilities();
2015-07-31 18:14:51 -03:00
startup_ground();
2017-07-18 23:19:08 -03:00
Mode *initial_mode = control_mode_from_num((enum mode)g.initial_mode.get());
if (initial_mode == nullptr) {
initial_mode = &mode_initializing;
}
2017-07-24 14:05:59 -03:00
set_mode(*initial_mode, MODE_REASON_INITIALISED);
2017-07-18 23:19:08 -03:00
// set the correct flight mode
// ---------------------------
reset_control_switch();
// disable safety if requested
BoardConfig.init_safety();
// 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)
{
2017-07-24 14:05:59 -03:00
set_mode(mode_initializing, MODE_REASON_INITIALISED);
gcs().send_text(MAV_SEVERITY_INFO, "<startup_ground> Ground start");
#if(GROUND_START_DELAY > 0)
gcs().send_text(MAV_SEVERITY_NOTICE, "<startup_ground> With delay");
delay(GROUND_START_DELAY * 1000);
#endif
// IMU ground start
//------------------------
//
startup_INS_ground();
// read the radio to set trims
// ---------------------------
trim_radio();
// initialise mission library
mission.init();
// initialise DataFlash library
DataFlash.set_mission(&mission);
DataFlash.setVehicle_Startup_Log_Writer(
FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void)
);
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);
gcs().send_text(MAV_SEVERITY_INFO, "Ready to drive");
}
/*
set the in_reverse flag
reset the throttle integrator if this changes in_reverse
*/
void Rover::set_reverse(bool reverse)
{
if (in_reverse == reverse) {
return;
}
in_reverse = reverse;
}
2017-07-24 14:05:59 -03:00
bool Rover::set_mode(Mode &new_mode, mode_reason_t 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
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE, new_mode.mode_number());
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 FRSKY_TELEM_ENABLED == ENABLED
2017-07-18 23:19:08 -03:00
frsky_telemetry.update_control_mode(control_mode->mode_number());
#endif
2017-07-18 23:19:08 -03:00
old_mode.exit();
if (should_log(MASK_LOG_MODE)) {
2017-07-24 14:05:59 -03:00
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode->mode_number(), reason);
}
2017-07-18 23:19:08 -03:00
notify_mode((enum mode)control_mode->mode_number());
return true;
}
void Rover::startup_INS_ground(void)
{
gcs().send_text(MAV_SEVERITY_INFO, "Warming up ADC");
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// -----------------------
gcs().send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle");
mavlink_delay(1000);
2013-01-13 01:03:54 -04:00
ahrs.init();
// say to EKF that rover only move by goind forward
ahrs.set_fly_forward(true);
2014-04-21 05:11:45 -03:00
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
2013-10-20 19:09:57 -03:00
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
}
2013-08-29 00:14:16 -03:00
// updates the notify state
2013-08-24 06:05:18 -03:00
// should be called at 50hz
void Rover::update_notify()
{
2013-08-29 00:14:16 -03:00
notify.update();
}
void Rover::resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
perf_mon_timer = millis();
}
void Rover::check_usb_mux(void)
{
bool usb_check = hal.gpio->usb_connected();
if (usb_check == usb_connected) {
return;
}
// the user has switched to/from the telemetry port
usb_connected = usb_check;
2013-04-19 04:53:07 -03:00
}
// update notify with mode change
void Rover::notify_mode(enum mode new_mode)
{
notify.flags.flight_mode = new_mode;
switch (new_mode) {
case MANUAL:
notify.set_flight_mode_str("MANU");
break;
case LEARNING:
notify.set_flight_mode_str("LERN");
break;
case STEERING:
notify.set_flight_mode_str("STER");
break;
case HOLD:
notify.set_flight_mode_str("HOLD");
break;
case AUTO:
notify.set_flight_mode_str("AUTO");
break;
case RTL:
notify.set_flight_mode_str("RTL");
break;
case GUIDED:
notify.set_flight_mode_str("GUID");
break;
case INITIALISING:
notify.set_flight_mode_str("INIT");
break;
default:
notify.set_flight_mode_str("----");
break;
}
}
/*
check a digitial pin for high,low (1/0)
*/
uint8_t Rover::check_digital_pin(uint8_t pin)
{
2017-01-31 06:12:26 -04:00
const int8_t dpin = hal.gpio->analogPinToDigitalPin(pin);
if (dpin == -1) {
return 0;
}
// ensure we are in input mode
2014-06-01 20:27:52 -03:00
hal.gpio->pinMode(dpin, HAL_GPIO_INPUT);
// enable pullup
hal.gpio->write(dpin, 1);
return hal.gpio->read(dpin);
}
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 DataFlash.should_log(mask);
2014-01-14 00:10:13 -04:00
}
/*
update AHRS soft arm state and log as needed
*/
void Rover::change_arm_state(void)
{
Log_Arm_Disarm();
update_soft_armed();
}
/*
arm motors
*/
bool Rover::arm_motors(AP_Arming::ArmingMethod method)
{
if (!arming.arm(method)) {
return false;
}
change_arm_state();
return true;
}
/*
disarm motors
*/
bool Rover::disarm_motors(void)
{
if (!arming.disarm()) {
return false;
}
2017-07-18 23:19:08 -03:00
if (control_mode != &mode_auto) {
// reset the mission on disarm if we are not in auto
mission.reset();
}
// only log if disarming was successful
change_arm_state();
return true;
}