/*
initialise the RangeFinder class. We do detection of attached range
finders here. For now we won't allow for hot-plugging of
rangefinders.
*/
void RangeFinder::init(void)
{
if (num_instances != 0) {
// init called a 2nd time?
return;
}
for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
// serial_instance will be increased inside detect_instance
// if a serial driver is loaded for this instance
detect_instance(i, serial_instance);
if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
}
// initialise pre-arm check variables
state[i].pre_arm_check = false;
state[i].pre_arm_distance_min = 9999; // initialise to an arbitrary large value
state[i].pre_arm_distance_max = 0;
// initialise status
state[i].status = RangeFinder_NotConnected;
state[i].range_valid_count = 0;
}
}
void AP_InertialSensor::init_accel()
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if(drivers[i] == NULL){
detect_instance(i);
}
else{
drivers[i]->init_accel();
}
}
}
/*
update one GPS instance. This should be called at 10Hz or greater
*/
void
AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) {
// in HIL, leave info alone
return;
}
if (_type[instance] == GPS_TYPE_NONE) {
// not enabled
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
return;
}
if (locked_ports & (1U<<instance)) {
// the port is locked by another driver
return;
}
if (drivers[instance] == NULL || state[instance].status == NO_GPS) {
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance(instance);
return;
}
send_blob_update(instance);
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 1.2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > 1200) {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = NULL;
memset(&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
timing[instance].last_message_time_ms = tnow;
}
} else {
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
}
}
bool AP_InertialSensor::init(Start_style style, Sample_rate sample_rate)
{
bool success = true;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if(drivers[i] == NULL)
detect_instance(i);
/*success &=*/ drivers[i]->init(style, sample_rate);
}
//TODO check return statement on drivers[i]->init();
return success;
}
/// update - returns true if all the IMU's values have been updated successfully
bool AP_InertialSensor::update()
{
bool success = true;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if(drivers[i] == NULL){
detect_instance(i);
}
else{
success &= drivers[i]->_update();
}
}
return success;
}
/*
initialise the RangeFinder class. We do detection of attached range
finders here. For now we won't allow for hot-plugging of
rangefinders.
*/
void RangeFinder::init(void)
{
if (num_instances != 0) {
// init called a 2nd time?
return;
}
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
detect_instance(i);
if (drivers[i] != NULL) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
}
}
}
// initialise the Proximity class. We do detection of attached sensors here
// we don't allow for hot-plugging of sensors (i.e. reboot required)
void AP_Proximity::init(void)
{
if (num_instances != 0) {
// init called a 2nd time?
return;
}
for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) {
detect_instance(i);
if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
}
// initialise status
state[i].status = Proximity_NotConnected;
}
}
