dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2248 from mcharleb/gyrosim-fix 

Gyrosim: Fixed constant looping
This commit is contained in:
Lorenz Meier 2015-06-02 01:40:49 -07:00
parent 9ad9dd13d2 2d85578599
commit 9882b78383
2 changed files with 4 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/simulator/simulator.cpp
View File

@ -63,9 +63,6 @@ Simulator *Simulator::getInstance()
bool Simulator::getMPUReport(uint8_t *buf, int len)
{
// Reads are paced from reading gyrosim and if
// we don't delay here we read too fast
usleep(50000);
return _mpu.copyData(buf, len);
}

127
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
View File

@ -259,14 +259,6 @@ private:
enum Rotation _rotation;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define GYROSIM_NUM_CHECKED_REGISTERS 9
static const uint8_t _checked_registers[GYROSIM_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[GYROSIM_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// last temperature reading for print_info()
float _last_temperature;
@ -331,14 +323,6 @@ private:
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the GYROSIM, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the GYROSIM measurement range.
*
@ -383,11 +367,6 @@ private:
*/
void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
GYROSIM(const GYROSIM&);
GYROSIM operator=(const GYROSIM&);
@ -413,22 +392,6 @@ private:
uint8_t _regdata[108];
};
/*
list of registers that will be checked in check_registers(). Note
that MPUREG_PRODUCT_ID must be first in the list.
*/
const uint8_t GYROSIM::_checked_registers[GYROSIM_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID,
MPUREG_PWR_MGMT_1,
MPUREG_USER_CTRL,
MPUREG_SMPLRT_DIV,
MPUREG_CONFIG,
MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG };
/**
* Helper class implementing the gyro driver node.
*/
@ -498,7 +461,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
_gyro_filter_y(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(GYROSIM_GYRO_DEFAULT_RATE, GYROSIM_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_rotation(rotation),
_checked_next(0),
_last_temperature(0)
{
// disable debug() calls
@ -527,7 +489,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
_checked_values[0] = _product;
}
GYROSIM::~GYROSIM()
@ -695,7 +656,7 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz)
uint8_t div = 1000 / desired_sample_rate_hz;
if(div>200) div=200;
if(div<1) div=1;
write_checked_reg(MPUREG_SMPLRT_DIV, div-1);
write_reg(MPUREG_SMPLRT_DIV, div-1);
_sample_rate = 1000 / div;
}
@ -729,7 +690,7 @@ GYROSIM::_set_dlpf_filter(uint16_t frequency_hz)
} else {
filter = BITS_DLPF_CFG_2100HZ_NOLPF;
}
write_checked_reg(MPUREG_CONFIG, filter);
write_reg(MPUREG_CONFIG, filter);
}
ssize_t
@ -1156,17 +1117,6 @@ GYROSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
write_reg(reg, val);
}
void
GYROSIM::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i=0; i<GYROSIM_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
}
}
int
GYROSIM::set_accel_range(unsigned max_g_in)
{
@ -1176,7 +1126,7 @@ GYROSIM::set_accel_range(unsigned max_g_in)
case GYROSIMES_REV_C5:
case GYROSIM_REV_C4:
case GYROSIM_REV_C5:
write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
_accel_range_scale = (GYROSIM_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * GYROSIM_ONE_G;
return OK;
@ -1204,7 +1154,7 @@ GYROSIM::set_accel_range(unsigned max_g_in)
max_accel_g = 2;
}
write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
write_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3);
_accel_range_scale = (GYROSIM_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * GYROSIM_ONE_G;
@ -1240,66 +1190,9 @@ GYROSIM::measure_trampoline(void *arg)
dev->measure();
}
void
GYROSIM::check_registers(void)
{
/*
we read the register at full speed, even though it isn't
listed as a high speed register. The low speed requirement
for some registers seems to be a propgation delay
requirement for changing sensor configuration, which should
not apply to reading a single register. It is also a better
test of VDev bus health to read at the same speed as we read
the data registers.
*/
uint8_t v;
if ((v=read_reg(_checked_registers[_checked_next], GYROSIM_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
if we get the wrong value then we know the VDev bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
// after doing a reset we need to wait a long
// time before we do any other register writes
// or we will end up with the gyrosim in a
// bizarre state where it has all correct
// register values but large offsets on the
// accel axes
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next+1) % GYROSIM_NUM_CHECKED_REGISTERS;
}
void
GYROSIM::measure()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct MPUReport mpu_report;
struct Report {
int16_t accel_x;
@ -1319,8 +1212,6 @@ GYROSIM::measure()
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
check_registers();
// sensor transfer at high clock speed
//set_frequency(GYROSIM_HIGH_BUS_SPEED);
@ -1509,16 +1400,6 @@ GYROSIM::print_info()
perf_print_counter(_reset_retries);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
PX4_WARN("checked_next: %u", _checked_next);
for (uint8_t i=0; i<GYROSIM_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i], GYROSIM_HIGH_BUS_SPEED);
if (v != _checked_values[i]) {
PX4_WARN("reg %02x:%02x should be %02x",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
}
PX4_WARN("temperature: %.1f", (double)_last_temperature);
}