Skip to content

B
bldc_uart_comm_stm32f4_discovery
Commit 85914615 authored by service-config's avatar service-config
Browse files
Options

Multiple fixes to stability. Added debug.

parent 56afc6c3
Hide whitespace changes
Inline Side-by-side
Showing with 171 additions and 46 deletions
bldc_interface.cpp
View file @ 85914615
......@@ -127,6 +127,9 @@ void bldc_interface_send_packet(unsigned char *data, int len) {
* The length of the buffer.
*/
void bldc_interface_process_packet(unsigned char *data, int len) {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] New packet to process!"));
#endif
if (!len) {
return;
}
......@@ -144,6 +147,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
switch (id) {
case COMM_FW_VERSION:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_FW_VERSION = ?"));
#endif
if (len == 2) {
ind = 0;
fw_major = data[ind++];
......@@ -155,7 +161,15 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_ERASE_NEW_APP:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_ERASE_NEW_APP = ?"));
#endif
// TODO
break;
case COMM_WRITE_NEW_APP_DATA:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_WRITE_NEW_APP_DATA = ?"));
#endif
// TODO
break;
......@@ -202,6 +216,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_PRINT:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_PRINT = ?"));
#endif
if (rx_printf_func) {
data[len] = '\0';
rx_printf_func((char*)data);
......@@ -209,10 +226,16 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_SAMPLE_PRINT:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_SAMPLE_PRINT = ?"));
#endif
// TODO
break;
case COMM_ROTOR_POSITION:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_ROTOR_POSITION = ?"));
#endif
ind = 0;
rotor_pos = buffer_get_float32(data, 100000.0, &ind);
......@@ -222,11 +245,22 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_EXPERIMENT_SAMPLE:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_EXPERIMENT_SAMPLE = ?"));
#endif
// TODO
break;
case COMM_GET_MCCONF:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_MCCONF = ?"));
#endif
//TODO
break;
case COMM_GET_MCCONF_DEFAULT:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_MCCONF_DEFAULT = ?"));
#endif
ind = 0;
mcconf.pwm_mode = (mc_pwm_mode)data[ind++];
mcconf.comm_mode = (mc_comm_mode)data[ind++];
......@@ -319,7 +353,15 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_GET_APPCONF:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_APPCONF = ?"));
#endif
// TODO
break;
case COMM_GET_APPCONF_DEFAULT:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_APPCONF_DEFAULT = ?"));
#endif
ind = 0;
appconf.controller_id = data[ind++];
appconf.timeout_msec = buffer_get_uint32(data, &ind);
......@@ -387,6 +429,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_DETECT_MOTOR_PARAM:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_DETECT_MOTOR_PARAM = ?"));
#endif
ind = 0;
detect_cycle_int_limit = buffer_get_float32(data, 1000.0, &ind);
detect_coupling_k = buffer_get_float32(data, 1000.0, &ind);
......@@ -402,22 +447,37 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_DETECT_MOTOR_R_L: {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_DETECT_MOTOR_R_L = ?"));
#endif
// TODO!
} break;
case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_DETECT_MOTOR_FLUX_LINKAGE = ?"));
#endif
// TODO!
} break;
case COMM_DETECT_ENCODER: {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_DETECT_ENCODER = ?"));
#endif
// TODO!
} break;
case COMM_DETECT_HALL_FOC: {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_DETECT_HALL_FOC = ?"));
#endif
// TODO!
} break;
case COMM_GET_DECODED_PPM:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_DECODED_PPM = ?"));
#endif
ind = 0;
dec_ppm = buffer_get_float32(data, 1000000.0, &ind);
dec_ppm_len = buffer_get_float32(data, 1000000.0, &ind);
......@@ -428,6 +488,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_GET_DECODED_ADC:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_DECODED_ADC = ?"));
#endif
ind = 0;
dec_adc = buffer_get_float32(data, 1000000.0, &ind);
dec_adc_voltage = buffer_get_float32(data, 1000000.0, &ind);
......@@ -439,6 +502,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_GET_DECODED_CHUK:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_GET_DECODED_CHUK = ?"));
#endif
ind = 0;
dec_chuk = buffer_get_float32(data, 1000000.0, &ind);
......@@ -448,6 +514,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_SET_MCCONF:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_SET_MCCONF = ?"));
#endif
// This is a confirmation that the new mcconf is received.
if (rx_mcconf_received_func) {
rx_mcconf_received_func();
......@@ -455,6 +524,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
case COMM_SET_APPCONF:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] COMM_SET_APPCONF = ?"));
#endif
// This is a confirmation that the new appconf is received.
if (rx_appconf_received_func) {
rx_appconf_received_func();
......@@ -462,6 +534,9 @@ void bldc_interface_process_packet(unsigned char *data, int len) {
break;
default:
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] default = ?"));
#endif
break;
}
}
......@@ -528,8 +603,7 @@ void bldc_interface_set_rx_appconf_received_func(void(*func)(void)) {
void bldc_interface_terminal_cmd(char* cmd) {
int send_index = 0;
int len = strlen(cmd);
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_TERMINAL_CMD;
memcpy(send_buffer + send_index, cmd, len);
send_index += len;
......@@ -538,8 +612,7 @@ void bldc_interface_terminal_cmd(char* cmd) {
void bldc_interface_set_duty_cycle(float dutyCycle) {
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_DUTY;
buffer_append_float32(send_buffer, dutyCycle, 100000.0, &send_index);
send_packet_no_fwd(send_buffer, send_index);
......@@ -548,26 +621,24 @@ void bldc_interface_set_duty_cycle(float dutyCycle) {
void bldc_interface_set_current(float current) {
uint8_t send_buffer[5];
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_CURRENT;
buffer_append_float32(send_buffer, current, 1000.0, &send_index);
//Serial.write(send_buffer, send_index); Serial.println("");
send_packet_no_fwd(send_buffer, send_index);
}
/*void bldc_interface_set_current_brake(float current) {
void bldc_interface_set_current_brake(float current) {
uint8_t send_buffer[5];
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_CURRENT_BRAKE;
buffer_append_float32(send_buffer, current, 1000.0, &send_index);
send_packet_no_fwd(send_buffer, send_index);
}
void bldc_interface_set_rpm(int rpm) {
/*void bldc_interface_set_rpm(int rpm) {
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_RPM;
buffer_append_int32(send_buffer, rpm, &send_index);
send_packet_no_fwd(send_buffer, send_index);
......@@ -575,8 +646,7 @@ void bldc_interface_set_rpm(int rpm) {
void bldc_interface_set_pos(float pos) {
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_POS;
buffer_append_float32(send_buffer, pos, 1000000.0, &send_index);
send_packet_no_fwd(send_buffer, send_index);
......@@ -584,8 +654,7 @@ void bldc_interface_set_pos(float pos) {
void bldc_interface_set_servo_pos(float pos) {
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_SERVO_POS;
buffer_append_float16(send_buffer, pos, 1000.0, &send_index);
send_packet_no_fwd(send_buffer, send_index);
......@@ -593,8 +662,7 @@ void bldc_interface_set_servo_pos(float pos) {
void bldc_interface_set_mcconf(const mc_configuration *mcconf) {
int send_index = 0;
//fwd_can_append(send_buffer, &send_index);
fwd_can_append();
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_SET_MCCONF;
send_buffer[send_index++] = mcconf->pwm_mode;
......@@ -764,7 +832,7 @@ void bldc_interface_get_fw_version(void) {
}
*/
void bldc_interface_get_values(void) {
uint8_t send_buffer[3];
unsigned char send_buffer[3];
int send_index = 0;
fwd_can_append(send_buffer, &send_index);
send_buffer[send_index++] = COMM_GET_VALUES;
......
comm_uart_arduino.cpp
View file @ 85914615
......@@ -27,7 +27,7 @@
// Settings
#define __CUA_UART_BAUDRATE 9600
#define __CUA_UART_BAUDRATE 9600 //115200
// Private functions
static void send_packet(unsigned char *data, int len);
......@@ -58,40 +58,49 @@ void softwareSerialEvent() {
bool isWork = false;
#ifdef DEBUG_BLDC
int len = 0;
uint8_t rbyte;
//uint8_t rbyte;
#endif
if(mySerial.available()) {
isWork = true;
//isWork = true;
#ifdef DEBUG_BLDC
Serial.print(F("[ bldc ] softwareSerialEvent = read["));
Serial.println(F("[ bldc ] softwareSerialEvent: Reading from serial..."));
#endif
}
while (mySerial.available()) {
while (mySerial.available()) {
//#ifndef DEBUG_BLDC
bldc_interface_uart_process_byte(mySerial.read());
#ifdef DEBUG_BLDC
len += sizeof(uint8_t);
#endif
/*#else
rbyte = mySerial.read();
bldc_interface_uart_process_byte(rbyte);
Serial.write(rbyte);
len += sizeof(uint8_t);
#endif*/
//delay(2);
}
#ifndef DEBUG_BLDC
bldc_interface_uart_process_byte(mySerial.read());
#else
rbyte = mySerial.read();
bldc_interface_uart_process_byte(rbyte);
Serial.write(rbyte);
len += sizeof(uint8_t);
#endif
}
if(isWork) {
#ifdef DEBUG_BLDC
Serial.print(F("] len["));
Serial.print(F("[ bldc ] softwareSerialEvent: Just read "));
Serial.print(len);
Serial.println(F("]"));
Serial.println(F(" bytes."));
#endif
is_data_ready = true;
}
else {
#ifdef DEBUG_BLDC
Serial.println(F("[ bldc ] softwareSerialEvent: nothing to do... "));
#endif
}
}
/**
......@@ -102,20 +111,24 @@ void softwareSerialEvent() {
* @param len
* Data array length
*/
static void send_packet(unsigned char *data, int len) {
void send_packet(unsigned char *data, int len) {
/*if (len > (PACKET_MAX_PL_LEN + 5)) {
return;
}*/
//data[0] = 4;
// Send the data over UART
#ifdef DEBUG_BLDC
Serial.print(F("[ bldc ] send_packet = write["));
Serial.write(data, len);
Serial.print(F("] len["));
Serial.print(F("[ bldc ] send_packet = len["));
Serial.print(len);
Serial.print(F("] write["));
for (int i = 0; i < len ; i++){
Serial.print(data[i]);
Serial.print(F(":"));
}
//Serial.write(data, len);
Serial.println(F("]"));
#endif
mySerial.write(data, len);
}
......
packet.cpp
View file @ 85914615
......@@ -86,20 +86,38 @@ void packet_timerfunc(void) {
handler_states[i].rx_timeout--;
} else {
handler_states[i].rx_state = 0;
/*#ifdef DEBUG_BLDC_P
Serial.println(F("[ bldc ] packet_timerfunc: Timeout!"));
#endif*/
}
}
}
void packet_process_byte(uint8_t rx_data, int handler_num) {
#ifdef DEBUG_BLDC_P
Serial.print(F("[ bldc ] packet_process_byte = rx_data[ "));
Serial.print(rx_data);
Serial.print(F(" ] handler_states.rx_data[ "));
Serial.print(handler_states[handler_num].rx_state);
Serial.println(F(" ]"));
#endif
switch (handler_states[handler_num].rx_state) {
case 0:
if (rx_data == 2) {
#ifdef DEBUG_BLDC_P
Serial.println(F("[ bldc ] packet_process_byte: 1 byte PL len"));
#endif
// 1 byte PL len
handler_states[handler_num].rx_state += 2;
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
handler_states[handler_num].rx_data_ptr = 0;
handler_states[handler_num].payload_length = 0;
} else if (rx_data == 3) {
#ifdef DEBUG_BLDC_P
Serial.println(F("[ bldc ] packet_process_byte: 2 byte PL len"));
#endif
// 2 byte PL len
handler_states[handler_num].rx_state++;
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
......@@ -112,12 +130,25 @@ void packet_process_byte(uint8_t rx_data, int handler_num) {
case 1:
handler_states[handler_num].payload_length = (unsigned int)rx_data << 8;
#ifdef DEBUG_BLDC_P
Serial.print(F("[ bldc ] packet_process_byte = payload_length[ "));
Serial.print( handler_states[handler_num].payload_length );
Serial.println(F(" ]"));
#endif
handler_states[handler_num].rx_state++;
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
break;
case 2:
handler_states[handler_num].payload_length |= (unsigned int)rx_data;
#ifdef DEBUG_BLDC_P
Serial.print(F("[ bldc ] packet_process_byte = payload_length[ "));
Serial.print( handler_states[handler_num].payload_length );
Serial.println(F(" ]"));
#endif
if (handler_states[handler_num].payload_length > 0 &&
handler_states[handler_num].payload_length <= PACKET_MAX_PL_LEN) {
handler_states[handler_num].rx_state++;
......@@ -132,18 +163,31 @@ void packet_process_byte(uint8_t rx_data, int handler_num) {
if (handler_states[handler_num].rx_data_ptr == handler_states[handler_num].payload_length) {
handler_states[handler_num].rx_state++;
}
#ifdef DEBUG_BLDC_P
Serial.print(F("[ bldc ] packet_process_byte = Received "));
Serial.print( handler_states[handler_num].rx_data_ptr );
Serial.print(F(" bytes of "));
Serial.print( handler_states[handler_num].payload_length );
Serial.println(F("."));
#endif
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
break;
case 4:
handler_states[handler_num].crc_high = rx_data;
handler_states[handler_num].rx_state++;
#ifdef DEBUG_BLDC_P
Serial.println(F("[ bldc ] packet_process_byte = Received crc_high byte."));
#endif
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
break;
case 5:
handler_states[handler_num].crc_low = rx_data;
handler_states[handler_num].rx_state++;
#ifdef DEBUG_BLDC_P
Serial.println(F("[ bldc ] packet_process_byte = Received crc_low byte."));
#endif
handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
break;
......
packet.h
View file @ 85914615
......@@ -31,7 +31,7 @@
//#define DEBUG_BLDC_P
// Settings
#define PACKET_RX_TIMEOUT 2
#define PACKET_RX_TIMEOUT 100
#define PACKET_HANDLERS 1
#define PACKET_MAX_PL_LEN 128 //256
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment