diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/sensor_angle.c | 71 |
1 files changed, 57 insertions, 14 deletions
diff --git a/src/sensor_angle.c b/src/sensor_angle.c index c03cc957..4d35aadf 100644 --- a/src/sensor_angle.c +++ b/src/sensor_angle.c @@ -90,14 +90,21 @@ angle_check_report(struct spi_angle *sa, uint8_t oid) angle_report(sa, oid); } +// Add an entry to the measurement buffer +static void +angle_add(struct spi_angle *sa, uint_fast8_t tcode, uint_fast16_t data) +{ + sa->data[sa->data_count] = tcode; + sa->data[sa->data_count + 1] = data; + sa->data[sa->data_count + 2] = data >> 8; + sa->data_count += 3; +} + // Add an error indicator to the measurement buffer static void angle_add_error(struct spi_angle *sa, uint_fast8_t error_code) { - sa->data[sa->data_count] = TCODE_ERROR; - sa->data[sa->data_count + 1] = error_code; - sa->data[sa->data_count + 2] = 0; - sa->data_count += 3; + angle_add(sa, TCODE_ERROR, error_code); } // Add a measurement to the buffer @@ -112,10 +119,7 @@ angle_add_data(struct spi_angle *sa, uint32_t stime, uint32_t mtime angle_add_error(sa, SE_SCHEDULE); return; } - sa->data[sa->data_count] = tdiff; - sa->data[sa->data_count + 1] = angle; - sa->data[sa->data_count + 2] = angle >> 8; - sa->data_count += 3; + angle_add(sa, tdiff, angle); } // a1333 sensor query @@ -200,18 +204,22 @@ tle5012b_query(struct spi_angle *sa, uint32_t stime) uint32_t mtime = timer_read_time(); irq_enable(); - uint8_t msg[6] = { TLE_READ_LATCH, (TLE_REG_AVAL << 4) | 0x01, 0, 0, 0, 0 }; - uint8_t start_crc = 0x3f; // 0x3f == crc8(crc8(0xff, msg[0]), msg[1]) + uint8_t msg[10] = { TLE_READ_LATCH, (TLE_REG_AVAL << 4) | 0x03 }; + uint8_t crc = 0x05; // 0x05 == crc8(crc8(0xff, msg[0]), msg[1]) spidev_transfer(sa->spi, 1, sizeof(msg), msg); - uint8_t crc = ~crc8(crc8(start_crc, msg[2]), msg[3]); - if (crc != msg[5]) + int i; + for (i=2; i<8; i++) + crc = crc8(crc, msg[i]); + if (((~crc) & 0xff) != msg[9]) angle_add_error(sa, SE_CRC); - else if (!(msg[4] & (1<<4))) + else if (!(msg[8] & (1<<4))) angle_add_error(sa, SE_NO_ANGLE); else if (!(msg[2] & 0x80)) angle_add_error(sa, SE_DUP); + else if (mtime - stime > timer_from_us(32 * 32 * 1000000UL / 750000)) + angle_add_error(sa, SE_SCHEDULE); else - angle_add_data(sa, stime, mtime, (msg[2] << 9) | (msg[3] << 1)); + angle_add(sa, (msg[6] >> 1) & 0x3f, (msg[2] << 9) | (msg[3] << 1)); } void @@ -240,6 +248,41 @@ command_query_spi_angle(uint32_t *args) DECL_COMMAND(command_query_spi_angle, "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c"); +void +command_spi_angle_transfer(uint32_t *args) +{ + uint8_t oid = args[0]; + struct spi_angle *sa = oid_lookup(oid, command_config_spi_angle); + uint8_t data_len = args[1]; + uint8_t *data = command_decode_ptr(args[2]); + uint32_t mtime; + uint_fast8_t chip = sa->chip_type; + if (chip == SA_CHIP_TLE5012B) { + // Latch data (data is latched on rising CS of a NULL message) + struct gpio_out cs_pin = spidev_get_cs_pin(sa->spi); + gpio_out_write(cs_pin, 0); + udelay(1); + irq_disable(); + gpio_out_write(cs_pin, 1); + mtime = timer_read_time(); + irq_enable(); + spidev_transfer(sa->spi, 1, data_len, data); + } else { + uint32_t mtime1 = timer_read_time(); + spidev_transfer(sa->spi, 1, data_len, data); + uint32_t mtime2 = timer_read_time(); + if (mtime2 - mtime1 > MAX_SPI_READ_TIME) + data_len = 0; + if (chip == SA_CHIP_AS5047D) + mtime = mtime2; + else + mtime = mtime1; + } + sendf("spi_angle_transfer_response oid=%c clock=%u response=%*s" + , oid, mtime, data_len, data); +} +DECL_COMMAND(command_spi_angle_transfer, "spi_angle_transfer oid=%c data=%*s"); + // Background task that performs measurements void spi_angle_task(void) |