ardupilot/AntennaTracker/system.cpp
skyscraper 662cefd086 AntennaTracker: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to  get and set functionsss

old public member(int16_t)   get function -> int16_t     set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in                     c.get_radio_in()           c.set_radio_in(v)
c.control_in                   c.get_control_in()         c.set_control_in(v)
c.servo_out                    c.get_servo_out()          c.set_servo_out(v)
c.pwm_out                      c.get_pwm_out()            // use existing
c.radio_out                    c.get_radio_out()          c.set_radio_out(v)
c.radio_max                    c.get_radio_max()          c.set_radio_max(v)
c.radio_min                    c.get_radio_min()          c.set_radio_min(v)
c.radio_trim                   c.get_radio_trim()         c.set_radio_trim(v);

c.min_max_configured() // return true if min and max are configured

Because data members of RC_Channels are now private and so cannot be written directly
 some overloads are provided in the Plane classes to provide the old functionality

new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);

new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;

Rename functions

 RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
    to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)

 RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
    to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)

 Rationale:

        RC_Channel is a complicated class, which combines
        several functionalities dealing with stick inputs
        in pwm and logical units, logical and actual actuator
        outputs, unit conversion etc, etc
        The intent of this PR is to clarify existing use of
        the class. At the basic level it should now be possible
        to grep all places where private variable is set by
        searching for the set_xx function.

        (The wider purpose is to provide a more generic and
        logically simpler method of output mixing. This is a small step)
2016-05-10 16:21:16 +10:00

262 lines
6.6 KiB
C++

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Tracker.h"
#include "version.h"
// mission storage
static const StorageAccess wp_storage(StorageManager::StorageMission);
static void mavlink_snoop_static(const mavlink_message_t* msg)
{
tracker.mavlink_snoop(msg);
}
static void mavlink_delay_cb_static()
{
tracker.mavlink_delay_cb();
}
void Tracker::init_tracker()
{
// initialise console serial port
serial_manager.init_console();
hal.console->printf("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n",
hal.util->available_memory());
// Check the EEPROM format version before loading any parameters from EEPROM
load_parameters();
BoardConfig.init();
// initialise serial ports
serial_manager.init();
// init baro before we start the GCS, so that the CLI baro test works
barometer.init();
// init the GCS and start snooping for vehicle data
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
gcs[0].set_snoop(mavlink_snoop_static);
// 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);
// 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 and start snooping for vehicle data
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
gcs[1].set_snoop(mavlink_snoop_static);
// setup serial port for telem2 and start snooping for vehicle data
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
gcs[2].set_snoop(mavlink_snoop_static);
// setup serial port for fourth telemetry port (not used by default) and start snooping for vehicle data
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
gcs[3].set_snoop(mavlink_snoop_static);
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
GCS_MAVLINK::set_dataflash(&DataFlash);
if (g.compass_enabled==true) {
if (!compass.init() || !compass.read()) {
hal.console->println("Compass initialisation failed!");
g.compass_enabled = false;
} else {
ahrs.set_compass(&compass);
}
}
// GPS Initialization
gps.init(NULL, serial_manager);
ahrs.init();
ahrs.set_fly_forward(false);
ins.init(scheduler.get_loop_rate_hz());
ahrs.reset();
init_barometer();
// set serial ports non-blocking
serial_manager.set_blocking_writes_all(false);
// initialise servos
init_servos();
// use given start positions - useful for indoor testing, and
// while waiting for GPS lock
// sanity check location
if (fabsf(g.start_latitude) <= 90.0f && fabsf(g.start_longitude) <= 180.0f) {
current_loc.lat = g.start_latitude * 1.0e7f;
current_loc.lng = g.start_longitude * 1.0e7f;
gcs_send_text(MAV_SEVERITY_NOTICE, "Ignoring invalid START_LATITUDE or START_LONGITUDE parameter");
}
// see if EEPROM has a default location as well
if (current_loc.lat == 0 && current_loc.lng == 0) {
get_home_eeprom(current_loc);
}
init_capabilities();
gcs_send_text(MAV_SEVERITY_INFO,"Ready to track");
hal.scheduler->delay(1000); // Why????
set_mode(AUTO); // tracking
if (g.startup_delay > 0) {
// arm servos with trim value to allow them to start up (required
// for some servos)
prepare_servos();
}
// calibrate pressure on startup by default
nav_status.need_altitude_calibration = true;
}
// updates the status of the notify objects
// should be called at 50hz
void Tracker::update_notify()
{
notify.update();
}
/*
fetch HOME from EEPROM
*/
bool Tracker::get_home_eeprom(struct Location &loc)
{
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (g.command_total.get() == 0) {
return false;
}
// read WP position
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);
return true;
}
void Tracker::set_home_eeprom(struct Location temp)
{
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);
// Now have a home location in EEPROM
g.command_total.set_and_save(1); // At most 1 entry for HOME
}
void Tracker::set_home(struct Location temp)
{
set_home_eeprom(temp);
current_loc = temp;
GCS_MAVLINK::send_home_all(temp);
}
void Tracker::arm_servos()
{
channel_yaw.enable_out();
channel_pitch.enable_out();
}
void Tracker::disarm_servos()
{
channel_yaw.disable_out();
channel_pitch.disable_out();
}
/*
setup servos to trim value after initialising
*/
void Tracker::prepare_servos()
{
start_time_ms = AP_HAL::millis();
channel_yaw.set_radio_out(channel_yaw.get_radio_trim());
channel_pitch.set_radio_out(channel_pitch.get_radio_trim());
channel_yaw.output();
channel_pitch.output();
}
void Tracker::set_mode(enum ControlMode mode)
{
if(control_mode == mode) {
// don't switch modes if we are already in the correct mode.
return;
}
control_mode = mode;
switch (control_mode) {
case AUTO:
case MANUAL:
case SCAN:
case SERVO_TEST:
arm_servos();
break;
case STOP:
case INITIALISING:
disarm_servos();
break;
}
// log mode change
DataFlash.Log_Write_Mode(control_mode);
}
/*
set_mode() wrapper for MAVLink SET_MODE
*/
bool Tracker::mavlink_set_mode(uint8_t mode)
{
switch (mode) {
case AUTO:
case MANUAL:
case SCAN:
case SERVO_TEST:
case STOP:
set_mode((enum ControlMode)mode);
return true;
}
return false;
}
void Tracker::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;
}
/*
should we log a message type now?
*/
bool Tracker::should_log(uint32_t mask)
{
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
return false;
}
return true;
}