/* 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; } }