Multiple fixes to stability. Added debug.

bldc_interface.cpp

@@ -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

@@ -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

@@ -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

@@ -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