Implemented reliable Serial Rx/Tx

► Reliable Serial Communication implemented featuring:
- start frame
- checksum
- out-of-sync handling
- timeout and disconnected line detection
► Arduino example code added
► Fixed ConsoleLog Item #5
This commit is contained in:
EmanuelFeru 2019-11-29 17:31:53 +01:00
parent 9a9eed7d10
commit 5d195696a2
8 changed files with 520 additions and 219 deletions

View File

@ -0,0 +1,174 @@
// *******************************************************************
// Arduino Nano 3.3V example code
// for https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC
//
// Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
//
// *******************************************************************
// INFO:
// • This sketch uses the the Serial Software interface to communicate and send commands to the hoverboard
// • The built-in (HW) Serial interface is used for debugging and visualization. In case the debugging is not needed,
// it is recommended to use the built-in Serial interface for full speed perfomace.
// • The data packaging includes a Start Frame, checksum, and re-syncronization capability for reliable communication
//
// CONFIGURATION on the hoverboard side in config.h:
// • Option 1: Serial on Left Sensor cable (long wired cable)
// #define CONTROL_SERIAL_USART2
// #define FEEDBACK_SERIAL_USART2
// // #define DEBUG_SERIAL_USART2
// • Option 2: Serial on Right Sensor cable (short wired cable) - recommended, so the ADCs on the other cable are still available
// #define CONTROL_SERIAL_USART3
// #define FEEDBACK_SERIAL_USART3
// // #define DEBUG_SERIAL_USART3
// *******************************************************************
// ########################## DEFINES ##########################
#define HOVER_SERIAL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
#define TIME_SEND 100 // [ms] Sending time interval
#define SPEED_MAX_TEST 300 // [-] Maximum speed for testing
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
#include <SoftwareSerial.h>
SoftwareSerial HoverSerial(2,3); // RX, TX
// Global variables
uint8_t idx = 0; // Index for new data pointer
uint16_t bufStartFrame; // Buffer Start Frame
byte *p; // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;
typedef struct{
uint16_t start;
int16_t steer;
int16_t speed;
uint16_t checksum;
} SerialCommand;
SerialCommand Command;
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;
// ########################## SETUP ##########################
void setup()
{
Serial.begin(SERIAL_BAUD);
Serial.println("Hoverboard Serial v1.0");
HoverSerial.begin(HOVER_SERIAL_BAUD);
pinMode(LED_BUILTIN, OUTPUT);
}
// ########################## SEND ##########################
void Send(int16_t uSteer, int16_t uSpeed)
{
// Create command
Command.start = (uint16_t)START_FRAME;
Command.steer = (int16_t)uSteer;
Command.speed = (int16_t)uSpeed;
Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed);
// Write to Serial
HoverSerial.write((uint8_t *) &Command, sizeof(Command));
}
// ########################## RECEIVE ##########################
void Receive()
{
// Check for new data availability in the Serial buffer
if (HoverSerial.available()) {
incomingByte = HoverSerial.read(); // Read the incoming byte
bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start marker
}
else {
return;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(incomingByte);
return;
#endif
// Copy received data
if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
p = (byte *)&NewFeedback;
*p++ = incomingBytePrev;
*p++ = incomingByte;
idx = 2;
} else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data
*p++ = incomingByte;
idx++;
}
// Check if we reached the end of the package
if (idx == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp;
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
// Print data to built-in Serial
Serial.print("1: "); Serial.print(Feedback.cmd1);
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
Serial.print(" 3: "); Serial.print(Feedback.speedR);
Serial.print(" 4: "); Serial.print(Feedback.speedL);
Serial.print(" 5: "); Serial.print(Feedback.speedR_meas);
Serial.print(" 6: "); Serial.print(Feedback.speedL_meas);
Serial.print(" 7: "); Serial.print(Feedback.batVoltage);
Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
} else {
Serial.println("Non-valid data skipped");
}
idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
}
// Update previous states
incomingBytePrev = incomingByte;
}
// ########################## LOOP ##########################
unsigned long iTimeSend = 0;
int iTestMax = SPEED_MAX_TEST;
int iTest = 0;
void loop(void)
{
unsigned long timeNow = millis();
// Check for new received data
Receive();
// Send commands
if (iTimeSend > timeNow) return;
iTimeSend = timeNow + TIME_SEND;
Send(0, abs(iTest));
// Calculate test command signal
iTest += 10;
if (iTest > iTestMax) iTest = -iTestMax;
// Blink the LED
digitalWrite(LED_BUILTIN, (timeNow%2000)<1000);
}
// ########################## END ##########################

View File

@ -76,19 +76,38 @@
//#define DEBUG_I2C_LCD // standard 16x2 or larger text-lcd via i2c-converter on right sensor board cable
// ############################### SERIAL DEBUG ###############################
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#define DEBUG_BAUD 115200 // UART baud rate
//#define DEBUG_SERIAL_SERVOTERM
#define DEBUG_SERIAL_ASCII // "1:345 2:1337 3:0 4:0 5:0 6:0 7:0 8:0\r\n"
// ############################### INPUT ###############################
// ###### CONTROL VIA UART (serial) ######
//#define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define CONTROL_BAUD 19200 // control via usart from eg an Arduino or raspberry
// for Arduino, use void loop(void){ Serial.write((uint8_t *) &steer, sizeof(steer)); Serial.write((uint8_t *) &speed, sizeof(speed));delay(20); }
#define START_FRAME 0xAAAA // [-] Start frame definition for serial commands
#define SERIAL_TIMEOUT 160 // [-] Serial timeout duration for the received data. 160 ~= 0.8 sec. Calculation: 0.8 sec / 0.005 sec
#define USART2_BAUD 38400 // UART2 baud rate (long wired cable)
#define USART2_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
// #define DEBUG_SERIAL_USART2 // left sensor board cable, disable if ADC or PPM is used!
#define USART3_BAUD 38400 // UART3 baud rate (short wired cable)
#define USART3_WORDLENGTH UART_WORDLENGTH_8B // UART_WORDLENGTH_8B or UART_WORDLENGTH_9B
// #define CONTROL_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used! For Arduino control check the hoverSerial.ino
// #define FEEDBACK_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#define DEBUG_SERIAL_USART3 // right sensor board cable, disable if I2C (nunchuck or lcd) is used!
#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
#define UART_DMA_CHANNEL DMA1_Channel7
#endif
#if defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
#define UART_DMA_CHANNEL DMA1_Channel2
#endif
// ###### CONTROL VIA RC REMOTE ######
// left sensor board cable. Channel 1: steering, Channel 2: speed.
@ -103,7 +122,7 @@
* For middle resting potis: Let the potis in the middle resting position, write value 1 to ADC1_MID and value 2 to ADC2_MID
* Make, flash and test it.
*/
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2!
#define CONTROL_ADC // use ADC as input. disable CONTROL_SERIAL_USART2, FEEDBACK_SERIAL_USART2, DEBUG_SERIAL_USART2!
#define ADC1_MID_POT // ADC1 middle resting poti: comment-out if NOT a middle resting poti
#define ADC2_MID_POT // ADC2 middle resting poti: comment-out if NOT a middle resting poti
#define ADC1_MIN 0 // min ADC1-value while poti at minimum-position (0 - 4095)
@ -119,7 +138,7 @@
* use the right one of the 2 types of nunchucks, add i2c pullups.
* use original nunchuck. most clones does not work very well.
*/
// #define CONTROL_NUNCHUCK // use nunchuck as input. disable DEBUG_SERIAL_USART3!
// #define CONTROL_NUNCHUCK // use nunchuck as input. disable FEEDBACK_SERIAL_USART3, DEBUG_SERIAL_USART3!
// ############################### MOTOR CONTROL (overwrite) #########################
@ -158,16 +177,16 @@
*/
// Value of RATE is in fixdt(1,16,4): VAL_fixedPoint = VAL_floatingPoint * 2^4. In this case 480 = 30 * 2^4
#define RATE 480 // 30.0f [-] lower value == slower rate [0, 32767] = [0.0 - 2047.9375]. Do NOT make rate negative (>32767)
#define RATE 480 // 30.0f [-] lower value == slower rate [0, 32767] = [0.0, 2047.9375]. Do NOT make rate negative (>32767)
// Value of FILTER is in fixdt(0,16,16): VAL_fixedPoint = VAL_floatingPoint * 2^16. In this case 6553 = 0.1 * 2^16
#define FILTER 6553 // 0.1f [-] lower value == softer filter [0, 65535] = [0.0 - 1.0].
#define FILTER 6553 // 0.1f [-] lower value == softer filter [0, 65535] = [0.0, 1.0].
// Value of COEFFICIENT is in fixdt(1,16,14)
// If VAL_floatingPoint >= 0, VAL_fixedPoint = VAL_floatingPoint * 2^14
// If VAL_floatingPoint < 0, VAL_fixedPoint = 2^16 + floor(VAL_floatingPoint * 2^14).
#define SPEED_COEFFICIENT 16384 // 1.0f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 16384 = 1.0 * 2^14
#define STEER_COEFFICIENT 8192 // 0.5f [-] higher value == stronger. [0, 65535] = [-2.0 - 2.0]. In this case 8192 = 0.5 * 2^14. If you do not want any steering, set it to 0.
#define SPEED_COEFFICIENT 16384 // 1.0f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 16384 = 1.0 * 2^14
#define STEER_COEFFICIENT 8192 // 0.5f [-] higher value == stronger. [0, 65535] = [-2.0, 2.0]. In this case 8192 = 0.5 * 2^14. If you do not want any steering, set it to 0.
#define INVERT_R_DIRECTION
#define INVERT_L_DIRECTION
@ -186,22 +205,42 @@
// ############################### VALIDATE SETTINGS ###############################
#if defined CONTROL_SERIAL_USART2 && defined CONTROL_ADC
#error CONTROL_ADC and CONTROL_SERIAL_USART2 not allowed. it is on the same cable.
#if defined(CONTROL_SERIAL_USART2) && defined(CONTROL_SERIAL_USART3)
#error CONTROL_SERIAL_USART2 and CONTROL_SERIAL_USART3 not allowed, choose one.
#endif
#if defined CONTROL_SERIAL_USART2 && defined CONTROL_PPM
#error CONTROL_PPM and CONTROL_SERIAL_USART2 not allowed. it is on the same cable.
#if defined(FEEDBACK_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART3)
#error FEEDBACK_SERIAL_USART2 and FEEDBACK_SERIAL_USART3 not allowed, choose one.
#endif
#if defined DEBUG_SERIAL_USART3 && defined CONTROL_NUNCHUCK
#error CONTROL_NUNCHUCK and DEBUG_SERIAL_USART3 not allowed. it is on the same cable.
#if defined(DEBUG_SERIAL_USART2) && defined(FEEDBACK_SERIAL_USART2)
#error DEBUG_SERIAL_USART2 and FEEDBACK_SERIAL_USART2 not allowed, choose one.
#endif
#if defined DEBUG_SERIAL_USART3 && defined DEBUG_I2C_LCD
#error DEBUG_I2C_LCD and DEBUG_SERIAL_USART3 not allowed. it is on the same cable.
#if defined(DEBUG_SERIAL_USART3) && defined(FEEDBACK_SERIAL_USART3)
#error DEBUG_SERIAL_USART3 and FEEDBACK_SERIAL_USART3 not allowed, choose one.
#endif
#if defined CONTROL_PPM && defined CONTROL_ADC && defined CONTROL_NUNCHUCK || defined CONTROL_PPM && defined CONTROL_ADC || defined CONTROL_ADC && defined CONTROL_NUNCHUCK || defined CONTROL_PPM && defined CONTROL_NUNCHUCK
#if defined(DEBUG_SERIAL_USART2) && defined(DEBUG_SERIAL_USART3)
#error DEBUG_SERIAL_USART2 and DEBUG_SERIAL_USART3 not allowed, choose one.
#endif
#if defined(CONTROL_ADC) && (defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2))
#error CONTROL_ADC and SERIAL_USART2 not allowed. It is on the same cable.
#endif
#if (defined(DEBUG_SERIAL_USART2) || defined(CONTROL_SERIAL_USART2)) && defined(CONTROL_PPM)
#error CONTROL_PPM and SERIAL_USART2 not allowed. It is on the same cable.
#endif
#if (defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3)) && defined(CONTROL_NUNCHUCK)
#error CONTROL_NUNCHUCK and SERIAL_USART3 not allowed. It is on the same cable.
#endif
#if (defined(DEBUG_SERIAL_USART3) || defined(CONTROL_SERIAL_USART3)) && defined(DEBUG_I2C_LCD)
#error DEBUG_I2C_LCD and SERIAL_USART3 not allowed. It is on the same cable.
#endif
#if defined(CONTROL_PPM) && defined(CONTROL_ADC) && defined(CONTROL_NUNCHUCK) || defined(CONTROL_PPM) && defined(CONTROL_ADC) || defined(CONTROL_ADC) && defined(CONTROL_NUNCHUCK) || defined(CONTROL_PPM) && defined(CONTROL_NUNCHUCK)
#error only 1 input method allowed. use CONTROL_PPM or CONTROL_ADC or CONTROL_NUNCHUCK.
#endif

View File

@ -27,4 +27,5 @@ void MX_GPIO_Init(void);
void MX_TIM_Init(void);
void MX_ADC1_Init(void);
void MX_ADC2_Init(void);
void UART_Init(void);
void UART2_Init(void);
void UART3_Init(void);

View File

@ -6,7 +6,7 @@
* smooth torque output
* improved motor efficiency -> lower energy consumption
*
* Copyright (C) 2019 Emanuel FERU <aerdronix@gmail.com>
* Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@ -46,7 +46,7 @@ extern ExtY rtY_Right; /* External outputs */
static int16_t pwm_margin = 100; /* This margin allows to always have a window in the PWM signal for proper Phase currents measurement */
uint8_t ctrlModReq = CTRL_MOD_REQ;
extern uint8_t ctrlModReq;
int16_t curL_phaA = 0, curL_phaB = 0, curL_DC = 0;
int16_t curR_phaB = 0, curR_phaC = 0, curR_DC = 0;
uint8_t errCode_Left = 0;

View File

@ -6,16 +6,8 @@
#include "config.h"
#include "comms.h"
UART_HandleTypeDef huart2;
#ifdef DEBUG_SERIAL_USART3
#define UART_DMA_CHANNEL DMA1_Channel2
#endif
#ifdef DEBUG_SERIAL_USART2
#define UART_DMA_CHANNEL DMA1_Channel7
#endif
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
static volatile uint8_t uart_buf[100];
static volatile int16_t ch_buf[8];
@ -47,12 +39,15 @@ void consoleScope(void) {
#endif
#if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3)
memset((void *)(uintptr_t)uart_buf, 0, sizeof(uart_buf));
sprintf((char *)(uintptr_t)uart_buf, "1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i 8:%i\r\n", ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3], ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]);
// memset((void *)(uintptr_t)uart_buf, 0, sizeof(uart_buf));
int strLength;
strLength = sprintf((char *)(uintptr_t)uart_buf,
"1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i 8:%i\r\n",
ch_buf[0], ch_buf[1], ch_buf[2], ch_buf[3], ch_buf[4], ch_buf[5], ch_buf[6], ch_buf[7]);
if(UART_DMA_CHANNEL->CNDTR == 0) {
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
UART_DMA_CHANNEL->CNDTR = strlen((char *)(uintptr_t)uart_buf);
UART_DMA_CHANNEL->CNDTR = strLength;
UART_DMA_CHANNEL->CMAR = (uint32_t)uart_buf;
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
}
@ -61,5 +56,12 @@ void consoleScope(void) {
void consoleLog(char *message)
{
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)message, (uint16_t)strlen(message));
#if defined DEBUG_SERIAL_ASCII && (defined DEBUG_SERIAL_USART2 || defined DEBUG_SERIAL_USART3)
if(UART_DMA_CHANNEL->CNDTR == 0) {
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
UART_DMA_CHANNEL->CNDTR = strlen((char *)(uintptr_t)message);
UART_DMA_CHANNEL->CMAR = (uint32_t)message;
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
}
#endif
}

View File

@ -4,6 +4,7 @@
* Copyright (C) 2017-2018 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2017-2018 Nico Stute <crinq@crinq.de>
* Copyright (C) 2017-2018 Niklas Fauth <niklas.fauth@kit.fail>
* Copyright (C) 2019-2020 Emanuel FERU <aerdronix@gmail.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@ -62,17 +63,43 @@ extern volatile adc_buf_t adc_buffer;
//LCD_PCF8574_HandleTypeDef lcd;
extern I2C_HandleTypeDef hi2c2;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
static UART_HandleTypeDef huart;
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
typedef struct{
uint16_t start;
int16_t steer;
int16_t speed;
//uint32_t crc;
uint16_t checksum;
} Serialcommand;
static volatile Serialcommand command;
static int16_t timeoutCnt = 0 // Timeout counter for Rx Serial command
#endif
static uint8_t timeoutFlag = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#if defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t checksum;
} SerialFeedback;
static SerialFeedback Feedback;
#endif
static uint8_t serialSendCounter; // serial send counter
#if defined(CONTROL_NUNCHUCK) || defined(CONTROL_PPM) || defined(CONTROL_ADC)
static uint8_t button1, button2;
#endif
uint8_t ctrlModReq = CTRL_MOD_REQ;
static int cmd1; // normalized input value. -1000 to 1000
static int cmd2; // normalized input value. -1000 to 1000
static int16_t steer; // local variable for steering. -1000 to 1000
@ -105,6 +132,7 @@ void poweroff(void) {
// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
buzzerPattern = 0;
enable = 0;
consoleLog("-- Motors disabled --\r\n");
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
@ -144,10 +172,6 @@ int main(void) {
MX_ADC1_Init();
MX_ADC2_Init();
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
UART_Init();
#endif
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 1);
HAL_ADC_Start(&hadc1);
@ -209,9 +233,16 @@ int main(void) {
Nunchuck_Init();
#endif
#ifdef CONTROL_SERIAL_USART2
UART_Control_Init();
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, 4);
#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
UART2_Init();
huart = huart2;
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
UART3_Init();
huart = huart3;
#endif
#if defined(CONTROL_SERIAL_USART2) || defined(CONTROL_SERIAL_USART3)
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
#endif
#ifdef DEBUG_I2C_LCD
@ -287,11 +318,41 @@ int main(void) {
timeout = 0;
#endif
#ifdef CONTROL_SERIAL_USART2
#if defined CONTROL_SERIAL_USART2 || defined CONTROL_SERIAL_USART3
// Handle received data validity, timeout and fix out-of-sync if necessary
if (command.start == START_FRAME && command.checksum == (command.start ^ command.steer ^ command.speed)) {
if (timeoutFlag) { // Check for previous timeout flag
if (timeoutCnt-- <= 0) // Timeout de-qualification
timeoutFlag = 0; // Timeout flag cleared
} else {
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
command.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCnt = 0; // Reset the timeout counter
}
} else {
if (timeoutCnt++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlag = 1; // Timeout detected
timeoutCnt = SERIAL_TIMEOUT; // Limit timout counter value
}
// Check the received Start Frame. If it is NOT OK, most probably we are out-of-sync.
// Try to re-sync by reseting the DMA
if (command.start != START_FRAME && command.start != 0xFFFF) {
HAL_UART_DMAStop(&huart);
HAL_UART_Receive_DMA(&huart, (uint8_t *)&command, sizeof(command));
}
}
if (timeoutFlag) { // In case of timeout bring the system to a Safe State
ctrlModReq = 0; // OPEN_MODE request. This will bring the motor power to 0 in a controlled way
cmd1 = 0;
cmd2 = 0;
} else {
ctrlModReq = CTRL_MOD_REQ; // Follow the Mode request
}
timeout = 0;
#endif
@ -302,6 +363,7 @@ int main(void) {
buzzerFreq = 4; HAL_Delay(200);
buzzerFreq = 0;
enable = 1; // enable motors
consoleLog("-- Motors enabled --\r\n");
}
// ####### LOW-PASS FILTER #######
@ -317,11 +379,6 @@ int main(void) {
// speedL = CLAMP((int)(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT), -1000, 1000);
mixerFcn(speedFixdt, steerFixdt, &speedR, &speedL); // This function implements the equations above
#ifdef ADDITIONAL_CODE
ADDITIONAL_CODE;
#endif
// ####### SET OUTPUTS (if the target change is less than +/- 50) #######
if ((speedL > lastSpeedL-50 && speedL < lastSpeedL+50) && (speedR > lastSpeedR-50 && speedR < lastSpeedR+50) && timeout < TIMEOUT) {
#ifdef INVERT_R_DIRECTION
@ -340,16 +397,20 @@ int main(void) {
lastSpeedR = speedR;
if (inactivity_timeout_counter % 25 == 0) {
// ####### CALC BOARD TEMPERATURE #######
filtLowPass16(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
board_temp_adcFilt = board_temp_adcFixdt >> 4; // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
serialSendCounter++; // Increment the counter
if (serialSendCounter > 20) { // Send data every 100 ms = 20 * 5 ms, where 5 ms is approximately the main loop duration
serialSendCounter = 0; // Reset the counter
// ####### DEBUG SERIAL OUT #######
#if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
#ifdef CONTROL_ADC
setScopeChannel(0, (int)adc_buffer.l_tx2); // 1: ADC1
setScopeChannel(1, (int)adc_buffer.l_rx2); // 2: ADC2
setScopeChannel(0, (int16_t)adc_buffer.l_tx2); // 1: ADC1
setScopeChannel(1, (int16_t)adc_buffer.l_rx2); // 2: ADC2
#endif
setScopeChannel(2, (int16_t)speedR); // 1: output command: [-1000, 1000]
setScopeChannel(3, (int16_t)speedL); // 2: output command: [-1000, 1000]
@ -358,6 +419,28 @@ int main(void) {
setScopeChannel(6, (int16_t)board_temp_adcFilt); // 7: for board temperature calibration
setScopeChannel(7, (int16_t)board_temp_deg_c); // 8: for verifying board temperature calibration
consoleScope();
// ####### FEEDBACK SERIAL OUT #######
#elif defined(FEEDBACK_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART3)
if(UART_DMA_CHANNEL->CNDTR == 0) {
Feedback.start = (uint16_t)START_FRAME;
Feedback.cmd1 = (int16_t)cmd1;
Feedback.cmd2 = (int16_t)cmd2;
Feedback.speedR = (int16_t)speedR;
Feedback.speedL = (int16_t)speedL;
Feedback.speedR_meas = (int16_t)rtY_Left.n_mot;
Feedback.speedL_meas = (int16_t)rtY_Right.n_mot;
Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC);
Feedback.boardTemp = (int16_t)board_temp_deg_c;
Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL
^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp);
UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN;
UART_DMA_CHANNEL->CNDTR = sizeof(Feedback);
UART_DMA_CHANNEL->CMAR = (uint32_t)&Feedback;
UART_DMA_CHANNEL->CCR |= DMA_CCR_EN;
}
#endif
}
HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
@ -385,9 +468,9 @@ int main(void) {
} else if (batVoltage < BAT_LOW_LVL2 && batVoltage >= BAT_LOW_DEAD && BAT_LOW_LVL2_ENABLE) { // low bat 2: fast beep
buzzerFreq = 5;
buzzerPattern = 6;
} else if (errCode_Left || errCode_Right) { // beep in case of Motor error - fast beep
buzzerFreq = 6;
buzzerPattern = 2;
} else if (errCode_Left || errCode_Right || timeoutFlag) { // beep in case of Motor error or serial timeout - fast beep
buzzerFreq = 12;
buzzerPattern = 1;
} else if (BEEPS_BACKWARD && speed < -50) { // backward beep
buzzerFreq = 5;
buzzerPattern = 1;

View File

@ -44,58 +44,67 @@ ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
I2C_HandleTypeDef hi2c2;
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart3_tx;
volatile adc_buf_t adc_buffer;
#ifdef CONTROL_SERIAL_USART2
#if defined(CONTROL_SERIAL_USART2) || defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
void UART2_Init(void) {
void UART_Control_Init(void) {
GPIO_InitTypeDef GPIO_InitStruct;
__HAL_RCC_USART2_CLK_ENABLE();
/* DMA1_Channel6_IRQn interrupt configuration */
/* The code below is commented out - otwerwise Serial Receive does not work */
// #ifdef CONTROL_SERIAL_USART2
// /* DMA1_Channel6_IRQn interrupt configuration */
// HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 6);
// HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 5, 6);
HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
/* DMA1_Channel7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 5, 7);
HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);
huart2.Instance = USART2;
huart2.Init.BaudRate = CONTROL_BAUD;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
// huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
// /* DMA1_Channel7_IRQn interrupt configuration */
// HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 5, 7);
// HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);
// #endif
// Disable serial interrupt - it is not needed
HAL_NVIC_DisableIRQ(DMA1_Channel6_IRQn); // Rx Channel
HAL_NVIC_DisableIRQ(DMA1_Channel7_IRQn); // Tx Channel
__HAL_RCC_DMA1_CLK_ENABLE();
/* USER CODE BEGIN USART2_MspInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
/* USER CODE END USART2_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_USART2_CLK_ENABLE();
GPIO_InitStruct.Pull = GPIO_PULLUP; //GPIO_NOPULL;
huart2.Instance = USART2;
huart2.Init.BaudRate = USART2_BAUD;
huart2.Init.WordLength = USART2_WORDLENGTH;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
#if defined(CONTROL_SERIAL_USART2)
huart2.Init.Mode = UART_MODE_TX_RX;
#elif defined(DEBUG_SERIAL_USART2)
huart2.Init.Mode = UART_MODE_TX;
#endif
HAL_UART_Init(&huart2);
#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
#endif
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Pull = GPIO_PULLUP; //GPIO_NOPULL;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
#ifdef CONTROL_SERIAL_USART2
GPIO_InitStruct.Pin = GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT; //GPIO_MODE_AF_PP;
// GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* Peripheral DMA init*/
hdma_usart2_rx.Instance = DMA1_Channel6;
hdma_usart2_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart2_rx.Init.PeriphInc = DMA_PINC_DISABLE;
@ -105,8 +114,8 @@ void UART_Control_Init(void) {
hdma_usart2_rx.Init.Mode = DMA_CIRCULAR; //DMA_NORMAL;
hdma_usart2_rx.Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_Init(&hdma_usart2_rx);
__HAL_LINKDMA(&huart2, hdmarx, hdma_usart2_rx);
#endif
hdma_usart2_tx.Instance = DMA1_Channel7;
hdma_usart2_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
@ -117,28 +126,57 @@ void UART_Control_Init(void) {
hdma_usart2_tx.Init.Mode = DMA_NORMAL;
hdma_usart2_tx.Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_Init(&hdma_usart2_tx);
__HAL_LINKDMA(&huart2,hdmatx,hdma_usart2_tx);
}
#ifdef CONTROL_SERIAL_USART2
__HAL_LINKDMA(&huart2, hdmatx, hdma_usart2_tx);
#endif
#if defined(FEEDBACK_SERIAL_USART2) || defined(DEBUG_SERIAL_USART2)
DMA1_Channel7->CPAR = (uint32_t) & (USART2->DR);
DMA1_Channel7->CNDTR = 0;
DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
#endif
#ifdef DEBUG_SERIAL_USART3
void UART_Init(void) {
__HAL_RCC_USART3_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
}
#endif
#if defined(CONTROL_SERIAL_USART3) || defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
void UART3_Init(void) {
/* The code below is commented out - otwerwise Serial Receive does not work */
// #ifdef CONTROL_SERIAL_USART3
// /* DMA1_Channel3_IRQn interrupt configuration */
// HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 3);
// HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
// /* DMA1_Channel2_IRQn interrupt configuration */
// HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 5, 2);
// HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
// #endif
// Disable serial interrupt - it is not needed
HAL_NVIC_DisableIRQ(DMA1_Channel3_IRQn); // Rx Channel
HAL_NVIC_DisableIRQ(DMA1_Channel2_IRQn); // Tx Channel
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_USART3_CLK_ENABLE();
UART_HandleTypeDef huart3;
huart3.Instance = USART3;
huart3.Init.BaudRate = DEBUG_BAUD;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.BaudRate = USART3_BAUD;
huart3.Init.WordLength = USART3_WORDLENGTH;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
huart3.Init.Mode = UART_MODE_TX;
huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart3.Init.OverSampling = UART_OVERSAMPLING_16;
#if defined(CONTROL_SERIAL_USART3)
huart3.Init.Mode = UART_MODE_TX_RX;
#elif defined(DEBUG_SERIAL_USART3)
huart3.Init.Mode = UART_MODE_TX;
#endif
HAL_UART_Init(&huart3);
#if defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
USART3->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
#endif
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_10;
@ -147,80 +185,45 @@ void UART_Init(void) {
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
DMA1_Channel2->CCR = 0;
#ifdef CONTROL_SERIAL_USART3
GPIO_InitStruct.Pin = GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Peripheral DMA init*/
hdma_usart3_rx.Instance = DMA1_Channel3;
hdma_usart3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart3_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart3_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart3_rx.Init.Mode = DMA_CIRCULAR; //DMA_NORMAL;
hdma_usart3_rx.Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_Init(&hdma_usart3_rx);
__HAL_LINKDMA(&huart3, hdmarx, hdma_usart3_rx);
#endif
hdma_usart3_tx.Instance = DMA1_Channel2;
hdma_usart3_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart3_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart3_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart3_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart3_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart3_tx.Init.Mode = DMA_NORMAL;
hdma_usart3_tx.Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_Init(&hdma_usart3_tx);
#ifdef CONTROL_SERIAL_USART3
__HAL_LINKDMA(&huart3, hdmatx, hdma_usart3_tx);
#endif
#if defined(FEEDBACK_SERIAL_USART3) || defined(DEBUG_SERIAL_USART3)
DMA1_Channel2->CPAR = (uint32_t) & (USART3->DR);
DMA1_Channel2->CNDTR = 0;
DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
#endif
}
#endif
#ifdef DEBUG_SERIAL_USART2
void UART_Init(void) {
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
UART_HandleTypeDef huart2;
huart2.Instance = USART2;
huart2.Init.BaudRate = DEBUG_BAUD;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
DMA1_Channel7->CCR = 0;
DMA1_Channel7->CPAR = (uint32_t) & (USART2->DR);
DMA1_Channel7->CNDTR = 0;
DMA1_Channel7->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
}
#endif
/*
void UART_Init(void) {
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
UART_HandleTypeDef huart2;
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS;
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
DMA1_Channel7->CCR = 0;
DMA1_Channel7->CPAR = (uint32_t) & (USART3->DR);
DMA1_Channel7->CNDTR = 0;
DMA1_Channel7->CCR = DMA_CCR_MINC | DMA_CCR_DIR;
DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7;
}
*/
DMA_HandleTypeDef hdma_i2c2_rx;
DMA_HandleTypeDef hdma_i2c2_tx;
@ -476,7 +479,6 @@ void MX_TIM_Init(void) {
HAL_TIM_PWM_Init(&htim_left);
sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
// sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC4REF;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_ENABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim_left, &sMasterConfig);

View File

@ -14,7 +14,7 @@ upload_protocol = stlink
; Serial Port settings (make sure the COM port is correct)
monitor_port = COM5
monitor_speed = 115200
monitor_speed = 38400
build_flags =
-I${PROJECT_DIR}/inc/