2015-06-01 02:04:25 -03:00
|
|
|
#include "Tracker.h"
|
2016-05-05 19:10:08 -03:00
|
|
|
#include "version.h"
|
2015-06-01 02:04:25 -03:00
|
|
|
|
2014-08-13 01:43:56 -03:00
|
|
|
// mission storage
|
|
|
|
static const StorageAccess wp_storage(StorageManager::StorageMission);
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
static void mavlink_snoop_static(const mavlink_message_t* msg)
|
|
|
|
{
|
2015-07-03 08:07:56 -03:00
|
|
|
tracker.mavlink_snoop(msg);
|
2015-06-01 02:04:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void mavlink_delay_cb_static()
|
|
|
|
{
|
|
|
|
tracker.mavlink_delay_cb();
|
|
|
|
}
|
|
|
|
|
|
|
|
void Tracker::init_tracker()
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
2015-01-28 01:27:03 -04:00
|
|
|
// initialise console serial port
|
|
|
|
serial_manager.init_console();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2015-10-25 17:10:41 -03:00
|
|
|
hal.console->printf("\n\nInit " THISFIRMWARE
|
2015-10-24 18:45:41 -03:00
|
|
|
"\n\nFree RAM: %u\n",
|
2015-06-01 02:04:25 -03:00
|
|
|
hal.util->available_memory());
|
2013-10-13 04:14:13 -03:00
|
|
|
|
|
|
|
// Check the EEPROM format version before loading any parameters from EEPROM
|
|
|
|
load_parameters();
|
|
|
|
|
2016-05-30 22:50:15 -03:00
|
|
|
gcs().set_dataflash(&DataFlash);
|
2014-03-26 18:06:50 -03:00
|
|
|
|
2016-10-16 19:21:08 -03:00
|
|
|
mavlink_system.sysid = g.sysid_this_mav;
|
|
|
|
|
2015-01-28 01:27:03 -04:00
|
|
|
// initialise serial ports
|
|
|
|
serial_manager.init();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2016-08-03 04:17:24 -03:00
|
|
|
// setup first port early to allow BoardConfig to report errors
|
2016-06-16 22:38:31 -03:00
|
|
|
gcs_chan[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
|
2016-08-03 04:17:24 -03:00
|
|
|
|
|
|
|
// 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();
|
|
|
|
|
2017-01-21 02:17:26 -04:00
|
|
|
// initialise notify
|
|
|
|
notify.init(false);
|
|
|
|
AP_Notify::flags.pre_arm_check = true;
|
|
|
|
AP_Notify::flags.pre_arm_gps_check = true;
|
|
|
|
AP_Notify::flags.failsafe_battery = false;
|
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
// init baro before we start the GCS, so that the CLI baro test works
|
|
|
|
barometer.init();
|
|
|
|
|
2014-03-22 05:31:28 -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.
|
|
|
|
usb_connected = true;
|
|
|
|
check_usb_mux();
|
|
|
|
|
2016-05-16 00:33:43 -03:00
|
|
|
// setup telem slots with serial ports
|
2016-08-03 04:17:24 -03:00
|
|
|
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
|
2016-06-16 22:38:31 -03:00
|
|
|
gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
|
|
|
|
gcs_chan[i].set_snoop(mavlink_snoop_static);
|
2016-05-16 00:33:43 -03:00
|
|
|
}
|
2015-05-02 09:38:58 -03:00
|
|
|
|
2015-12-27 03:05:14 -04:00
|
|
|
#if LOGGING_ENABLED == ENABLED
|
|
|
|
log_init();
|
|
|
|
#endif
|
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
if (g.compass_enabled==true) {
|
|
|
|
if (!compass.init() || !compass.read()) {
|
2017-01-21 00:32:39 -04:00
|
|
|
hal.console->printf("Compass initialisation failed!\n");
|
2013-10-13 04:14:13 -03:00
|
|
|
g.compass_enabled = false;
|
|
|
|
} else {
|
|
|
|
ahrs.set_compass(&compass);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// GPS Initialization
|
2016-10-28 10:09:31 -03:00
|
|
|
gps.init(nullptr, serial_manager);
|
2013-10-13 04:14:13 -03:00
|
|
|
|
|
|
|
ahrs.init();
|
|
|
|
ahrs.set_fly_forward(false);
|
|
|
|
|
2015-12-26 00:08:38 -04:00
|
|
|
ins.init(scheduler.get_loop_rate_hz());
|
2013-10-13 04:14:13 -03:00
|
|
|
ahrs.reset();
|
|
|
|
|
2016-05-23 22:31:41 -03:00
|
|
|
init_barometer(true);
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2017-05-01 03:22:35 -03:00
|
|
|
// initialise DataFlash library
|
|
|
|
DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void));
|
|
|
|
|
2015-01-28 01:27:03 -04:00
|
|
|
// set serial ports non-blocking
|
|
|
|
serial_manager.set_blocking_writes_all(false);
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2014-10-06 02:56:25 -03:00
|
|
|
// initialise servos
|
|
|
|
init_servos();
|
2013-10-13 04:14:13 -03:00
|
|
|
|
2014-03-22 05:09:01 -03:00
|
|
|
// use given start positions - useful for indoor testing, and
|
|
|
|
// while waiting for GPS lock
|
2015-08-27 02:38:00 -03:00
|
|
|
// sanity check location
|
2015-10-27 00:50:29 -03:00
|
|
|
if (fabsf(g.start_latitude) <= 90.0f && fabsf(g.start_longitude) <= 180.0f) {
|
2015-08-27 02:38:00 -03:00
|
|
|
current_loc.lat = g.start_latitude * 1.0e7f;
|
|
|
|
current_loc.lng = g.start_longitude * 1.0e7f;
|
2016-07-05 23:03:33 -03:00
|
|
|
} else {
|
2015-11-18 14:47:45 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_NOTICE, "Ignoring invalid START_LATITUDE or START_LONGITUDE parameter");
|
2015-08-27 02:38:00 -03:00
|
|
|
}
|
2014-03-05 01:47:47 -04:00
|
|
|
|
2014-03-22 05:09:01 -03:00
|
|
|
// see if EEPROM has a default location as well
|
2014-04-09 01:30:27 -03:00
|
|
|
if (current_loc.lat == 0 && current_loc.lng == 0) {
|
|
|
|
get_home_eeprom(current_loc);
|
|
|
|
}
|
2014-03-02 03:00:37 -04:00
|
|
|
|
2015-07-31 19:30:03 -03:00
|
|
|
init_capabilities();
|
|
|
|
|
2015-11-18 14:47:45 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_INFO,"Ready to track");
|
2014-03-02 03:00:37 -04:00
|
|
|
hal.scheduler->delay(1000); // Why????
|
2014-03-06 18:13:53 -04:00
|
|
|
|
|
|
|
set_mode(AUTO); // tracking
|
2014-03-22 05:09:01 -03:00
|
|
|
|
|
|
|
if (g.startup_delay > 0) {
|
|
|
|
// arm servos with trim value to allow them to start up (required
|
|
|
|
// for some servos)
|
|
|
|
prepare_servos();
|
|
|
|
}
|
2014-08-03 04:38:19 -03:00
|
|
|
|
2017-04-29 21:48:00 -03:00
|
|
|
// disable safety if requested
|
|
|
|
BoardConfig.init_safety();
|
2014-03-02 03:00:37 -04:00
|
|
|
}
|
|
|
|
|
2013-10-13 04:14:13 -03:00
|
|
|
// updates the status of the notify objects
|
|
|
|
// should be called at 50hz
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::update_notify()
|
2013-10-13 04:14:13 -03:00
|
|
|
{
|
|
|
|
notify.update();
|
|
|
|
}
|
|
|
|
|
2014-03-02 03:00:37 -04:00
|
|
|
/*
|
|
|
|
fetch HOME from EEPROM
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
bool Tracker::get_home_eeprom(struct Location &loc)
|
2014-03-02 03:00:37 -04:00
|
|
|
{
|
|
|
|
// Find out proper location in memory by using the start_byte position + the index
|
|
|
|
// --------------------------------------------------------------------------------
|
|
|
|
if (g.command_total.get() == 0) {
|
2014-03-22 05:09:01 -03:00
|
|
|
return false;
|
2014-03-02 03:00:37 -04:00
|
|
|
}
|
|
|
|
|
2014-03-22 05:09:01 -03:00
|
|
|
// read WP position
|
2014-08-13 01:43:56 -03:00
|
|
|
loc.options = wp_storage.read_byte(0);
|
|
|
|
loc.alt = wp_storage.read_uint32(1);
|
|
|
|
loc.lat = wp_storage.read_uint32(5);
|
|
|
|
loc.lng = wp_storage.read_uint32(9);
|
2014-03-22 05:09:01 -03:00
|
|
|
|
|
|
|
return true;
|
2014-03-02 03:00:37 -04:00
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::set_home_eeprom(struct Location temp)
|
2014-03-02 03:00:37 -04:00
|
|
|
{
|
2014-08-13 01:43:56 -03:00
|
|
|
wp_storage.write_byte(0, temp.options);
|
|
|
|
wp_storage.write_uint32(1, temp.alt);
|
|
|
|
wp_storage.write_uint32(5, temp.lat);
|
|
|
|
wp_storage.write_uint32(9, temp.lng);
|
2014-03-02 03:00:37 -04:00
|
|
|
|
|
|
|
// Now have a home location in EEPROM
|
|
|
|
g.command_total.set_and_save(1); // At most 1 entry for HOME
|
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::set_home(struct Location temp)
|
2014-03-02 03:00:37 -04:00
|
|
|
{
|
|
|
|
set_home_eeprom(temp);
|
2014-03-05 01:47:47 -04:00
|
|
|
current_loc = temp;
|
2015-10-02 08:53:28 -03:00
|
|
|
GCS_MAVLINK::send_home_all(temp);
|
2014-03-02 03:00:37 -04:00
|
|
|
}
|
2014-03-03 20:37:15 -04:00
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::arm_servos()
|
2017-01-06 22:51:56 -04:00
|
|
|
{
|
|
|
|
hal.util->set_soft_armed(true);
|
2017-06-16 10:22:54 -03:00
|
|
|
DataFlash.set_vehicle_armed(true);
|
2014-03-03 20:37:15 -04:00
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::disarm_servos()
|
2014-03-03 20:37:15 -04:00
|
|
|
{
|
2017-01-06 22:51:56 -04:00
|
|
|
hal.util->set_soft_armed(false);
|
2017-06-16 10:22:54 -03:00
|
|
|
DataFlash.set_vehicle_armed(false);
|
2014-03-03 20:37:15 -04:00
|
|
|
}
|
2014-03-06 18:13:53 -04:00
|
|
|
|
2014-03-22 05:09:01 -03:00
|
|
|
/*
|
|
|
|
setup servos to trim value after initialising
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::prepare_servos()
|
2014-03-22 05:09:01 -03:00
|
|
|
{
|
2015-11-19 23:04:37 -04:00
|
|
|
start_time_ms = AP_HAL::millis();
|
2017-01-08 18:30:37 -04:00
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_tracker_yaw, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_tracker_pitch, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
2017-01-06 22:51:56 -04:00
|
|
|
SRV_Channels::calc_pwm();
|
|
|
|
SRV_Channels::output_ch_all();
|
2014-03-22 05:09:01 -03:00
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::set_mode(enum ControlMode mode)
|
2014-03-06 18:13:53 -04:00
|
|
|
{
|
2016-12-13 22:06:30 -04:00
|
|
|
if (control_mode == mode) {
|
2014-03-06 18:13:53 -04:00
|
|
|
// don't switch modes if we are already in the correct mode.
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
control_mode = mode;
|
2014-03-22 05:09:01 -03:00
|
|
|
|
|
|
|
switch (control_mode) {
|
|
|
|
case AUTO:
|
|
|
|
case MANUAL:
|
2014-04-09 02:27:56 -03:00
|
|
|
case SCAN:
|
2014-10-06 08:45:07 -03:00
|
|
|
case SERVO_TEST:
|
2014-03-22 05:09:01 -03:00
|
|
|
arm_servos();
|
|
|
|
break;
|
|
|
|
|
|
|
|
case STOP:
|
|
|
|
case INITIALISING:
|
|
|
|
disarm_servos();
|
|
|
|
break;
|
|
|
|
}
|
2016-02-09 04:00:18 -04:00
|
|
|
|
|
|
|
// log mode change
|
|
|
|
DataFlash.Log_Write_Mode(control_mode);
|
2014-03-06 18:13:53 -04:00
|
|
|
}
|
|
|
|
|
2014-09-29 06:47:29 -03:00
|
|
|
/*
|
|
|
|
set_mode() wrapper for MAVLink SET_MODE
|
|
|
|
*/
|
2015-06-01 02:04:25 -03:00
|
|
|
bool Tracker::mavlink_set_mode(uint8_t mode)
|
2014-09-29 06:47:29 -03:00
|
|
|
{
|
|
|
|
switch (mode) {
|
|
|
|
case AUTO:
|
|
|
|
case MANUAL:
|
|
|
|
case SCAN:
|
2014-10-06 08:45:07 -03:00
|
|
|
case SERVO_TEST:
|
2014-09-29 06:47:29 -03:00
|
|
|
case STOP:
|
|
|
|
set_mode((enum ControlMode)mode);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-06-01 02:04:25 -03:00
|
|
|
void Tracker::check_usb_mux(void)
|
2014-03-22 05:31:28 -03:00
|
|
|
{
|
|
|
|
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;
|
|
|
|
}
|
2015-12-27 03:05:14 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
should we log a message type now?
|
|
|
|
*/
|
|
|
|
bool Tracker::should_log(uint32_t mask)
|
|
|
|
{
|
2017-06-09 00:38:39 -03:00
|
|
|
if (!(mask & g.log_bitmask)) {
|
2015-12-27 03:05:14 -04:00
|
|
|
return false;
|
|
|
|
}
|
2017-06-14 20:27:25 -03:00
|
|
|
if (!DataFlash.should_log()) {
|
|
|
|
return false;
|
|
|
|
}
|
2015-12-27 03:05:14 -04:00
|
|
|
return true;
|
|
|
|
}
|