/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0));
}
}
// constructor
AP_Beacon_Pozyx::AP_Beacon_Pozyx(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend),
linebuf_len(0)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar, 0));
}
}
/*
The constructor also initialises the proximity sensor. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the proximity sensor
*/
AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state,
AP_SerialManager &serial_manager) :
AP_Proximity_Backend(_frontend, _state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder &_ranger, uint8_t instance,
RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager) :
AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_RADAR)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model) :
AP_RangeFinder_Backend(_state),
model_type(model)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
last_update_ms = 0;
parse_state = RECV_HDR; // current state of receive data
num_bytes_in_block_received = 0; // bytes received
data_id = 0;
hedge._have_new_values = false;
hedge.positions_beacons.num_beacons = 0;
hedge.positions_beacons.updated = false;
}
}
/*
setup a UART, handling begin() and init()
*/
void
GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance)
{
// search for serial port
AP_HAL::UARTDriver *uart;
uart = serial_manager.find_serial(protocol, instance);
if (uart == NULL) {
// return immediately if not found
return;
}
// get associated mavlink channel
mavlink_channel_t mav_chan;
if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) {
// return immediately in unlikely case mavlink channel cannot be found
return;
}
/*
Now try to cope with SiK radios that may be stuck in bootloader
mode because CTS was held while powering on. This tells the
bootloader to wait for a firmware. It affects any SiK radio with
CTS connected that is externally powered. To cope we send 0x30
0x20 at 115200 on startup, which tells the bootloader to reset
and boot normally
*/
uart->begin(115200);
AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control();
uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
for (uint8_t i=0; i<3; i++) {
hal.scheduler->delay(1);
uart->write(0x30);
uart->write(0x20);
}
// since tcdrain() and TCSADRAIN may not be implemented...
hal.scheduler->delay(1);
uart->set_flow_control(old_flow_control);
// now change back to desired baudrate
uart->begin(serial_manager.find_baudrate(protocol, instance));
// and init the gcs instance
init(uart, mav_chan);
}
// update - give mount opportunity to update servos. should be called at 10hz or higher
// update - give mount opportunity to update servos. should be called at 10hz or higher
void AP_Mount::update(uint8_t mount_compid, AP_SerialManager& serial_manager)
{
#if AP_AHRS_NAVEKF_AVAILABLE
static AP_HAL::UARTDriver *uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MAVLink,1);
static bool begin_gmb_uart;
static uint32_t baud = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_MAVLink,1);
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if(_retries > MAX_RETRIES) { //no mavlink gimbal found
break;
}
MountType mount_type = get_mount_type(instance);
// check for MAVLink mounts
if (mount_type == Mount_Type_MAVLink && !_mav_gimbal_found) {
if(mount_compid == MAV_COMP_ID_GIMBAL) {
_backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance);
_num_instances++;
_mav_gimbal_found = true;
}
if (mount_compid == MAV_COMP_ID_R10C_GIMBAL) {
_backends[instance] = new AP_Mount_R10C(*this, state[instance], instance);
_num_instances++;
_mav_gimbal_found = true;
}
// init new instance
if (_backends[instance] != NULL && _mav_gimbal_found) {
_backends[instance]->init(serial_manager);
if (!primary_set) {
_primary = instance;
primary_set = true;
}
} else if(_timeout) {
//change baud rate of gimbal port and retry
if(baud == 921600) {
hal.console->printf("Looking for R10C Gimbal!!\n");
uart->end();
baud = 230400;
} else if(baud == 230400) {
hal.console->printf("Looking for Solo Gimbal!!\n");
uart->end();
baud = 921600;
} else {
hal.console->printf("Setting Back to Default Baud: %d!!\n", 921600);
uart->end();
baud = 921600;
}
_timeout = false;
begin_gmb_uart = true;
_last_time = hal.scheduler->millis();
_retries++;
if(_retries == MAX_RETRIES) {
hal.console->printf("No MavLink Gimbal Found!!\n");
}
} else {
if(begin_gmb_uart && ((hal.scheduler->millis() - _last_time)>=1000)) {
uart->begin(baud);
hal.console->printf("Setting Gimbal port baud to %d\n", baud);
begin_gmb_uart = false;
}
_timeout = ((hal.scheduler->millis() - _last_time)>=5000) ? true : false;
}
}
}
#endif
// update each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->update();
}
}
}
