Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use mailbox for thread-safe data transfer, Improve SPI reliability #107

Closed
wants to merge 13 commits into from
Closed

Use mailbox for thread-safe data transfer, Improve SPI reliability #107

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
f513965
[SDK][Firmware] Add partial support for power-board, Update protocol …
thomasfla Jul 16, 2021
e49b0b9
Merge branch 'master' of https://github.com/thomasfla/master-board
thomasfla Jul 16, 2021
5cc30d1
add pyton bindings for power baord
Jul 28, 2021
2e8904e
update doc including powerboard data in sensor packets
thomasfla Jul 28, 2021
8f7fd01
Merge branch 'open-dynamic-robot-initiative:master' into master
thomasfla Jul 28, 2021
e09fadc
Merge pull request #2 from paLeziart/bindings_powerboard
thomasfla Jul 29, 2021
d1d9e50
update BOM, correct C2 and R17 MPN; remove deprecated seeed BOM
thomasfla Jul 30, 2021
4a5f421
Fix 0402 footprint to comply with seeed, fix HR911105A footprint, add…
thomasfla Sep 30, 2021
da743d9
Update production files, add xy files inside the zip
thomasfla Sep 30, 2021
768f83f
Replace non thread safe ring buffer by a FreeRTOS mailbox queue
thomasfla Mar 26, 2022
4706076
change CS from hardware control to software control. This adds a smal…
thomasfla Mar 28, 2022
4e3c911
Rewrite IMU driver using mailbox, fixing esp32 hardware bug on uart fifo
thomasfla Apr 8, 2022
013589b
Merge branch 'master' into usemailbox
nim65s Dec 16, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 19 additions & 2 deletions firmware/components/spi_manager/spi_manager.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>

#include "driver/spi_master.h"
#include "soc/gpio_struct.h"
#include "driver/gpio.h"

#include "spi_manager.h"
#include <string.h>

Expand Down Expand Up @@ -34,10 +39,12 @@ void config_demux() {
}

void IRAM_ATTR spi_pre_transfer_callback(spi_transaction_t *trans) {
/*
uint slave_nb = ((spi_trans_info*) trans->user)->demux_nb;
gpio_set_level(GPIO_DEMUX_A0, slave_nb&0x1);
gpio_set_level(GPIO_DEMUX_A1, (slave_nb>>1)&0x1);
gpio_set_level(GPIO_DEMUX_A2, (slave_nb>>2)&0x1);
*/
}

void IRAM_ATTR spi_post_transfer_callback(spi_transaction_t *trans) {
Expand All @@ -59,7 +66,7 @@ void spi_init() {
spi_device_interface_config_t devcfg={
.clock_speed_hz=SPI_MASTER_FREQ_80M / CONFIG_SPI_DATARATE_FACTOR, //Clock out
.mode=0, //SPI mode 0
.spics_io_num=GPIO_DEMUX_OE, //CS pin
.spics_io_num=-1, //CS pin
.queue_size=10, //We want to be able to queue 10 transactions at a time
.pre_cb=spi_pre_transfer_callback, //Specify pre-transfer callback to handle D/C line
.post_cb=spi_post_transfer_callback, //Specify pre-transfer callback to handle D/C line
Expand All @@ -71,6 +78,15 @@ void spi_init() {
}

bool spi_send(int slave, uint8_t *tx_data, uint8_t *rx_data, int len) {
//Select the CS with DEMUX
gpio_set_level(GPIO_DEMUX_A0, slave&0x1);
gpio_set_level(GPIO_DEMUX_A1, (slave>>1)&0x1);
gpio_set_level(GPIO_DEMUX_A2, (slave>>2)&0x1);

//Low CS
gpio_set_level(GPIO_DEMUX_OE, 0);
usleep(1);

spi_transaction_t trans;
memset(&trans, 0, sizeof(trans));

Expand All @@ -85,6 +101,7 @@ bool spi_send(int slave, uint8_t *tx_data, uint8_t *rx_data, int len) {
trans.length=8*len;

esp_err_t err = spi_device_polling_transmit(spi, &trans);

//High CS
gpio_set_level(GPIO_DEMUX_OE, 1);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There seems to be some inconsistency in the indentation (some lines using tabs, some spaces). Might be good to clean this up with some auto-formatter.

return err == ESP_OK;
}
2 changes: 1 addition & 1 deletion firmware/components/spi_manager/spi_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#define GPIO_DEMUX_A1 4
#define GPIO_DEMUX_A2 16

#define GPIO_DEMUX_PIN_SEL ((1ULL<<GPIO_DEMUX_A0) | (1ULL<<GPIO_DEMUX_A1) | (1ULL<<GPIO_DEMUX_A2))
#define GPIO_DEMUX_PIN_SEL ((1ULL<<GPIO_DEMUX_A0) | (1ULL<<GPIO_DEMUX_A1) | (1ULL<<GPIO_DEMUX_A2)) | (1ULL<<GPIO_DEMUX_OE)


#if CONFIG_SPI_DATARATE_1
Expand Down
115 changes: 58 additions & 57 deletions firmware/main/masterboard_main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
#include "quad_crc.h"
#include "uart_imu.h"
#include "ws2812_led_control.h"

#include "defines.h"

#define ENABLE_DEBUG_PRINTF false
Expand Down Expand Up @@ -56,19 +55,18 @@ struct led_state ws_led;

static uint16_t spi_index_trans = 0;

static uint16_t spi_rx_packet[CONFIG_N_SLAVES][SPI_TOTAL_LEN + 1]; // +1 prevents any overflow

static uint16_t spi_tx_packet_a[CONFIG_N_SLAVES][SPI_TOTAL_LEN];
static uint16_t spi_tx_packet_b[CONFIG_N_SLAVES][SPI_TOTAL_LEN];
static uint16_t spi_tx_packet_stop[CONFIG_N_SLAVES][SPI_TOTAL_LEN];
static uint16_t spi_rx_packet[CONFIG_N_SLAVES][SPI_TOTAL_LEN + 1]; // +1 prevents any overflow //TODO understand why we need this?
static uint16_t spi_tx_packet[CONFIG_N_SLAVES][SPI_TOTAL_LEN];

uint16_t command_index_prev = 0;

struct wifi_eth_packet_sensor wifi_eth_tx_data;

struct wifi_eth_packet_ack wifi_eth_tx_ack;

bool spi_use_a = true;
struct wifi_eth_packet_command wifi_eth_rx_cmd_reader_buffer;
QueueHandle_t wifi_eth_rx_cmd_mailbox;

bool send_zero_cmd = true; //If true, all cmd packet sent to udrivers ar set to 0.

void print_spi_connected()
{
Expand Down Expand Up @@ -157,12 +155,7 @@ static void periodic_timer_callback(void *arg)
/* Prepare spi transactions */
bool spi_done[CONFIG_N_SLAVES] = {0}; // used to keep track of which spi transactions are done
spi_index_trans++;

/* Choose spi packets to send*/
uint16_t(*p_tx)[SPI_TOTAL_LEN];

p_tx = spi_tx_packet_stop; // default command is stop

send_zero_cmd = true;
spi_count++;

switch (current_state)
Expand Down Expand Up @@ -211,7 +204,12 @@ static void periodic_timer_callback(void *arg)
}
else
{
p_tx = spi_use_a ? spi_tx_packet_a : spi_tx_packet_b;
send_zero_cmd = false;
// Read command message in a RTOS mailbox for thread safe comunication */
if (!xQueuePeek(wifi_eth_rx_cmd_mailbox, &wifi_eth_rx_cmd_reader_buffer, 0))
{
send_zero_cmd = true; //mailbox is empty -> we never got a command packet
}
}

wifi_eth_count++;
Expand Down Expand Up @@ -253,22 +251,52 @@ static void periodic_timer_callback(void *arg)
}
//print_imu();
printf("\nlast CMD packet:\n");
print_packet(p_tx[2], SPI_TOTAL_LEN * 2);
print_packet(spi_tx_packet[2], SPI_TOTAL_LEN * 2);
}

/* Complete and send each packet */

// Fills the outgoing spi packet
/* Prepare the outgoing spi TX packet */
for (int i = 0; i < CONFIG_N_SLAVES; i++)
{
//printf("%d\n",send_zero_cmd);
if (!TEST_BIT(spi_connected, i) && !spi_autodetect)
continue; // ignoring this slave if it is not connected

SPI_REG_u16(p_tx[i], SPI_TOTAL_INDEX) = SPI_SWAP_DATA_TX(spi_index_trans, 16);
SPI_REG_u32(p_tx[i], SPI_TOTAL_CRC) = SPI_SWAP_DATA_TX(packet_compute_CRC(p_tx[i]), 32);
if (!send_zero_cmd)
{
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_MODE) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].mode, 16);
SPI_REG_32(spi_tx_packet[i], SPI_COMMAND_POS_1) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].position[0], 32);
SPI_REG_32(spi_tx_packet[i], SPI_COMMAND_POS_2) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].position[1], 32);
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_VEL_1) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].velocity[0], 16);
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_VEL_2) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].velocity[1], 16);
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_IQ_1) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].current[0], 16);
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_IQ_2) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].current[1], 16);
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KP_1) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].kp[0], 16);
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KP_2) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].kp[1], 16);
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KD_1) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].kd[0], 16);
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KD_2) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].kd[1], 16);
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_ISAT_12) = SPI_SWAP_DATA_TX(wifi_eth_rx_cmd_reader_buffer.command[i].isat, 16);
}
else
{
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_MODE) = SPI_SWAP_DATA_TX(1<<15, 16); //In case of zero commands, keep the system enabled
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using a constant with a speaking name for the 1<<15 would make reading the code a bit easier.

SPI_REG_32(spi_tx_packet[i], SPI_COMMAND_POS_1) = 0;
SPI_REG_32(spi_tx_packet[i], SPI_COMMAND_POS_2) = 0;
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_VEL_1) = 0;
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_VEL_2) = 0;
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_IQ_1) = 0;
SPI_REG_16(spi_tx_packet[i], SPI_COMMAND_IQ_2) = 0;
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KP_1) = 0;
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KP_2) = 0;
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KD_1) = 0;
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_KD_2) = 0;
SPI_REG_u16(spi_tx_packet[i], SPI_COMMAND_ISAT_12) = 0;
}
SPI_REG_u16(spi_tx_packet[i], SPI_TOTAL_INDEX) = SPI_SWAP_DATA_TX(spi_index_trans, 16);
SPI_REG_u32(spi_tx_packet[i], SPI_TOTAL_CRC) = SPI_SWAP_DATA_TX(packet_compute_CRC(spi_tx_packet[i]), 32);
}

/* Send every trans the needed number of times */
/* Perform every transaction the needed number of times */
for (int spi_try = 0; spi_try < spi_n_attempt; spi_try++)
{
for (int i = 0; i < CONFIG_N_SLAVES; i++)
Expand All @@ -278,9 +306,8 @@ static void periodic_timer_callback(void *arg)

if (spi_done[i])
continue; // ignoring this slave if the transaction has already been done

spi_send(i, (uint8_t *)p_tx[i], (uint8_t *)spi_rx_packet[i], SPI_TOTAL_LEN * 2);


spi_send(i, (uint8_t *)spi_tx_packet[i], (uint8_t *)spi_rx_packet[i], SPI_TOTAL_LEN * 2);
//if (ms_cpt % 500 == 0) printf("%d %d\n", spi_try, i);

// checking if data is correct
Expand Down Expand Up @@ -314,13 +341,14 @@ static void periodic_timer_callback(void *arg)

else
{
//printf("%d\n",ms_cpt);
// zeroing sensor data in packet, except the status field
memset(&(wifi_eth_tx_data.sensor[i]), 0, sizeof(struct sensor_data));
wifi_eth_tx_data.sensor[i].status = 0xf; // specifying that the transaction failed in the sensor packet
}
}

//Slave 7 is always the power board

uint8_t tx_spi_powerboard[12]={0xaa,0xbb,0xcc,0xdd,0xee,0xff,0xaa,0xbb,0xcc,0xdd,0xee,0xff}; //Fake packet, cmd to the power board are not yet implemented
uint8_t rx_spi_powerboard[12]={0};
spi_send(7, tx_spi_powerboard,rx_spi_powerboard, 12);
Expand All @@ -334,7 +362,6 @@ static void periodic_timer_callback(void *arg)
fb.b[1] = rx_spi_powerboard[8];
fb.b[0] = rx_spi_powerboard[9];
wifi_eth_tx_data.powerboard.energy = fb.f;

}
}

Expand Down Expand Up @@ -379,7 +406,7 @@ static void periodic_timer_callback(void *arg)
case SPI_AUTODETECT:
case ACTIVE_CONTROL:
case WIFI_ETH_ERROR:
/* Send all spi_sensor packets to PC */
/* Send sensors packet to PC */
wifi_eth_tx_data.sensor_index++;
if (use_wifi)
{
Expand All @@ -404,12 +431,6 @@ static void periodic_timer_callback(void *arg)
void setup_spi()
{
spi_init();

for (int i = 0; i < CONFIG_N_SLAVES; i++)
{
memset(spi_tx_packet_stop[i], 0, SPI_TOTAL_INDEX * 2);
SPI_REG_u16(spi_tx_packet_stop[i], SPI_COMMAND_MODE) = SPI_SWAP_DATA_TX(1<<15, 16);
}
wifi_eth_tx_data.sensor_index = 0;
wifi_eth_tx_data.packet_loss = 0;

Expand Down Expand Up @@ -481,28 +502,8 @@ void wifi_eth_receive_cb(uint8_t src_mac[6], uint8_t *data, int len, char eth_or
command_index_prev = packet_recv->command_index - 1;
}

/* Prepare SPI packets */
uint16_t(*to_fill)[SPI_TOTAL_LEN] = spi_use_a ? spi_tx_packet_b : spi_tx_packet_a;

for (int i = 0; i < CONFIG_N_SLAVES; i++)
{
if (!TEST_BIT(spi_connected, i) && !spi_autodetect)
continue; // ignoring this slave if it is not connected

SPI_REG_u16(to_fill[i], SPI_COMMAND_MODE) = SPI_SWAP_DATA_TX(packet_recv->command[i].mode, 16);
SPI_REG_32(to_fill[i], SPI_COMMAND_POS_1) = SPI_SWAP_DATA_TX(packet_recv->command[i].position[0], 32);
SPI_REG_32(to_fill[i], SPI_COMMAND_POS_2) = SPI_SWAP_DATA_TX(packet_recv->command[i].position[1], 32);
SPI_REG_16(to_fill[i], SPI_COMMAND_VEL_1) = SPI_SWAP_DATA_TX(packet_recv->command[i].velocity[0], 16);
SPI_REG_16(to_fill[i], SPI_COMMAND_VEL_2) = SPI_SWAP_DATA_TX(packet_recv->command[i].velocity[1], 16);
SPI_REG_16(to_fill[i], SPI_COMMAND_IQ_1) = SPI_SWAP_DATA_TX(packet_recv->command[i].current[0], 16);
SPI_REG_16(to_fill[i], SPI_COMMAND_IQ_2) = SPI_SWAP_DATA_TX(packet_recv->command[i].current[1], 16);
SPI_REG_u16(to_fill[i], SPI_COMMAND_KP_1) = SPI_SWAP_DATA_TX(packet_recv->command[i].kp[0], 16);
SPI_REG_u16(to_fill[i], SPI_COMMAND_KP_2) = SPI_SWAP_DATA_TX(packet_recv->command[i].kp[1], 16);
SPI_REG_u16(to_fill[i], SPI_COMMAND_KD_1) = SPI_SWAP_DATA_TX(packet_recv->command[i].kd[0], 16);
SPI_REG_u16(to_fill[i], SPI_COMMAND_KD_2) = SPI_SWAP_DATA_TX(packet_recv->command[i].kd[1], 16);
SPI_REG_u16(to_fill[i], SPI_COMMAND_ISAT_12) = SPI_SWAP_DATA_TX(packet_recv->command[i].isat, 16);
}
spi_use_a = !spi_use_a;
/* Write command message in a RTOS mailbox for thread safe comunication */
xQueueOverwrite( wifi_eth_rx_cmd_mailbox, packet_recv );

/* Compute data for next wifi_eth_sensor packet */
wifi_eth_tx_data.packet_loss += ((struct wifi_eth_packet_command *)data)->command_index - command_index_prev - 1;
Expand All @@ -515,7 +516,6 @@ void wifi_eth_receive_cb(uint8_t src_mac[6], uint8_t *data, int len, char eth_or
}

//function that will be called on a link state change

void wifi_eth_link_state_cb(bool new_state)
{
// In WAITING_FOR_INIT, we don't know if wifi or ethernet is used
Expand All @@ -530,9 +530,10 @@ void wifi_eth_link_state_cb(bool new_state)

void app_main()
{

uart_set_baudrate(UART_NUM_0, 2000000);
nvs_flash_init();

wifi_eth_rx_cmd_mailbox = xQueueCreate( 1, sizeof(struct wifi_eth_packet_command));
ws2812_control_init(); //init the LEDs
set_all_leds(0x0f0f0f);

Expand Down