// init - detect and initialise all mounts
void AP_Mount::init(const AP_SerialManager& serial_manager)
{
// check init has not been called before
if (_num_instances != 0) {
return;
}
// primary is reset to the first instantiated mount
bool primary_set = false;
// create each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
MountType mount_type = get_mount_type(instance);
// check for servo mounts
if (mount_type == Mount_Type_Servo) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_num_instances++;
#if AP_AHRS_NAVEKF_AVAILABLE
// check for MAVLink mounts
} else if (mount_type == Mount_Type_MAVLink) {
_backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance);
_num_instances++;
#endif
// check for Alexmos mounts
} else if (mount_type == Mount_Type_Alexmos) {
_backends[instance] = new AP_Mount_Alexmos(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts
} else if (mount_type == Mount_Type_SToRM32) {
_backends[instance] = new AP_Mount_SToRM32(*this, state[instance], instance);
_num_instances++;
}
// init new instance
if (_backends[instance] != NULL) {
_backends[instance]->init(serial_manager);
if (!primary_set) {
_primary = instance;
primary_set = true;
}
}
}
}
// init - detect and initialise all mounts
void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
{
// check init has not been called before
if (_num_instances != 0) {
return;
}
_dataflash = dataflash;
// create each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
MountType mount_type = get_mount_type(instance);
// check for servo mounts
if (mount_type == Mount_Type_Servo) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_num_instances++;
// check for Alexmos mounts
} else if (mount_type == Mount_Type_Alexmos) {
_backends[instance] = new AP_Mount_Alexmos(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts
} else if (mount_type == Mount_Type_SToRM32) {
_backends[instance] = new AP_Mount_SToRM32(*this, state[instance], instance);
_num_instances++;
}
// init new instance
if (_backends[instance] != NULL) {
_backends[instance]->init(serial_manager);
if (!primary_set) {
_primary = instance;
primary_set = true;
}
}
}
}
// init - detect and initialise all mounts
void AP_Mount::init(const AP_SerialManager& serial_manager)
{
// check init has not been called before
if (_num_instances != 0) {
return;
}
// default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
if (!state[0]._type.load()) {
if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_tilt) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_roll)) {
state[0]._type.set_and_save(Mount_Type_Servo);
}
}
// primary is reset to the first instantiated mount
bool primary_set = false;
// create each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
// default instance's state
state[instance]._mode = (enum MAV_MOUNT_MODE)state[instance]._default_mode.get();
MountType mount_type = get_mount_type(instance);
// check for servo mounts
if (mount_type == Mount_Type_Servo) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_num_instances++;
#if AP_AHRS_NAVEKF_AVAILABLE
// check for MAVLink mounts
} else if (mount_type == Mount_Type_MAVLink) {
_backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance);
_num_instances++;
#endif
// check for Alexmos mounts
} else if (mount_type == Mount_Type_Alexmos) {
_backends[instance] = new AP_Mount_Alexmos(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts using MAVLink protocol
} else if (mount_type == Mount_Type_SToRM32) {
_backends[instance] = new AP_Mount_SToRM32(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts using serial protocol
} else if (mount_type == Mount_Type_SToRM32_serial) {
_backends[instance] = new AP_Mount_SToRM32_serial(*this, state[instance], instance);
_num_instances++;
}
// init new instance
if (_backends[instance] != NULL) {
_backends[instance]->init(serial_manager);
if (!primary_set) {
_primary = instance;
primary_set = true;
}
}
}
}
// init - detect and initialise all mounts
void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
{
// check init has not been called before
if (_num_instances != 0) {
return;
}
_dataflash = dataflash;
// default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
if (!state[0]._type.configured()) {
if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_tilt) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_roll)) {
state[0]._type.set_and_save(Mount_Type_Servo);
}
}
// primary is reset to the first instantiated mount
bool primary_set = false;
// create each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
// default instance's state
state[instance]._mode = (enum MAV_MOUNT_MODE)state[instance]._default_mode.get();
MountType mount_type = get_mount_type(instance);
// check for servo mounts
//OW
//XX if (mount_type == Mount_Type_STorM32_Native) {
// we check if we can find the respective uart, it would be better to directly test the parameter field
// we do this first to ensure that the MNT_TYPE setting is ignored in case a native STorM32 protocol is used
// this is quite hacky, as doing it here assumes that AP_MOUNT_MAX_INSTANCES is 1
if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_STorM32_Native, 0)) {
_backends[instance] = new BP_Mount_Component(*this, state[instance], instance);
_num_instances++;
} else
//OWEND
if (mount_type == Mount_Type_Servo) {
_backends[instance] = new AP_Mount_Servo(*this, state[instance], instance);
_num_instances++;
#if AP_AHRS_NAVEKF_AVAILABLE
// check for MAVLink mounts
} else if (mount_type == Mount_Type_SoloGimbal) {
_backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance);
_num_instances++;
#endif
// check for Alexmos mounts
} else if (mount_type == Mount_Type_Alexmos) {
_backends[instance] = new AP_Mount_Alexmos(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts using MAVLink protocol
} else if (mount_type == Mount_Type_SToRM32) {
_backends[instance] = new AP_Mount_SToRM32(*this, state[instance], instance);
_num_instances++;
// check for SToRM32 mounts using serial protocol
} else if (mount_type == Mount_Type_SToRM32_serial) {
_backends[instance] = new AP_Mount_SToRM32_serial(*this, state[instance], instance);
_num_instances++;
}
// init new instance
if (_backends[instance] != NULL) {
_backends[instance]->init(serial_manager);
if (!primary_set) {
_primary = instance;
primary_set = true;
}
}
}
}
// 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();
}
}
}
