//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
void Plane::navigate()
{
// allow change of nav controller mid-flight
set_nav_controller();
// do not navigate with corrupt data
// ---------------------------------
if (!have_position) {
return;
}
if (next_WP_loc.lat == 0) {
return;
}
// waypoint distance from plane
// ----------------------------
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
// update total loiter angle
loiter_angle_update();
// control mode specific updates to navigation demands
// ---------------------------------------------------
update_navigation();
}
void Plane::init_ardupilot()
{
// initialise serial port
serial_manager.init_console();
cliSerial->printf_P(PSTR("\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();
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet
for (uint8_t tries=0; tries<10; tries++) {
if (setup_failsafe_mixing()) {
break;
}
hal.scheduler->delay(10);
}
#endif
BoardConfig.init();
// initialise serial ports
serial_manager.init();
// allow servo set on all channels except first 4
ServoRelayEvents.set_channel_mask(0xFFF0);
set_control_channels();
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// initialise rangefinder
init_rangefinder();
// initialise battery monitoring
battery.init();
// init the GCS
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true;
check_usb_mux();
// setup serial port for telem1
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
#if MAVLINK_COMM_NUM_BUFFERS > 2
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
#endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#endif
// setup frsky
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.init(serial_manager);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
apm1_adc.Init(); // APM ADC library initialization
#endif
// initialise airspeed sensor
airspeed.init();
if (g.compass_enabled==true) {
if (!compass.init() || !compass.read()) {
cliSerial->println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
}
#if OPTFLOW == ENABLED
// make optflow available to libraries
ahrs.set_optflow(&optflow);
#endif
// 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);
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// GPS Initialization
gps.init(&DataFlash, serial_manager);
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
#endif
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
if (g.cli_enabled == 1) {
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println_P(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println_P(msg);
}
}
#endif // CLI_ENABLED
startup_ground();
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// choose the nav controller
set_nav_controller();
set_mode((FlightMode)g.initial_mode.get());
// set the correct flight mode
// ---------------------------
reset_control_switch();
// initialise sensor
#if OPTFLOW == ENABLED
optflow.init();
#endif
}
void Plane::init_ardupilot()
{
// initialise serial port
serial_manager.init_console();
cliSerial->printf("\n\nInit " FIRMWARE_STRING
"\n\nFree RAM: %u\n",
(unsigned)hal.util->available_memory());
//
// Check the EEPROM format version before loading any parameters from EEPROM
//
load_parameters();
#if HIL_SUPPORT
if (g.hil_mode == 1) {
// set sensors to HIL mode
ins.set_hil_mode();
compass.set_hil_mode();
barometer.set_hil_mode();
}
#endif
set_control_channels();
init_rc_out_main();
#if HAVE_PX4_MIXER
if (!quadplane.enable) {
// this must be before BoardConfig.init() so if
// BRD_SAFETYENABLE==0 then we don't have safety off yet. For
// quadplanes we wait till AP_Motors is initialised
for (uint8_t tries=0; tries<10; tries++) {
if (setup_failsafe_mixing()) {
break;
}
hal.scheduler->delay(10);
}
}
#endif
GCS_MAVLINK::set_dataflash(&DataFlash);
// initialise serial ports
serial_manager.init();
gcs[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);
// setup any board specific drivers
BoardConfig.init();
// allow servo set on all channels except first 4
ServoRelayEvents.set_channel_mask(0xFFF0);
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// initialise rangefinder
init_rangefinder();
// initialise battery monitoring
battery.init();
rpm_sensor.init();
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true;
check_usb_mux();
// setup telem slots with serial ports
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
// setup frsky
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky, and pass a number of parameters to the library
frsky_telemetry.init(serial_manager, FIRMWARE_STRING,
MAV_TYPE_FIXED_WING,
&g.fs_batt_voltage, &g.fs_batt_mah);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// initialise airspeed sensor
airspeed.init();
if (g.compass_enabled==true) {
bool compass_ok = compass.init() && compass.read();
#if HIL_SUPPORT
if (g.hil_mode != 0) {
compass_ok = true;
}
#endif
if (!compass_ok) {
cliSerial->println("Compass initialisation failed!");
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
}
#if OPTFLOW == ENABLED
// make optflow available to libraries
if (optflow.enabled()) {
ahrs.set_optflow(&optflow);
}
#endif
// give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed);
// GPS Initialization
gps.init(&DataFlash, serial_manager);
init_rc_in(); // sets up rc channels from radio
relay.init();
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(&DataFlash, serial_manager);
#endif
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
if (g.cli_enabled == 1) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->println(msg);
if (gcs[1].initialised && (gcs[1].get_uart() != NULL)) {
gcs[1].get_uart()->println(msg);
}
if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) {
gcs[2].get_uart()->println(msg);
}
}
#endif // CLI_ENABLED
init_capabilities();
quadplane.setup();
startup_ground();
// don't initialise aux rc output until after quadplane is setup as
// that can change initial values of channels
init_rc_out_aux();
// choose the nav controller
set_nav_controller();
set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN);
// set the correct flight mode
// ---------------------------
reset_control_switch();
// initialise sensor
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
optflow.init();
}
#endif
}