int initialise() {
setbuf(stdout, NULL);
if (initialiseDS18B20() == 0)
return 0;
if (initialiseMCP3008() == 0)
return 0;
if (initialiseGPS() == 0)
return 0;
if (initialiseGeiger() == 0)
return 0;
bmp085_Calibration();
return 1;
}
void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
{
if (!_gps_initialised) {
initialiseGPS(gps);
return;
}
if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
_last_valid_gps_time_us = hrt_absolute_time();
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
_gps_speed_valid = gps->vel_ned_valid;
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_posRef, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
gps_sample_new.pos(0) = lpos_x;
gps_sample_new.pos(1) = lpos_y;
gps_sample_new.hgt = gps->alt / 1e3f;
_gps_buffer.push(gps_sample_new);
}
}
