Skip to content

Commit

Permalink
Timing modifications
Browse files Browse the repository at this point in the history
  • Loading branch information
@avtoku
avtoku committed Jul 8, 2024
1 parent 9836f6b commit 079c6b1
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 31 deletions.
18 changes: 11 additions & 7 deletions common/drivers/Ist8308.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ __attribute__((aligned(32))) static uint8_t ist8308_i2c_dma_buf[I2C_DMA_MAX_BUFF
__attribute__((section("my_buffers")))
__attribute__((aligned(32))) static uint8_t ist8308_fifo_rx_buffer[IST8308_FIFO_BUFFERS * sizeof(MagPacket)] = {0};

#define IST8308_OK (0x0000)
//#define IST8308_OK (0x0000)

#define WAI_REG 0x0
#define DEVICE_ID 0x08
Expand Down Expand Up @@ -183,13 +183,13 @@ uint32_t Ist8308::init(
PollingState Ist8308::state(uint64_t poll_counter)
{
uint32_t rollover = 10000; // us (100 Hz) 0-99 slots at 10 kHz
PollingStateStruct lut[] = // BARO at 50 Hz
PollingStateStruct lut[] = //
{
// at 10kHz, each count is 100us. I2C is 90us pre byte
// Mag
{0, IST8308_CMD}, // addr + 2 byte, so leave at least 3 counts
{80, IST8308_TX}, // addr + 2 bytes
{85, IST8308_RX}, // addr + 6 bytes (start at leat 84 counts after cmd
{89, IST8308_TX}, // addr + 2 bytes
{92, IST8308_RX}, // addr + 6 bytes (start at leat 84 counts after cmd
};
return PollingStateLookup(lut, sizeof(lut) / sizeof(PollingStateStruct),
poll_counter % (rollover / POLLING_PERIOD_US));
Expand All @@ -209,7 +209,7 @@ bool Ist8308::poll(uint64_t poll_counter)
else
i2cState_ = IST8308_ERROR;
}
else if (poll_state == IST8308_TX) // Write register we want to read
else if (poll_state == IST8308_TX) // Write the register we want to read
{
drdy_ = time64.Us();
ist8308_i2c_dma_buf[0] = STAT1_REG;
Expand Down Expand Up @@ -251,7 +251,7 @@ void Ist8308::endDma(void)
iflux = ((int16_t)ist8308_i2c_dma_buf[6] << 8) | (int16_t)ist8308_i2c_dma_buf[5];
p.flux[2] = -(double)iflux * 1.1515e-7; // Tesla

if (p.status == IST8308_OK)
if (p.status == STAT1_VAL_DRDY )
rxFifo_.write((uint8_t *)&p, sizeof(p));
}
i2cState_ = IDLE_STATE;
Expand All @@ -268,7 +268,11 @@ bool Ist8308::display()
misc_printf("%10.3f %10.3f %10.3f uT ", p.flux[0] * 1e6 + 10.9, p.flux[1] * 1e6 + 45.0,
p.flux[2] * 1e6 - 37.5);
misc_printf(" | ");
misc_printf(" | %7.1f C | | 0x%04X\n", p.temperature - 273.15, p.status);
misc_printf(" | N/A C | | 0x%04X", p.status);
if (p.status == STAT1_VAL_DRDY)
misc_printf(" - OK\n");
else
misc_printf(" - NOK\n");
return 1;
}
else
Expand Down
71 changes: 52 additions & 19 deletions common/drivers/Ms4525.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,10 +91,15 @@ uint32_t Ms4525::init(
PollingState Ms4525::state(uint64_t poll_counter)
{
uint32_t rollover = 10000; // us (100 Hz) 0-99 slots at 10 kHz
PollingStateStruct lut[] = // BARO at 50 Hz
PollingStateStruct lut[] = // BARO at 400 Hz
{
// at 10kHz, each count is 100us. I2C is 90us pre byte
{0, MS4525_CMDRX}, // addr + 4 bytes
{0, MS4525_CMDRXSTART}, // addr + 4 bytes
{15, MS4525_CMDRX}, // addr + 4 bytes
{30, MS4525_CMDRX}, // addr + 4 bytes
{45, MS4525_CMDRX}, // addr + 4 bytes
{60, MS4525_CMDRX}, // addr + 4 bytes
{75, MS4525_CMDRXSEND}, // addr + 4 bytes
};
return PollingStateLookup(lut, sizeof(lut) / sizeof(PollingStateStruct),
poll_counter % (rollover / POLLING_PERIOD_US));
Expand All @@ -103,9 +108,10 @@ PollingState Ms4525::state(uint64_t poll_counter)
bool Ms4525::poll(uint64_t poll_counter)
{
PollingState poll_state = state(poll_counter);
if (poll_state == MS4525_CMDRX)
if ((poll_state == MS4525_CMDRXSTART)||(poll_state == MS4525_CMDRX)||(poll_state == MS4525_CMDRXSEND))
{
launchUs_ = time64.Us();
if(poll_state == MS4525_CMDRXSTART) launchUs_ = time64.Us();

if ((dmaRunning_ =
(HAL_OK == HAL_I2C_Master_Receive_DMA(hi2c_, address_, ms4525_i2c_dma_buf,
MS4525_I2C_DMA_SIZE)))) // Receive 7 bytes of data over I2C
Expand All @@ -118,29 +124,51 @@ bool Ms4525::poll(uint64_t poll_counter)

void Ms4525::endDma(void)
{
if (i2cState_ == MS4525_CMDRX)
static float pressure_filtered = 0;

if ((i2cState_ == MS4525_CMDRXSTART)||(i2cState_ == MS4525_CMDRX)||(i2cState_ == MS4525_CMDRXSEND))
{
if ((ms4525_i2c_dma_buf[0] & 0xC0) == MS4525_OK)
{
uint32_t i_pressure = (uint32_t)(ms4525_i2c_dma_buf[0]&0x3F) << 8 | (uint32_t)ms4525_i2c_dma_buf[1];

static double pmax = 6894.76; // (=-pmin) Pa

float pressure = (((double)i_pressure - 1638.3)/6553.2 -1.0)*pmax; // Pa

const float alpha = 0.5;
pressure_filtered = alpha*pressure+(1.0-alpha)*pressure_filtered;

if (i2cState_ == MS4525_CMDRXSEND)
{
PressurePacket p;

p.pressure = pressure_filtered;

uint32_t i_temperature = ((uint32_t)ms4525_i2c_dma_buf[2] << 3 | (uint32_t)(ms4525_i2c_dma_buf[3]& 0xE0) >> 5);
p.temperature = (double)i_temperature * 200.0 / 2047.0 - 50.0 + 273.15; // K
p.status = ms4525_i2c_dma_buf[0] & 0xC0;
if (p.status == 0x00)
{

p.timestamp = time64.Us();
p.drdy = p.timestamp;
p.groupDelay = (p.timestamp - launchUs_);
uint32_t i_pressure = (uint32_t)ms4525_i2c_dma_buf[0] << 8 | (uint32_t)ms4525_i2c_dma_buf[1];
uint32_t i_temperature =
((uint32_t)ms4525_i2c_dma_buf[2] << 3 | (uint32_t)ms4525_i2c_dma_buf[3] >> 5) & 0x7FF;
p.groupDelay = (p.timestamp - launchUs_)/2;

double pmax = 6894.76; // (=-pmin) Pa

p.status = (ms4525_i2c_dma_buf[0] >> 6) & 0x0003;
p.pressure = ((double)i_pressure / 8192.0 - 1.0) * 1.25 * pmax; // Pa
p.temperature = (double)i_temperature * 200.0 / 2047.0 - 50.0 + 273.15; // K

if (p.status == MS4525_OK)
rxFifo_.write((uint8_t *)&p, sizeof(p));
}
}
}












i2cState_ = IDLE_STATE;
dmaRunning_ = false;
}
Expand All @@ -153,8 +181,13 @@ bool Ms4525::display(void)
{
misc_header(name, p.drdy, p.timestamp, p.groupDelay);
misc_printf("%10.3f Pa | | %7.1f C | "
" | 0x%04X\n",
" | 0x%04X",
p.pressure, p.temperature - 273.15, p.status);
if (p.status == MS4525_OK)
misc_printf(" - OK\n");
else
misc_printf(" - NOK\n");

return 1;
}
else
Expand Down
2 changes: 2 additions & 0 deletions common/drivers/Polling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,9 @@ typedef enum
DLHR_RX,
DLHR_ERROR,
// MS4525 Pitot
MS4525_CMDRXSTART,
MS4525_CMDRX,
MS4525_CMDRXSEND,
MS4525_ERROR,
// IST8308 Mag
IST8308_CMD,
Expand Down
7 changes: 4 additions & 3 deletions pixracer_pro/specific/Callbacks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,16 +54,17 @@ extern Time64 time64;
//
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
static uint64_t poll_counter = 0;
poll_counter++;

if (htim->Instance == POLL_HTIM_INSTANCE) // Filter out other timer interrupts.
{
static uint64_t poll_counter = 0;
poll_counter++;
varmint.baro_.poll(poll_counter);

// Mag and Pitot are on the same I2C. Avoid i2c collisions, this is not done automatically!
// Offset Pitot to avoid collision with mag. The first mag command takes around 4 time slots at 10kHz.
varmint.mag_.poll(poll_counter);
varmint.pitot_.poll(poll_counter + 4);
varmint.pitot_.poll(poll_counter - 5);

varmint.rc_.poll(); // Restart if dead
varmint.gps_.poll(); // Restart if dead
Expand Down
4 changes: 2 additions & 2 deletions varmint_10X/specific/Callbacks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,10 @@ extern Time64 time64;
//
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
static uint64_t poll_counter = 0;
poll_counter++;
if (htim->Instance == POLL_HTIM_INSTANCE) // Filter out other timer interrupts.
{
static uint64_t poll_counter = 0;
poll_counter++;
varmint.baro_.poll(poll_counter);
varmint.mag_.poll(poll_counter);
varmint.pitot_.poll(poll_counter);
Expand Down

0 comments on commit 079c6b1

Please sign in to comment.