diff --git a/conf/modules/encoder_amt22.xml b/conf/modules/encoder_amt22.xml new file mode 100644 index 0000000000..83811448be --- /dev/null +++ b/conf/modules/encoder_amt22.xml @@ -0,0 +1,33 @@ + + + + Driver for AMT22 encoder from CUI devices. + + + spi_master + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/humid_sht_i2c.xml b/conf/modules/humid_sht_i2c.xml index a956442058..c2b1c9ff7e 100644 --- a/conf/modules/humid_sht_i2c.xml +++ b/conf/modules/humid_sht_i2c.xml @@ -3,18 +3,14 @@ Sensirion SHT25 humidity sensor (I2C) - +
- - - - diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c index 23eb7d6fdb..7bba45ecd5 100644 --- a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c @@ -235,6 +235,7 @@ static void handle_i2c_thd(struct i2c_periph *p) } i2cReleaseBus((I2CDriver *)p->reg_addr); + pprz_bsem_signal(&t->bsem); } /** @@ -440,6 +441,7 @@ static bool i2c_chibios_submit(struct i2c_periph *p, struct i2c_transaction *t) p->trans_insert_idx = temp; chSysUnlock(); + pprz_bsem_init(&t->bsem, true); chSemSignal(&((struct i2c_init *)p->init_struct)->sem); // transaction submitted return TRUE; diff --git a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c index 1c75dd53d2..c6e26d5676 100644 --- a/sw/airborne/arch/chibios/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/spi_arch.c @@ -389,6 +389,7 @@ static void handle_spi_thd(struct spi_periph *p) t->after_cb(t); } + pprz_bsem_signal(&t->bsem); } /** @@ -552,6 +553,7 @@ bool spi_submit(struct spi_periph *p, struct spi_transaction *t) p->trans_insert_idx = idx; chSysUnlock(); + pprz_bsem_init(&t->bsem, true); chSemSignal(&((struct spi_init *)p->init_struct)->sem); // transaction submitted return TRUE; diff --git a/sw/airborne/arch/chibios/modules/core/threads_arch.c b/sw/airborne/arch/chibios/modules/core/threads_arch.c index ddd093e674..57811c5cf0 100644 --- a/sw/airborne/arch/chibios/modules/core/threads_arch.c +++ b/sw/airborne/arch/chibios/modules/core/threads_arch.c @@ -44,6 +44,16 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) { chBSemWait(&bsem->bsem); } +int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) { + sysinterval_t chtimeout; + if(timeout < 0.002) { + chtimeout = chTimeUS2I(timeout*1e6); + } else { + chtimeout = chTimeMS2I(timeout*1e3); + } + return chBSemWaitTimeout(&bsem->bsem, chtimeout); +} + void pprz_bsem_signal(pprz_bsem_t* bsem) { chBSemSignal(&bsem->bsem); } diff --git a/sw/airborne/arch/linux/mcu_periph/i2c_arch.c b/sw/airborne/arch/linux/mcu_periph/i2c_arch.c index 706a05ace3..41774e7735 100644 --- a/sw/airborne/arch/linux/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/i2c_arch.c @@ -104,6 +104,7 @@ static bool i2c_linux_submit(struct i2c_periph *p, struct i2c_transaction *t) p->trans[p->trans_insert_idx] = t; p->trans_insert_idx = next_idx; + pprz_bsem_init(&t->bsem, true); /* wake handler thread */ pthread_cond_signal(condition); pthread_mutex_unlock(mutex); @@ -199,6 +200,7 @@ static void *i2c_thread(void *data) pthread_mutex_lock(mutex); p->trans_extract_idx = (p->trans_extract_idx + 1) % I2C_TRANSACTION_QUEUE_LEN; pthread_mutex_unlock(mutex); + pprz_bsem_signal(&t->bsem); } return NULL; } diff --git a/sw/airborne/arch/linux/modules/core/threads_arch.c b/sw/airborne/arch/linux/modules/core/threads_arch.c index a4c1e22de7..e5d14975f3 100644 --- a/sw/airborne/arch/linux/modules/core/threads_arch.c +++ b/sw/airborne/arch/linux/modules/core/threads_arch.c @@ -15,6 +15,7 @@ #include "stdbool.h" #include #include +#include int pprz_mtx_init(pprz_mutex_t* mtx) { @@ -44,6 +45,16 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) { sem_wait(&bsem->sem); } +int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + time_t timeout_secs = (time_t)timeout; + long timeout_nsecs = (timeout - timeout_secs) * 1e9; + ts.tv_sec += timeout_secs; + ts.tv_nsec += timeout_nsecs; + return sem_timedwait(&bsem->sem, &ts); +} + void pprz_bsem_signal(pprz_bsem_t* bsem) { int val; pthread_mutex_lock(&bsem->mtx); diff --git a/sw/airborne/arch/stm32/modules/core/threads_arch.c b/sw/airborne/arch/stm32/modules/core/threads_arch.c index fab9ba91a8..ba9a02cc66 100644 --- a/sw/airborne/arch/stm32/modules/core/threads_arch.c +++ b/sw/airborne/arch/stm32/modules/core/threads_arch.c @@ -51,6 +51,18 @@ void pprz_bsem_wait(pprz_bsem_t* bsem) { bsem->value = 0; } +int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout) { + float time_end = get_sys_time_float() + timeout; + while(get_sys_time_float() - time_end > 0) { + // active wait + if(bsem->value) { + bsem->value = 0; + return 0; + } + } + return -1; +} + void pprz_bsem_signal(pprz_bsem_t* bsem) { bsem->value = 1; } diff --git a/sw/airborne/boards/bebop/actuators.c b/sw/airborne/boards/bebop/actuators.c index d1eae6e314..08c2422232 100644 --- a/sw/airborne/boards/bebop/actuators.c +++ b/sw/airborne/boards/bebop/actuators.c @@ -74,7 +74,7 @@ void actuators_bebop_commit(void) { // Receive the status actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA; - i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13); + i2c_blocking_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13, 0.5); // Update status electrical.vsupply = (float)(actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 1000.f; @@ -94,7 +94,7 @@ void actuators_bebop_commit(void) if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_get_motors_on()) { // Reset the error actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR; - i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1); + i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 0.5); // Start the motors actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP; @@ -104,12 +104,12 @@ void actuators_bebop_commit(void) #else actuators_bebop.i2c_trans.buf[1] = 0b00000101; #endif - i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2); + i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, 0.5); } // Stop the motors else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_get_motors_on()) { actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP; - i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1); + i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 0.5); } else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_get_motors_on()) { // Send the commands actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED; @@ -126,14 +126,14 @@ void actuators_bebop_commit(void) #pragma GCC diagnostic ignored "-Wcast-qual" actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9); #pragma GCC diagnostic pop - i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11); + i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11, 0.5); } // Update the LEDs if (actuators_bebop.led != (led_hw_values & 0x3)) { actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_TOGGLE_GPIO; actuators_bebop.i2c_trans.buf[1] = (led_hw_values & 0x3); - i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2); + i2c_blocking_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2, 0.5); actuators_bebop.led = led_hw_values & 0x3; } diff --git a/sw/airborne/boards/bebop/mt9f002.c b/sw/airborne/boards/bebop/mt9f002.c index a0744a3409..9f2a6f469c 100644 --- a/sw/airborne/boards/bebop/mt9f002.c +++ b/sw/airborne/boards/bebop/mt9f002.c @@ -194,7 +194,7 @@ static void write_reg(struct mt9f002_t *mt, uint16_t addr, uint32_t val, uint8_t } // Transmit the buffer - i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, len + 2); + i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, len + 2, 0.5); } /** @@ -207,7 +207,7 @@ static uint32_t read_reg(struct mt9f002_t *mt, uint16_t addr, uint8_t len) mt->i2c_trans.buf[1] = addr & 0xFF; // Transmit the buffer and receive back - i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len); + i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9F002_ADDRESS, 2, len, 0.5); /* Fix signdness */ for (uint8_t i = 0; i < len; i++) { diff --git a/sw/airborne/boards/bebop/mt9v117.c b/sw/airborne/boards/bebop/mt9v117.c index 0d2d8a5a3a..46ab8804d5 100644 --- a/sw/airborne/boards/bebop/mt9v117.c +++ b/sw/airborne/boards/bebop/mt9v117.c @@ -231,7 +231,7 @@ static void write_reg(struct mt9v117_t *mt, uint16_t addr, uint32_t val, uint16_ } // Transmit the buffer - i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, len + 2); + i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, len + 2, 0.5); } /** @@ -244,7 +244,7 @@ static uint32_t read_reg(struct mt9v117_t *mt, uint16_t addr, uint16_t len) mt->i2c_trans.buf[1] = addr & 0xFF; // Transmit the buffer and receive back - i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, 2, len); + i2c_blocking_transceive(mt->i2c_periph, &mt->i2c_trans, MT9V117_ADDRESS, 2, len, 0.5); /* Fix sigdness */ for (uint8_t i = 0; i < len; i++) { @@ -302,7 +302,7 @@ static inline void mt9v117_write_patch(struct mt9v117_t *mt) } // Transmit the buffer - i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, mt9v117_patch_lines[i].len); + i2c_blocking_transmit(mt->i2c_periph, &mt->i2c_trans, mt->i2c_trans.slave_addr, mt9v117_patch_lines[i].len, 0.5); } write_reg(mt, MT9V117_LOGICAL_ADDRESS_ACCESS, 0x0000, 2); diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c index 47db6d574a..45b4cc850a 100644 --- a/sw/airborne/boards/disco/actuators.c +++ b/sw/airborne/boards/disco/actuators.c @@ -119,7 +119,7 @@ void actuators_disco_commit(void) // Receive the status actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_GET_OBS_DATA; - i2c_blocking_transceive(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, sizeof(obs_data)); + i2c_blocking_transceive(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, sizeof(obs_data), 0.5); // copy data from buffer #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" @@ -149,17 +149,17 @@ void actuators_disco_commit(void) actuators_disco.motor_rpm > DISCO_BLDC_START_MOTOR_THRESHOLD) { // Reset the error actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_CLEAR_ERROR; - i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5); // Start the motors actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_START_PROP; - i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5); } // Stop the motors else if ((bldc_status == DISCO_BLDC_STATUS_RUNNING || bldc_status == DISCO_BLDC_STATUS_RAMPUP) && actuators_disco.motor_rpm < DISCO_BLDC_START_MOTOR_THRESHOLD) { actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_STOP_PROP; - i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, 0.5); } else if (bldc_status == DISCO_BLDC_STATUS_RUNNING) { // Send the commands actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_SET_REF_SPEED; @@ -170,7 +170,7 @@ void actuators_disco_commit(void) #pragma GCC diagnostic ignored "-Wcast-qual" actuators_disco.i2c_trans.buf[4] = actuators_disco_checksum((uint8_t *)actuators_disco.i2c_trans.buf, 3); #pragma GCC diagnostic pop - i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 11); + i2c_blocking_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 11, 0.5); } // Send ABI message diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index 3bffe9017b..31961e430f 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -229,73 +229,45 @@ bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, return i2c_submit(p, t); } -/** Default timeout for blocking I2C transactions */ -#ifndef I2C_BLOCKING_TIMEOUT -#define I2C_BLOCKING_TIMEOUT 1.f -#endif +static enum I2CTransactionStatus i2c_blocking_submit(struct i2c_periph *p, struct i2c_transaction *t, float timeout) { + if (!i2c_submit(p, t)) { + return I2CTransFailed; + } -bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint8_t len) + // Wait for transaction to complete + if(pprz_bsem_wait_timeout(&t->bsem, timeout) == 0) { + return t->status; + } else { + return I2CTransFailed; + } +} + +enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len, float timeout) { t->type = I2CTransTx; t->slave_addr = s_addr; t->len_w = len; t->len_r = 0; - if (!i2c_submit(p, t)) { - return false; - } - - // Wait for transaction to complete - float start_t = get_sys_time_float(); - while (t->status == I2CTransPending || t->status == I2CTransRunning) { - if (p->spin) p->spin(p); - if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) { - break; // timeout after 1 second - } - } - return true; + return i2c_blocking_submit(p, t, timeout); } -bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint16_t len) +enum I2CTransactionStatus i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint16_t len, float timeout) { t->type = I2CTransRx; t->slave_addr = s_addr; t->len_w = 0; t->len_r = len; - if (!i2c_submit(p, t)) { - return false; - } - - // Wait for transaction to complete - float start_t = get_sys_time_float(); - while (t->status == I2CTransPending || t->status == I2CTransRunning) { - if (p->spin) p->spin(p); - if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) { - break; // timeout after 1 second - } - } - return true; + return i2c_blocking_submit(p, t, timeout); } -bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint8_t len_w, uint16_t len_r) +enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout) { t->type = I2CTransTxRx; t->slave_addr = s_addr; t->len_w = len_w; t->len_r = len_r; - if (!i2c_submit(p, t)) { - return false; - } - - // Wait for transaction to complete - float start_t = get_sys_time_float(); - while (t->status == I2CTransPending || t->status == I2CTransRunning) { - if (p->spin) p->spin(p); - if (get_sys_time_float() - start_t > I2C_BLOCKING_TIMEOUT) { - break; // timeout after 1 second - } - } - return true; + return i2c_blocking_submit(p, t, timeout); } diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h index 27ef685cde..ae7af74175 100644 --- a/sw/airborne/mcu_periph/i2c.h +++ b/sw/airborne/mcu_periph/i2c.h @@ -33,6 +33,7 @@ #include "std.h" #include "mcu_periph/i2c_arch.h" +#include "modules/core/threads.h" /** * @addtogroup mcu_periph @@ -124,6 +125,10 @@ struct i2c_transaction { /** Transaction status. */ volatile enum I2CTransactionStatus status; + + /** binary semaphore for blocking functions + */ + pprz_bsem_t bsem; }; /** I2C transaction queue length. @@ -328,10 +333,11 @@ extern bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, * @param t i2c transaction * @param s_addr slave address * @param len number of bytes to transmit - * @return TRUE if insertion to the transaction queue succeeded + * @param timeout timeout in seconds after which the transaction is considered failed + * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed */ -bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint8_t len); +enum I2CTransactionStatus i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len, float timeout); /** Submit a read only transaction and wait for it to complete. * Convenience function which is usually preferred over i2c_submit, @@ -340,10 +346,11 @@ bool i2c_blocking_transmit(struct i2c_periph *p, struct i2c_transaction *t, * @param t i2c transaction * @param s_addr slave address * @param len number of bytes to receive - * @return TRUE if insertion to the transaction queue succeeded + * @param timeout timeout in seconds after which the transaction is considered failed + * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed */ -bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint16_t len); +enum I2CTransactionStatus i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint16_t len, float timeout); /** Submit a write/read transaction and wait for it to complete. * Convenience function which is usually preferred over i2c_submit, @@ -353,10 +360,11 @@ bool i2c_blocking_receive(struct i2c_periph *p, struct i2c_transaction *t, * @param s_addr slave address * @param len_w number of bytes to transmit * @param len_r number of bytes to receive - * @return TRUE if insertion to the transaction queue succeeded + * @param timeout timeout in seconds after which the transaction is considered failed + * @return the status of the transaction, or I2CTransFailed if insertion to the transaction queue failed */ -bool i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, - uint8_t s_addr, uint8_t len_w, uint16_t len_r); +enum I2CTransactionStatus i2c_blocking_transceive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len_w, uint16_t len_r, float timeout); /** @}*/ /** @}*/ diff --git a/sw/airborne/mcu_periph/spi.c b/sw/airborne/mcu_periph/spi.c index ef5327d89b..d46ca6ed90 100644 --- a/sw/airborne/mcu_periph/spi.c +++ b/sw/airborne/mcu_periph/spi.c @@ -164,3 +164,17 @@ extern void spi_slave_init(struct spi_periph *p) #endif /* SPI_SLAVE */ + + +enum SPITransactionStatus spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t, float timeout) { + if (!spi_submit(p, t)) { + return SPITransFailed; + } + + // Wait for transaction to complete + if(pprz_bsem_wait_timeout(&t->bsem, timeout) == 0) { + return t->status; + } else { + return SPITransFailed; + } +} \ No newline at end of file diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index f6f8ed06ec..e8f6b4e4de 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -35,11 +35,7 @@ #include "mcu_periph/spi_arch.h" #include "mcu_periph/sys_time.h" - -#ifndef SPI_BLOCKING_TIMEOUT -#define SPI_BLOCKING_TIMEOUT 1.f -#endif - +#include "modules/core/threads.h" /** * @addtogroup mcu_periph @@ -160,6 +156,7 @@ struct spi_transaction { SPICallback before_cb; ///< NULL or function called before the transaction SPICallback after_cb; ///< NULL or function called after the transaction volatile enum SPITransactionStatus status; + pprz_bsem_t bsem; }; /** SPI transaction queue length. @@ -294,21 +291,10 @@ extern bool spi_submit(struct spi_periph *p, struct spi_transaction *t); /** Perform a spi transaction (blocking). * @param p spi peripheral to be used * @param t spi transaction - * @return TRUE if transaction completed (success or failure) + * @param timeout timeout in seconds after which the transaction is considered failed + * @return the status of the transaction, or SPITransFailed if insertion to the transaction queue failed */ -static inline bool spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t) { - if (!spi_submit(p, t)) { - return false; - } - // Wait for transaction to complete - float start_t = get_sys_time_float(); - while (t->status == SPITransPending || t->status == SPITransRunning) { - if (get_sys_time_float() - start_t > SPI_BLOCKING_TIMEOUT) { - break; - } - } - return true; -} +enum SPITransactionStatus spi_blocking_transceive(struct spi_periph *p, struct spi_transaction *t, float timeout); /** Select a slave. * @param slave slave id diff --git a/sw/airborne/modules/core/threads.h b/sw/airborne/modules/core/threads.h index 1feee7db7a..b8c32e5c07 100644 --- a/sw/airborne/modules/core/threads.h +++ b/sw/airborne/modules/core/threads.h @@ -34,6 +34,14 @@ int pprz_mtx_unlock(pprz_mutex_t* mtx); void pprz_bsem_init(pprz_bsem_t* bsem, bool taken); void pprz_bsem_wait(pprz_bsem_t* bsem); + +/** + * @brief Wait on semaphore no more than timeout. + * @param timeout in seconds + * @returns 0 on success + */ +int pprz_bsem_wait_timeout(pprz_bsem_t* bsem, float timeout); + void pprz_bsem_signal(pprz_bsem_t* bsem); /** diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c index 85e8a312db..b4ec87b5b0 100644 --- a/sw/airborne/modules/meteo/humid_sht_i2c.c +++ b/sw/airborne/modules/meteo/humid_sht_i2c.c @@ -27,9 +27,6 @@ #include "modules/meteo/humid_sht_i2c.h" - -#include "mcu_periph/i2c.h" -#include "mcu_periph/uart.h" #include "pprzlink/messages.h" #include "modules/datalink/downlink.h" @@ -40,14 +37,146 @@ #define SHT_SLAVE_ADDR 0x80 -struct i2c_transaction sht_trans; -uint8_t sht_status; -uint8_t sht_serial[8] = {0}; -uint32_t sht_serial1 = 0, sht_serial2 = 0; -uint16_t humidsht_i2c, tempsht_i2c; -float fhumidsht_i2c, ftempsht_i2c; +#define SHT2_WRITE_USER 0xE6 +#define SHT2_READ_USER 0xE7 +#define SHT2_TRIGGER_TEMP 0xF3 +#define SHT2_TRIGGER_HUMID 0xF5 +#define SHT2_SOFT_RESET 0xFE -int8_t humid_sht_crc(volatile uint8_t *data) +static void humid_sht_thd(void* arg); +static void sht_read_serial(struct sht_humid_t* sht); +static int sht_read_temp(struct sht_humid_t* sht); +static int sht_read_humid(struct sht_humid_t* sht); +static int8_t humid_sht_crc(volatile uint8_t *data); + + + +static struct sht_humid_t sht; + + +void humid_sht_init_i2c(void) +{ + sht.sht_status = SHT2_UNINIT; + pprz_bsem_init(&sht.bsem_sht_status, true); + pprz_thread_create(&sht.thd_handle, 512, "humid_sht25", PPRZ_NORMAL_PRIO+1, humid_sht_thd, &sht); +} + +void humid_sht_periodic_i2c(void) +{ + if(sht.sht_status == SHT2_READING) { + /* send serial number every 30 seconds */ + RunOnceEvery((4 * 30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht.sht_serial1, &sht.sht_serial2)); + } + + if(pprz_bsem_wait_timeout(&sht.bsem_sht_status, 0) == 0) { + DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &sht.humidsht_i2c, &sht.tempsht_i2c, &sht.fhumidsht_i2c, &sht.ftempsht_i2c); + } +} + + +static void humid_sht_thd(void* arg) { + sys_time_usleep(100000); + struct sht_humid_t* sht = (struct sht_humid_t*)arg; + + /* soft reset sensor */ + sht->sht_trans.buf[0] = SHT2_SOFT_RESET; + while(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess) { + sys_time_usleep(100000); + } + + /* read serial number */ + sht_read_serial(sht); + + sht->sht_status = SHT2_READING; + + while(true) { + /* read temperature */ + if(sht_read_temp(sht)) {continue;} + /* read humidity */ + if(sht_read_humid(sht)) {continue;} + /* signal semaphore to handle data */ + pprz_bsem_signal(&sht->bsem_sht_status); + + sys_time_usleep(500000); + } +} + + +static void sht_read_serial(struct sht_humid_t* sht) { + uint8_t sht_serial[8] = {0}; + + /* request serial number part 1 */ + sht->sht_trans.buf[0] = 0xFA; + sht->sht_trans.buf[1] = 0x0F; + while(i2c_blocking_transceive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 2, 8, 0.5) != I2CTransSuccess) { + sys_time_usleep(10000); + } + /* read serial number part 1 */ + sht_serial[5] = sht->sht_trans.buf[0]; + sht_serial[4] = sht->sht_trans.buf[2]; + sht_serial[3] = sht->sht_trans.buf[4]; + sht_serial[2] = sht->sht_trans.buf[6]; + + /* request serial number part 2 */ + sht->sht_trans.buf[0] = 0xFC; + sht->sht_trans.buf[1] = 0xC9; + while(i2c_blocking_transceive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 2, 6, 0.5) != I2CTransSuccess) { + sys_time_usleep(10000); + } + /* read serial number part 2 */ + sht_serial[1] = sht->sht_trans.buf[0]; + sht_serial[0] = sht->sht_trans.buf[1]; + sht_serial[7] = sht->sht_trans.buf[3]; + sht_serial[6] = sht->sht_trans.buf[4]; + + sht->sht_serial1 = sht_serial[7] << 24 | sht_serial[6] << 16 | sht_serial[5] << 8 | sht_serial[4]; + sht->sht_serial2 = sht_serial[3] << 24 | sht_serial[2] << 16 | sht_serial[1] << 8 | sht_serial[0]; +} + +static int sht_read_temp(struct sht_humid_t* sht) { + /* trigger temp measurement, no master hold */ + sht->sht_trans.buf[0] = SHT2_TRIGGER_TEMP; + if(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess ) { + return -1; + } + /* needs 85ms delay from temp trigger measurement */ + sys_time_usleep(85000); + /* read temperature */ + if(i2c_blocking_receive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 3, 0.5) != I2CTransSuccess) { + return -1; + } + if (humid_sht_crc(sht->sht_trans.buf) != 0) { + /* checksum error*/ + return -1; + } + + sht->tempsht_i2c = ((sht->sht_trans.buf[0] << 8) | sht->sht_trans.buf[1]) & 0xFFFC; + sht->ftempsht_i2c = -46.85 + 175.72 / 65536. * sht->tempsht_i2c; + return 0; +} + +static int sht_read_humid(struct sht_humid_t* sht) { + /* trigger humid measurement, no master hold */ + sht->sht_trans.buf[0] = SHT2_TRIGGER_HUMID; + if(i2c_blocking_transmit(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 1, 0.5) != I2CTransSuccess) { + return -1; + } + /* needs 29ms delay from humid trigger measurement */ + sys_time_usleep(29000); + /* read humidity */ + if(i2c_blocking_receive(&SHT_I2C_DEV, &sht->sht_trans, SHT_SLAVE_ADDR, 3, 0.5) != I2CTransSuccess) { + return -1; + } + if (humid_sht_crc(sht->sht_trans.buf) != 0) { + /* checksum error */ + return -1; + } + sht->humidsht_i2c = ((sht->sht_trans.buf[0] << 8) | sht->sht_trans.buf[1]) & 0xFFFC; + sht->fhumidsht_i2c = -6. + 125. / 65536. * sht->humidsht_i2c; + return 0; +} + +static int8_t humid_sht_crc(volatile uint8_t *data) { uint8_t i, bit, crc = 0; @@ -67,147 +196,3 @@ int8_t humid_sht_crc(volatile uint8_t *data) return 0; } } - -void humid_sht_init_i2c(void) -{ - sht_status = SHT2_UNINIT; -} - -void humid_sht_periodic_i2c(void) -{ - switch (sht_status) { - - case SHT2_UNINIT: - /* do soft reset, then wait at least 15ms */ - sht_status = SHT2_RESET; - sht_trans.buf[0] = SHT2_SOFT_RESET; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - break; - - case SHT2_SERIAL: - /* get serial number part 1 */ - sht_status = SHT2_SERIAL1; - sht_trans.buf[0] = 0xFA; - sht_trans.buf[1] = 0x0F; - i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 8); - break; - - case SHT2_SERIAL1: - case SHT2_SERIAL2: - break; - - default: - /* trigger temp measurement, no master hold */ - sht_trans.buf[0] = SHT2_TRIGGER_TEMP; - sht_status = SHT2_TRIG_TEMP; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - /* send serial number every 30 seconds */ - RunOnceEvery((4 * 30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2)); - break; - } -} - -/* needs 85ms delay from temp trigger measurement */ -void humid_sht_p_temp(void) -{ - if (sht_status == SHT2_GET_TEMP) { - /* get temp */ - sht_status = SHT2_READ_TEMP; - i2c_receive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 3); - } -} - -/* needs 29ms delay from humid trigger measurement */ -void humid_sht_p_humid(void) -{ - if (sht_status == SHT2_GET_HUMID) { - /* read humid */ - sht_status = SHT2_READ_HUMID; - i2c_receive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 3); - } -} - -void humid_sht_event_i2c(void) -{ - if (sht_trans.status == I2CTransSuccess) { - switch (sht_status) { - - case SHT2_TRIG_TEMP: - sht_status = SHT2_GET_TEMP; - sht_trans.status = I2CTransDone; - break; - - case SHT2_READ_TEMP: - /* read temperature */ - tempsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; - tempsht_i2c &= 0xFFFC; - if (humid_sht_crc(sht_trans.buf) == 0) { - /* trigger humid measurement, no master hold */ - sht_trans.buf[0] = SHT2_TRIGGER_HUMID; - sht_status = SHT2_TRIG_HUMID; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - } else { - /* checksum error, restart */ - sht_status = SHT2_IDLE; - sht_trans.status = I2CTransDone; - } - break; - - case SHT2_TRIG_HUMID: - sht_status = SHT2_GET_HUMID; - sht_trans.status = I2CTransDone; - break; - - case SHT2_READ_HUMID: - /* read humidity */ - humidsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; - humidsht_i2c &= 0xFFFC; - fhumidsht_i2c = -6. + 125. / 65536. * humidsht_i2c; - ftempsht_i2c = -46.85 + 175.72 / 65536. * tempsht_i2c; - - sht_status = SHT2_IDLE; - sht_trans.status = I2CTransDone; - - if (humid_sht_crc(sht_trans.buf) == 0) { - DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c); - } - break; - - case SHT2_RESET: - sht_status = SHT2_SERIAL; - sht_trans.status = I2CTransDone; - break; - - case SHT2_SERIAL1: - /* read serial number part 1 */ - sht_serial[5] = sht_trans.buf[0]; - sht_serial[4] = sht_trans.buf[2]; - sht_serial[3] = sht_trans.buf[4]; - sht_serial[2] = sht_trans.buf[6]; - /* get serial number part 2 */ - sht_status = SHT2_SERIAL2; - sht_trans.buf[0] = 0xFC; - sht_trans.buf[1] = 0xC9; - i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 6); - break; - - case SHT2_SERIAL2: - /* read serial number part 2 */ - sht_serial[1] = sht_trans.buf[0]; - sht_serial[0] = sht_trans.buf[1]; - sht_serial[7] = sht_trans.buf[3]; - sht_serial[6] = sht_trans.buf[4]; - sht_serial1 = sht_serial[7] << 24 | sht_serial[6] << 16 | sht_serial[5] << 8 | sht_serial[4]; - sht_serial2 = sht_serial[3] << 24 | sht_serial[2] << 16 | sht_serial[1] << 8 | sht_serial[0]; - DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2); - sht_status = SHT2_IDLE; - sht_trans.status = I2CTransDone; - break; - - default: - sht_trans.status = I2CTransDone; - break; - } - } -} - diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.h b/sw/airborne/modules/meteo/humid_sht_i2c.h index 0669bb2948..fc3b398ef5 100644 --- a/sw/airborne/modules/meteo/humid_sht_i2c.h +++ b/sw/airborne/modules/meteo/humid_sht_i2c.h @@ -1,39 +1,32 @@ #ifndef HUMID_SHT_I2C_H #define HUMID_SHT_I2C_H +#include "mcu_periph/i2c.h" +#include "modules/core/threads.h" #include "std.h" -#define SHT2_WRITE_USER 0xE6 -#define SHT2_READ_USER 0xE7 -#define SHT2_TRIGGER_TEMP 0xF3 -#define SHT2_TRIGGER_HUMID 0xF5 -#define SHT2_SOFT_RESET 0xFE - enum sht_stat_i2c { SHT2_UNINIT, - SHT2_IDLE, - SHT2_RESET, - SHT2_SERIAL, - SHT2_SERIAL1, - SHT2_SERIAL2, - SHT2_SET_CONFIG, - SHT2_READ_SERIAL, - SHT2_TRIG_TEMP, - SHT2_GET_TEMP, - SHT2_READ_TEMP, - SHT2_TRIG_HUMID, - SHT2_GET_HUMID, - SHT2_READ_HUMID + SHT2_READING, }; -int8_t humid_sht_crc(volatile uint8_t *data); + +struct sht_humid_t { + struct i2c_transaction sht_trans; + enum sht_stat_i2c sht_status; + uint32_t sht_serial1; + uint32_t sht_serial2; + uint16_t humidsht_i2c; + uint16_t tempsht_i2c; + float fhumidsht_i2c; + float ftempsht_i2c; + + pprz_thread_t thd_handle; + pprz_bsem_t bsem_sht_status; +}; + + void humid_sht_init_i2c(void); void humid_sht_periodic_i2c(void); -void humid_sht_p_temp(void); -void humid_sht_p_humid(void); -void humid_sht_event_i2c(void); - -extern uint16_t humidsht_i2c, tempsht_i2c; -extern float fhumidsht_i2c, ftempsht_i2c; #endif diff --git a/sw/airborne/modules/sensors/encoder_amt22.c b/sw/airborne/modules/sensors/encoder_amt22.c new file mode 100644 index 0000000000..01491ee402 --- /dev/null +++ b/sw/airborne/modules/sensors/encoder_amt22.c @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2025 Flo&Fab + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/sensors/encoder_amt22.c" + * @author Flo&Fab + * Driver for AMT22 encoder from CUI devices. + */ + +#include "modules/sensors/encoder_amt22.h" +#include "peripherals/amt22.h" +#include "modules/datalink/downlink.h" + +static amt22_t amt22; +static amt22_config_t amt22_conf = { + .p = &AMT22_SPI_DEV, + .slave_idx = AMT22_SPI_SLAVE_IDX, + .type = AMT22_12_SINGLE, +}; + +void encoder_amt22_init(void) +{ + amt22_init(&amt22, &amt22_conf); +} + +void encoder_amt22_periodic(void) +{ + amt22_periodic(&amt22); + float f[2] = {amt22_get_position(&amt22), amt22_get_turns(&amt22)}; + DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 2, f); +} diff --git a/sw/airborne/modules/sensors/encoder_amt22.h b/sw/airborne/modules/sensors/encoder_amt22.h new file mode 100644 index 0000000000..5448ba3256 --- /dev/null +++ b/sw/airborne/modules/sensors/encoder_amt22.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2025 Flo&Fab + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** @file "modules/sensors/encoder_amt22.h" + * @author Flo&Fab + * Driver for AMT22 encoder from CUI devices. + */ + +#ifndef ENCODER_AMT22_H +#define ENCODER_AMT22_H + +extern void encoder_amt22_init(void); +extern void encoder_amt22_periodic(void); + +#endif // ENCODER_AMT22_H diff --git a/sw/airborne/peripherals/amt22.c b/sw/airborne/peripherals/amt22.c new file mode 100644 index 0000000000..e6f899071f --- /dev/null +++ b/sw/airborne/peripherals/amt22.c @@ -0,0 +1,133 @@ +#include "amt22.h" + + +#ifndef AMT22_SPI_CDIV +#define AMT22_SPI_CDIV SPIDiv256 +#endif + +static bool amt22_checkbit(uint8_t p0, uint8_t p1); + +static void amt22_thd(void* arg); + +void amt22_init(amt22_t* amt, amt22_config_t* conf) { + amt->config = conf; + // Set up SPI peripheral and transaction + amt->trans.slave_idx = amt->config->slave_idx; + amt->trans.input_buf = amt->spi_input_buf; + amt->trans.output_buf = amt->spi_output_buf; + amt->trans.select = SPISelectUnselect; + amt->trans.cpol = SPICpolIdleLow; + amt->trans.cpha = SPICphaEdge1; + amt->trans.dss = SPIDss8bit; + amt->trans.bitorder = SPIMSBFirst; + amt->trans.cdiv = AMT22_SPI_CDIV; + amt->trans.before_cb = NULL; + amt->trans.after_cb = NULL; + amt->trans.status = SPITransDone; + amt->position = 0; + amt->turns = 0; + + + if(amt->config->type == AMT22_12_SINGLE || amt->config->type == AMT22_14_SINGLE) { + //single turn + amt->trans.output_buf[0] = 0x00; + amt->trans.output_buf[1] = 0x00; + amt->trans.output_length = 2; + amt->trans.input_length = 2; + } else { + //multi_turn + amt->trans.output_buf[0] = 0x00; + amt->trans.output_buf[1] = 0xA0; + amt->trans.output_buf[2] = 0x00; + amt->trans.output_buf[3] = 0x00; + amt->trans.output_length = 4; + amt->trans.input_length = 4; + } + + pprz_bsem_init(&amt->bsem_amt22_read, true); + pprz_mtx_init(&amt->mtx); + pprz_thread_create(&amt->thd_handle, 512, "amt22", PPRZ_NORMAL_PRIO+1, amt22_thd, amt); +} + +void amt22_periodic(amt22_t* amt) { + // trigger read + pprz_bsem_signal(&amt->bsem_amt22_read); +} + + +bool amt22_read(amt22_t* amt) { + if(spi_blocking_transceive(amt->config->p, &amt->trans, 0.5) != SPITransSuccess) { + return false; + } + + uint8_t p0 = amt->trans.input_buf[0]; + uint8_t p1 = amt->trans.input_buf[1]; + + if(!amt22_checkbit(p0,p1)) { + return false; + } + + uint16_t position = (p0 << 8 | p1) & 0x3fff; + int16_t turns = 0; + + // 12 bits so shift 2 + if(amt->config->type == AMT22_12_SINGLE || amt->config->type == AMT22_12_MULTI) { + position >>= 2; + } + + if(amt->config->type == AMT22_12_MULTI || amt->config->type == AMT22_14_MULTI) { + uint8_t t0 = amt->trans.input_buf[2]; + uint8_t t1 = amt->trans.input_buf[3]; + turns = (t0 << 8 | t1); + } + + pprz_mtx_lock(&amt->mtx); + amt->position = position; + amt->turns = turns; + pprz_mtx_unlock(&amt->mtx); + return true; +} + +static bool amt22_checkbit(uint8_t p0, uint8_t p1) { + uint16_t data = ((p0 << 8 | p1) & 0x3fff) >> 2; + uint8_t odd = 0; + uint8_t even = 0; + while (data) + { + even ^= data & 1; + data >>= 1; + odd ^= data & 1; + data >>= 1; + } + even = !even; + odd = !odd; + if((even == ((p0 & 0x40) >> 6)) && (odd == ((p0 & 0x80) >> 7))) { + return 1; + } else { + return 0; + } +} + +uint16_t amt22_get_position(amt22_t* amt) { + pprz_mtx_lock(&amt->mtx); + uint16_t pos = amt->position; + pprz_mtx_unlock(&amt->mtx); + return pos; +} + +int16_t amt22_get_turns(amt22_t* amt) { + pprz_mtx_lock(&amt->mtx); + int16_t turns = amt->turns; + pprz_mtx_unlock(&amt->mtx); + return turns; +} + + +static void amt22_thd(void* arg) { + amt22_t* amt = (amt22_t*)arg; + + while(true) { + pprz_bsem_wait(&amt->bsem_amt22_read); + amt22_read(amt); + } +} \ No newline at end of file diff --git a/sw/airborne/peripherals/amt22.h b/sw/airborne/peripherals/amt22.h new file mode 100644 index 0000000000..a5087e1e3f --- /dev/null +++ b/sw/airborne/peripherals/amt22.h @@ -0,0 +1,38 @@ +#pragma once + +#include "mcu_periph/spi.h" +#include "modules/core/threads.h" + +enum amt22_type { + AMT22_12_SINGLE, ///< 12-bits, single-turn + AMT22_14_SINGLE, ///< 14-bits, single-turn + AMT22_12_MULTI, ///< 12-bits, multi-turn + AMT22_14_MULTI, ///< 14-bits, multi-turn +}; + +typedef struct { + struct spi_periph *p; + uint8_t slave_idx; + enum amt22_type type; +} amt22_config_t; + +typedef struct { + amt22_config_t *config; + + // private + struct spi_transaction trans; + volatile uint8_t spi_input_buf[4]; + volatile uint8_t spi_output_buf[4]; + + uint16_t position; + int16_t turns; + + pprz_thread_t thd_handle; + pprz_bsem_t bsem_amt22_read; + pprz_mutex_t mtx; +} amt22_t; + +void amt22_init(amt22_t* amt22, amt22_config_t* conf); +void amt22_periodic(amt22_t* amt22); +uint16_t amt22_get_position(amt22_t* amt22); +int16_t amt22_get_turns(amt22_t* amt22); diff --git a/sw/airborne/peripherals/pca95xx.c b/sw/airborne/peripherals/pca95xx.c index 337351f4c0..2e3159d72b 100644 --- a/sw/airborne/peripherals/pca95xx.c +++ b/sw/airborne/peripherals/pca95xx.c @@ -50,7 +50,7 @@ bool pca95xx_configure(struct pca95xx *dev, uint8_t val, bool blocking) dev->i2c_trans.buf[0] = PCA95XX_CONFIG_REG; dev->i2c_trans.buf[1] = val; if (blocking) { - return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2); + return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2, 0.5) == I2CTransSuccess; } else { return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2); } @@ -68,7 +68,7 @@ bool pca95xx_set_output(struct pca95xx *dev, uint8_t mask, bool blocking) dev->i2c_trans.buf[0] = PCA95XX_OUTPUT_REG; dev->i2c_trans.buf[1] = mask; if (blocking) { - return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2); + return i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2, 0.5) == I2CTransSuccess; } else { return i2c_transmit(dev->i2c_p, &dev->i2c_trans, dev->i2c_trans.slave_addr, 2); } diff --git a/sw/airborne/peripherals/vl53l1_platform.c b/sw/airborne/peripherals/vl53l1_platform.c index 1e062635c3..845a57b6a1 100644 --- a/sw/airborne/peripherals/vl53l1_platform.c +++ b/sw/airborne/peripherals/vl53l1_platform.c @@ -47,7 +47,7 @@ int8_t VL53L1_WriteMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_ dev->i2c_trans.buf[1] = (index & 0x00FF); memcpy((uint8_t *) dev->i2c_trans.buf + 2, pdata, count); return !i2c_blocking_transmit(dev->i2c_p, &dev->i2c_trans, - dev->i2c_trans.slave_addr, 2 + count); + dev->i2c_trans.slave_addr, 2 + count, 1.f); } int8_t VL53L1_ReadMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_t count)