Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use millis/micros/panic functions

Browse Source
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-11-20 12:11:52 +09:00 committed by Randy Mackay
parent 87b9b4463e
commit d7601095fa
11 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -322,7 +322,7 @@ AP_InertialSensor::AP_InertialSensor() :
_dataflash(NULL)
{
if (_s_instance) {
hal.scheduler->panic("Too many inertial sensors");
AP_HAL::panic("Too many inertial sensors");
}
_s_instance = this;
AP_Param::setup_object_defaults(this, var_info);
@ -371,7 +371,7 @@ AP_InertialSensor *AP_InertialSensor::get_instance()
uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz)
{
if (_gyro_count == INS_MAX_INSTANCES) {
hal.scheduler->panic("Too many gyros");
AP_HAL::panic("Too many gyros");
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
return _gyro_count++;
@ -383,7 +383,7 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz)
uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz)
{
if (_accel_count == INS_MAX_INSTANCES) {
hal.scheduler->panic("Too many accels");
AP_HAL::panic("Too many accels");
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
return _accel_count++;
@ -403,7 +403,7 @@ void AP_InertialSensor::_start_backends()
}
if (_gyro_count == 0 || _accel_count == 0) {
hal.scheduler->panic("INS needs at least 1 gyro and 1 accel");
AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
}
}
@ -480,7 +480,7 @@ void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
if (!backend)
return;
if (_backend_count == INS_MAX_BACKENDS)
hal.scheduler->panic("Too many INS backends");
AP_HAL::panic("Too many INS backends");
_backends[_backend_count++] = backend;
}
@ -530,7 +530,7 @@ AP_InertialSensor::detect_backends(void)
#endif
if (_backend_count == 0) {
hal.scheduler->panic("No INS backends available");
AP_HAL::panic("No INS backends available");
}
// set the product ID to the ID of the first backend
@ -1352,7 +1352,7 @@ void AP_InertialSensor::wait_for_sample(void)
return;
}
uint32_t now = hal.scheduler->micros();
uint32_t now = AP_HAL::micros();
if (_next_sample_usec == 0 && _delta_time <= 0) {
// this is the first call to wait_for_sample()
@ -1366,7 +1366,7 @@ void AP_InertialSensor::wait_for_sample(void)
// we're ahead on time, schedule next sample at expected period
uint32_t wait_usec = _next_sample_usec - now;
hal.scheduler->delay_microseconds_boost(wait_usec);
uint32_t now2 = hal.scheduler->micros();
uint32_t now2 = AP_HAL::micros();
if (now2+100 < _next_sample_usec) {
timing_printf("shortsleep %u\n", (unsigned)(_next_sample_usec-now2));
}
@ -1408,7 +1408,7 @@ check_sample:
}
}
now = hal.scheduler->micros();
now = AP_HAL::micros();
if (_hil_mode && _hil.delta_time > 0) {
_delta_time = _hil.delta_time;
_hil.delta_time = 0;

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -98,7 +98,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
uint64_t now = hal.scheduler->micros64();
uint64_t now = AP_HAL::micros64();
struct log_GYRO pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+instance)),
time_us : now,
@ -153,7 +153,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
uint64_t now = hal.scheduler->micros64();
uint64_t now = AP_HAL::micros64();
struct log_ACCEL pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+instance)),
time_us : now,

8
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -96,7 +96,7 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
uint8_t data;
hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
if (data != FLYMAPLE_ACCELEROMETER_XL345_DEVID)
hal.scheduler->panic("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor");
AP_HAL::panic("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor");
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
hal.scheduler->delay(5);
hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
@ -118,7 +118,7 @@ bool AP_InertialSensor_Flymaple::_init_sensor(void)
// Expect to read the same as the Gyro I2C adress:
hal.i2c->readRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_WHO_AM_I, &data);
if (data != FLYMAPLE_GYRO_ADDRESS)
hal.scheduler->panic("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor");
AP_HAL::panic("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor");
hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00);
hal.scheduler->delay(1);
// Sample rate divider: with 8kHz internal clock (see FLYMAPLE_GYRO_DLPF_FS),
@ -176,7 +176,7 @@ void AP_InertialSensor_Flymaple::accumulate(void)
// Read accelerometer
// ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
uint8_t buffer[6];
uint32_t now = hal.scheduler->micros();
uint32_t now = AP_HAL::micros();
// This takes about 250us at 400kHz I2C speed
if ((now - _last_accel_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
@ -197,7 +197,7 @@ void AP_InertialSensor_Flymaple::accumulate(void)
}
// Read gyro
now = hal.scheduler->micros();
now = AP_HAL::micros();
// This takes about 250us at 400kHz I2C speed
if ((now - _last_gyro_timestamp) >= raw_sample_interval_us
&& hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)

4
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -151,7 +151,7 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
uint8_t data = 0;
hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_DEVID, &data);
if (data != ADXL345_ACCELEROMETER_XL345_DEVID) {
hal.scheduler->panic("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor");
AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor");
}
hal.i2c->writeRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
hal.scheduler->delay(5);
@ -180,7 +180,7 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
// Expect to read the right 'WHO_AM_I' value
hal.i2c->readRegister(L3G4200D_I2C_ADDRESS, L3G4200D_REG_WHO_AM_I, &data);
if (data != L3G4200D_REG_WHO_AM_I_VALUE)
hal.scheduler->panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor");
AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor");
// setup for 800Hz sampling with 110Hz filter
hal.i2c->writeRegister(L3G4200D_I2C_ADDRESS,

4
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -410,14 +410,14 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
if (_drdy_pin_num_a >= 0) {
_drdy_pin_a = hal.gpio->channel(_drdy_pin_num_a);
if (_drdy_pin_a == NULL) {
hal.scheduler->panic("LSM9DS0: null accel data-ready GPIO channel\n");
AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel\n");
}
}
if (_drdy_pin_num_g >= 0) {
_drdy_pin_g = hal.gpio->channel(_drdy_pin_num_g);
if (_drdy_pin_g == NULL) {
hal.scheduler->panic("LSM9DS0: null gyro data-ready GPIO channel\n");
AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel\n");
}
}

4
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -521,7 +521,7 @@ void AP_InertialSensor_MPU6000::start()
hal.scheduler->suspend_timer_procs();
if (!_bus_sem->take(100)) {
hal.scheduler->panic("MPU6000: Unable to get semaphore");
AP_HAL::panic("MPU6000: Unable to get semaphore");
}
// initially run the bus at low speed
@ -765,7 +765,7 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
bool AP_InertialSensor_MPU6000::_hardware_init(void)
{
if (!_bus_sem->take(100)) {
hal.scheduler->panic("MPU6000: Unable to get semaphore");
AP_HAL::panic("MPU6000: Unable to get semaphore");
}
// initially run the bus at low speed

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -395,11 +395,11 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
// ;
// }
// else {
// hal.scheduler->panic("AP_InertialSensor_MPU9150: Unsupported software product rev.\n");
// AP_HAL::panic("AP_InertialSensor_MPU9150: Unsupported software product rev.\n");
// goto failed;
// }
// } else {
// hal.scheduler->panic("Product ID read as 0 indicates device is either incompatible or an MPU3050.\n");
// AP_HAL::panic("Product ID read as 0 indicates device is either incompatible or an MPU3050.\n");
// goto failed;
// }
@ -961,7 +961,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel,
}
}
*timestamp = hal.scheduler->millis();
*timestamp = AP_HAL::millis();
// read the data
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
more[0] = fifo_count / packet_size - 1;

8
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -112,7 +112,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
// calculate gyro sample time
int samplerate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
if (samplerate < 100 || samplerate > 10000) {
hal.scheduler->panic("Invalid gyro sample rate");
AP_HAL::panic("Invalid gyro sample rate");
}
_gyro_instance[i] = _imu.register_gyro(samplerate);
_gyro_sample_time[i] = 1.0f / samplerate;
@ -152,7 +152,7 @@ bool AP_InertialSensor_PX4::_init_sensor(void)
// calculate accel sample time
int samplerate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
if (samplerate < 100 || samplerate > 10000) {
hal.scheduler->panic("Invalid accel sample rate");
AP_HAL::panic("Invalid accel sample rate");
}
_accel_instance[i] = _imu.register_accel(samplerate);
_accel_sample_time[i] = 1.0f / samplerate;
@ -213,7 +213,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
_accel_meas_count[i] ++;
if(_accel_meas_count[i] >= 10000) {
uint32_t tnow = hal.scheduler->micros();
uint32_t tnow = AP_HAL::micros();
::printf("a%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0f/((tnow-_accel_meas_count_start_us[i])*1.0e-6f),_accel_dt_max[i]);
@ -248,7 +248,7 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report
_gyro_meas_count[i] ++;
if(_gyro_meas_count[i] >= 10000) {
uint32_t tnow = hal.scheduler->micros();
uint32_t tnow = AP_HAL::micros();
::printf("g%d %.2f Hz max %.8f s\n", frontend_instance, 10000.0f/((tnow-_gyro_meas_count_start_us[i])*1.0e-6f), _gyro_dt_max[i]);

2
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -118,7 +118,7 @@ float AP_InertialSensor_SITL::gyro_drift(void)
return 0;
}
double period = sitl->drift_time * 2;
double minutes = fmod(hal.scheduler->micros64() / 60.0e6, period);
double minutes = fmod(AP_HAL::micros64() / 60.0e6, period);
if (minutes < period/2) {
return minutes * ToRad(sitl->drift_speed);
}

4
libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp
View File

@ -23,11 +23,11 @@ static void _snoop(const mavlink_message_t* msg)
bool AP_InertialSensor_UserInteract_MAVLink::blocking_read(void)
{
uint32_t start_ms = hal.scheduler->millis();
uint32_t start_ms = AP_HAL::millis();
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
_got_ack = false;
while (hal.scheduler->millis() - start_ms < 30000U) {
while (AP_HAL::millis() - start_ms < 30000U) {
hal.scheduler->delay(10);
if (_got_ack) {
_gcs->set_snoop(NULL);

10
libraries/AP_InertialSensor/examples/VibTest/VibTest.cpp
View File

@ -73,7 +73,7 @@ void setup(void)
gyro_fd[i] = open(gyro_path, O_RDONLY);
}
if (accel_fd[0] == -1 || gyro_fd[0] == -1) {
hal.scheduler->panic("Failed to open accel/gyro 0");
AP_HAL::panic("Failed to open accel/gyro 0");
}
ioctl(gyro_fd[0], SENSORIOCSPOLLRATE, 1000);
@ -130,7 +130,7 @@ void loop(void)
struct log_ACCEL pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+i)),
time_us : hal.scheduler->micros64(),
time_us : AP_HAL::micros64(),
sample_us : accel_report.timestamp,
AccX : accel_report.x,
AccY : accel_report.y,
@ -154,7 +154,7 @@ void loop(void)
struct log_GYRO pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+i)),
time_us : hal.scheduler->micros64(),
time_us : AP_HAL::micros64(),
sample_us : gyro_report.timestamp,
GyrX : gyro_report.x,
GyrY : gyro_report.y,
@ -169,7 +169,7 @@ void loop(void)
if (total_samples[0] % 2000 == 0 && last_print != total_samples[0]) {
last_print = total_samples[0];
hal.console->printf("t=%lu total_samples=%lu/%lu/%lu adt=%u:%u/%u:%u/%u:%u gdt=%u:%u/%u:%u/%u:%u\n",
(unsigned long)hal.scheduler->millis(),
(unsigned long)AP_HAL::millis(),
(unsigned long)total_samples[0],
(unsigned long)total_samples[1],
(unsigned long)total_samples[2],
@ -183,7 +183,7 @@ void loop(void)
gyro_deltat_min[2], gyro_deltat_max[2]);
#if 0
::printf("t=%lu total_samples=%lu/%lu/%lu adt=%u:%u/%u:%u/%u:%u gdt=%u:%u/%u:%u/%u:%u\n",
hal.scheduler->millis(),
AP_HAL::millis(),
total_samples[0], total_samples[1],total_samples[2],
accel_deltat_min[0], accel_deltat_max[0],
accel_deltat_min[1], accel_deltat_max[1],