You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
473 lines
14 KiB
473 lines
14 KiB
/*
|
|
* bsp_decode_command.c
|
|
*
|
|
* Created on: Sep 24, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "bsp_decode_command.h"
|
|
#include "bsp_Error.pb.h"
|
|
#include "main.h"
|
|
|
|
#include "pb.h"
|
|
#include "pb_encode.h"
|
|
#include "pb_common.h"
|
|
#include "fsm_state.h"
|
|
#include "motors_power_action.h"
|
|
#include "robot_move_actions.h"
|
|
|
|
#include "fsm_state_control.h"
|
|
|
|
#define success 1
|
|
#define fail 0
|
|
|
|
Cmd decoded_Cmd = Cmd_init_default;
|
|
ReCmd send_Cmd = ReCmd_init_default;
|
|
pb_istream_t i_stream =
|
|
{ 0 };
|
|
char StartDownLoadFlag = 0;
|
|
static uint32_t downloadCount = 0;
|
|
static uint8_t ReceivedData[1024];
|
|
static uint16_t calbriation;
|
|
|
|
void decode_command_and_feedback(uint8_t *buffer, uint16_t length, char sendWay,
|
|
struct UARTHandler *send_Handler)
|
|
{
|
|
|
|
// char array[1000];
|
|
// memset(array, 0, sizeof(array));
|
|
// memcpy(array, buffer, length);
|
|
//
|
|
// if (length >= 1)
|
|
// {
|
|
//
|
|
// i_stream = pb_istream_from_buffer(buffer, length);
|
|
//
|
|
// pb_decode(&i_stream, Cmd_fields, &decoded_Cmd);
|
|
//
|
|
// memcpy(&decoded_Cmd.Buff_Data,
|
|
// &buffer[length - decoded_Cmd.Buff_Data_Length],
|
|
// decoded_Cmd.Buff_Data_Length);
|
|
//
|
|
// switch (decoded_Cmd.CommadNum)
|
|
// {
|
|
//
|
|
// case 17: // 上位机获取控制权
|
|
// {
|
|
//
|
|
// switch (decoded_Cmd.Parameter0)
|
|
// {
|
|
// case 0:
|
|
// {
|
|
//
|
|
// //read CV
|
|
// CV = GF_BSP_EEPROM_Get_CV(); //重新配置CV
|
|
// is_upper_computer_take_over_control = 0;
|
|
//
|
|
// WrapInCmdAndSendMessage(send_Cmd, 10, success,
|
|
// "return contorl to Remote Controller success",
|
|
// sendWay, send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_halt_state);
|
|
// //设置电机供电为 On
|
|
// //fsm_state_set(¤t_motor_power_state, &motor_power_off_state);
|
|
// break;
|
|
// }
|
|
// case 1:
|
|
// {
|
|
//
|
|
// is_upper_computer_take_over_control = 1;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 17, success,
|
|
// "Take control success", sendWay, send_Handler);
|
|
// fsm_state_set(¤t_motor_power_state,
|
|
// &motor_power_on_state);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_halt_state);
|
|
// GV.Robot_Move_Speed = 1000;
|
|
// CV.LeftTurnSpeed = 1;
|
|
// CV.RightTurnSpeed = 1;
|
|
// break;
|
|
// }
|
|
//
|
|
// default:
|
|
// break;
|
|
//
|
|
// }
|
|
//
|
|
// break;
|
|
// }
|
|
// case 16: // 上位机手动控制机器人运动
|
|
// {
|
|
//
|
|
// switch (decoded_Cmd.Parameter0)
|
|
// {
|
|
// case 0:
|
|
// {
|
|
//
|
|
// WrapInCmdAndSendMessage(send_Cmd, 16, success,
|
|
// "manual control halt success", sendWay,
|
|
// send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_halt_state);
|
|
// break;
|
|
// }
|
|
// case 1:
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 16, success,
|
|
// "manual control forward success", sendWay,
|
|
// send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_forwards_state);
|
|
// break;
|
|
// }
|
|
// case 2:
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 16, success,
|
|
// "manual control backward success", sendWay,
|
|
// send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_backwards_state);
|
|
// break;
|
|
// }
|
|
// case 3:
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 16, success,
|
|
// "manual control turn left success", sendWay,
|
|
// send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_turn_left_state);
|
|
// break;
|
|
// }
|
|
// case 4:
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 16, success,
|
|
// "manual control turn right success", sendWay,
|
|
// send_Handler);
|
|
// fsm_state_set(¤t_robot_move_state,
|
|
// &robot_turn_right_state);
|
|
// break;
|
|
// }
|
|
// default:
|
|
// break;
|
|
//
|
|
// }
|
|
//
|
|
// break;
|
|
// }
|
|
//
|
|
// case 15: // 返回错误信息
|
|
// {
|
|
//
|
|
// pb_ostream_t Error_o_stream =
|
|
// { 0 };
|
|
// char buf[1024];
|
|
// Error_o_stream = pb_ostream_from_buffer(buf, sizeof(buf));
|
|
// pb_encode(&Error_o_stream, ErrorData_fields,
|
|
// &GV.SystemErrorData);
|
|
//
|
|
// //将数据拷贝到Command中
|
|
// send_Cmd.CommadNum = 16;
|
|
// send_Cmd.Buff_Data_Length = Error_o_stream.bytes_written;
|
|
//
|
|
// WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
|
|
//
|
|
// break;
|
|
// }
|
|
//
|
|
// case 14: //这里是初始化Flash 好重新写入
|
|
// {
|
|
// NeedToFeedBackToComputer = 1;
|
|
//
|
|
// WrapInCmdAndSendMessage(send_Cmd, 14, success,
|
|
// "disable periodical call of serialports and can",
|
|
// sendWay, send_Handler);
|
|
//
|
|
// Error_Detect_Enable = 0; //disable ErrorDetect Function;
|
|
//
|
|
// DLT_LOG_ENABLE_LEVEL = 0; //7 send all information //0 send nothing
|
|
//
|
|
// //将所有串口的周期性调用暂停
|
|
// RS_485_1_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// RS_485_2_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// RS_485_3_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// RS_485_4_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// E28_SBUS_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// LPUART1_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// InterCall_DEBUG_UART_Handler.dispacherController->Dispacher_Enable =
|
|
// 0;
|
|
// LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
|
|
// 0;
|
|
// RS_485_4_UART_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// is_udp_GV_update_loop_enalbed = 0;
|
|
//
|
|
// //将CAN总线的周期性调用停掉
|
|
// //因为电机有可能处于运动状态，所有CAN线不能停，得发送下去
|
|
// //FD_CAN_1_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// //FD_CAN_2_Handler.dispacherController->Dispacher_Enable = 0;
|
|
// fsm_state_set(¤t_robot_move_state, &robot_halt_state);
|
|
//
|
|
// break;
|
|
// }
|
|
// case 13: // 使能 GV 周期性性发送数据
|
|
// {
|
|
// if (decoded_Cmd.Parameter0 == 0) //关闭上传
|
|
// {
|
|
// NeedToFeedBackToComputer = 1;
|
|
// LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
|
|
// 0;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 14, success,
|
|
// "disable send GV", sendWay, send_Handler);
|
|
//
|
|
// is_udp_GV_update_loop_enalbed = 0;
|
|
// }
|
|
//
|
|
// if (decoded_Cmd.Parameter0 == 1) //打开上传
|
|
// {
|
|
// NeedToFeedBackToComputer = 0;
|
|
// LTE_7S0_Serial_UART_Handler.dispacherController->Dispacher_Enable =
|
|
// 1;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 14, success,
|
|
// "enable send GV", sendWay, send_Handler);
|
|
//
|
|
// is_udp_GV_update_loop_enalbed = 1;
|
|
// }
|
|
//
|
|
// break;
|
|
// }
|
|
// case 12: //重启系统，进入bootloader
|
|
// {
|
|
//
|
|
// WrapInCmdAndSendMessage(send_Cmd, 12, success, "STM32 reboot",
|
|
// sendWay, send_Handler);
|
|
// NVIC_SystemReset();
|
|
//
|
|
// break;
|
|
// }
|
|
// case 11: //配置无线网络模块，此处为透传指令
|
|
// {
|
|
// Send_WH_LTE_7S0_Data(decoded_Cmd.Buff_Data,
|
|
// decoded_Cmd.Parameter0);
|
|
// break;
|
|
// }
|
|
// case 10: //擦除flash
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 10, success,
|
|
// "start erasing flash ", sendWay, send_Handler);
|
|
// QSPI_W25Qx_EraseDownLoadFlash();
|
|
// downloadCount = 0;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 10, success,
|
|
// "erase flash success", sendWay, send_Handler);
|
|
// break;
|
|
// }
|
|
// case 9: //上位机下发保存软件代码指令
|
|
// {
|
|
// memcpy(ReceivedData,
|
|
// &buffer[length - decoded_Cmd.Buff_Data_Length],
|
|
// decoded_Cmd.Buff_Data_Length);
|
|
//
|
|
// calbriation = MB_CRC16(&ReceivedData,
|
|
// decoded_Cmd.Buff_Data_Length);
|
|
//
|
|
// if (calbriation == decoded_Cmd.Parameter4) //Parameter4是校验
|
|
// {
|
|
//
|
|
// if (downloadCount == decoded_Cmd.Parameter3)
|
|
// {
|
|
// QSPI_W25Qx_Write_Buffer(&decoded_Cmd.Buff_Data,
|
|
// CODE_DOWNLOAD_FLASH_BEGIN_ADDRESS
|
|
// + decoded_Cmd.Parameter3,
|
|
// decoded_Cmd.Buff_Data_Length);
|
|
// downloadCount += decoded_Cmd.Buff_Data_Length;
|
|
// send_Cmd.Parameter3 = length;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 9, success,
|
|
// "download success", sendWay, send_Handler);
|
|
//
|
|
// }
|
|
// else
|
|
// {
|
|
// decoded_Cmd.Parameter2 = downloadCount; //返回download count;
|
|
// send_Cmd.Parameter3 = length;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 9, fail,
|
|
// "downloadCount!=Parameter3", sendWay,
|
|
// send_Handler);
|
|
// }
|
|
//
|
|
// if (decoded_Cmd.Parameter1 == 1)
|
|
// {
|
|
// WrapInCmdAndSendMessage(send_Cmd, 9, success,
|
|
// "download new software sucess", sendWay,
|
|
// send_Handler);
|
|
// IAP_struct_define iap = GF_BSP_EEPROM_Get_IAP();//Get the version of the code
|
|
// iap.UpgradeSucceeded = 1;
|
|
// iap.Total_Bytes = decoded_Cmd.Parameter2;
|
|
// StartDownLoadFlag = 0;
|
|
// downloadCount = 0;
|
|
// GF_BSP_EEPROM_Set_IAP(iap);
|
|
//
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
//
|
|
// decoded_Cmd.Parameter2 = downloadCount; //返回download count;
|
|
// send_Cmd.Parameter3 = length;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 9, fail,
|
|
// "MB CRC failed and the length ", sendWay,
|
|
// send_Handler);
|
|
//
|
|
// }
|
|
//
|
|
// break;
|
|
// }
|
|
//
|
|
//// 定义 7 上位机获取拟合点
|
|
//// 定义 8 上位机获取位置点 */
|
|
// case 8:
|
|
// {
|
|
//// send_data_to_upper_computer(*Desulfurizer_Angle,
|
|
//// *Desulfurizer_Wire_Length, *Desulfurizer_Thickness, 0,
|
|
//// 0, desulfurizer_message_UART_Handler);
|
|
// break;
|
|
// }
|
|
// case 7:
|
|
// {
|
|
//// send_data_to_upper_computer(*Desulfurizer_Angle,
|
|
//// *Desulfurizer_Wire_Length, *Desulfurizer_Thickness, 1,
|
|
//// 0, desulfurizer_message_UART_Handler);
|
|
// break;
|
|
// }
|
|
// //定义 6 上位机设定编码器角度值为0
|
|
// case 6:
|
|
// {
|
|
// //reset_current_value_0();
|
|
// break;
|
|
// }
|
|
// //定义 4 上位机设定Trace等级值，无返回
|
|
// case 4:
|
|
// {
|
|
//
|
|
// break;
|
|
// }
|
|
// //定义 3 上位机设定CV值
|
|
// case 3:
|
|
// {
|
|
// CV_struct_define decoded_CV = CV_struct_define_init_default;
|
|
// pb_istream_t i_cv_stream =
|
|
// { 0 };
|
|
//
|
|
// i_cv_stream = pb_istream_from_buffer(decoded_Cmd.Buff_Data,
|
|
// decoded_Cmd.Buff_Data_Length);
|
|
// pb_decode(&i_cv_stream, CV_struct_define_fields, &decoded_CV);
|
|
// //将CV写入EEPROM
|
|
// GF_BSP_EEPROM_Set_CV(decoded_CV);
|
|
// CV = decoded_CV;
|
|
// send_Cmd.Parameter3 = length;
|
|
// WrapInCmdAndSendMessage(send_Cmd, 3, success,
|
|
// "Set CV succeeded", sendWay, send_Handler);
|
|
// CV_GV_Init();
|
|
// break;
|
|
// }
|
|
// //定义 1 上位机获取默认CV值, 下位机返回CV的值，
|
|
// //定义 2 下位机返回CV值
|
|
// case 1:
|
|
// {
|
|
// CV = GF_BSP_EEPROM_Get_CV();
|
|
// pb_ostream_t CV_o_stream =
|
|
// { 0 };
|
|
// char buf[1024];
|
|
// CV_o_stream = pb_ostream_from_buffer(buf, sizeof(buf));
|
|
// pb_encode(&CV_o_stream, CV_struct_define_fields, &CV);
|
|
//
|
|
// //将数据拷贝到Command中
|
|
// send_Cmd.CommadNum = 2;
|
|
// send_Cmd.Buff_Data_Length = CV_o_stream.bytes_written;
|
|
//
|
|
// WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
|
|
//
|
|
// break;
|
|
// }
|
|
// default:
|
|
// WrapInCmdAndSendMessage(send_Cmd, -1, fail,
|
|
// "Not the right command", sendWay, send_Handler);
|
|
// break;
|
|
//
|
|
// }
|
|
//
|
|
// }
|
|
// else
|
|
// {
|
|
//
|
|
// }
|
|
|
|
}
|
|
void WrapInCmdAndSend(ReCmd send_Cmd, uint8_t *buf, int8_t sendWay,
|
|
struct UARTHandler *send_Handler)
|
|
{
|
|
|
|
memcpy(send_Cmd.Buff_Data, buf, send_Cmd.Buff_Data_Length);
|
|
pb_ostream_t ReCmd_out_stream =
|
|
{ 0 };
|
|
|
|
if (sendWay == 1) //MQTT
|
|
{
|
|
ReCmd_out_stream = pb_ostream_from_buffer(&send_Handler->Tx_Buf[4],
|
|
sizeof(send_Handler->Tx_Buf) - 4);
|
|
pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer
|
|
|
|
send_Handler->Tx_Buf[0] = '3';
|
|
send_Handler->Tx_Buf[1] = ',';
|
|
send_Handler->Tx_Buf[2] = 0xfe;
|
|
send_Handler->Tx_Buf[3] = 0xfe;
|
|
send_Handler->TxCount = ReCmd_out_stream.bytes_written + 4;
|
|
send_Handler->UART_Tx(send_Handler);
|
|
}
|
|
else if (sendWay == 2) //UDP 发送
|
|
{
|
|
uint8_t Tx_Buf[2048];
|
|
ReCmd_out_stream = pb_ostream_from_buffer(&Tx_Buf[2],
|
|
sizeof(Tx_Buf) - 2);
|
|
pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer
|
|
Tx_Buf[0] = 0xfe;
|
|
Tx_Buf[1] = 0xfe;
|
|
udp_cmd_send_back(Tx_Buf, ReCmd_out_stream.bytes_written + 2);
|
|
|
|
}
|
|
else //485发送
|
|
{
|
|
ReCmd_out_stream = pb_ostream_from_buffer(&send_Handler->Tx_Buf[2],
|
|
sizeof(send_Handler->Tx_Buf) - 2);
|
|
pb_encode(&ReCmd_out_stream, ReCmd_fields, &send_Cmd); //encode to buffer
|
|
send_Handler->Tx_Buf[0] = 0xfe;
|
|
send_Handler->Tx_Buf[1] = 0xfe;
|
|
//send_Handler->AddSendList(send_Handler,send_Handler->Tx_Buf,send_Handler->TxCount,NULL);
|
|
send_Handler->TxCount = ReCmd_out_stream.bytes_written + 2;
|
|
//send_Handler->TxCount = 200;
|
|
send_Handler->UART_Tx(send_Handler);
|
|
}
|
|
|
|
}
|
|
|
|
//send_Cmd.CommadNum=0xff;//代表返回信息
|
|
//send_Cmd.Parameter0 代表执行的指令，发过来的命令号;
|
|
//send_Cmd.Parameter1 1 代表成功，0 代表失败;
|
|
//send_Cmd.databuf 存的是相关信息，可以通过ACII转成string;
|
|
|
|
void WrapInCmdAndSendMessage(ReCmd send_Cmd, int8_t functionNum,
|
|
int8_t isSuccess, uint8_t *buf, int8_t sendWay,
|
|
struct UARTHandler *send_Handler)
|
|
{
|
|
|
|
send_Cmd.CommadNum = 0xff; //代表返回信息
|
|
send_Cmd.Parameter0 = functionNum; //代表返回信息
|
|
send_Cmd.Parameter1 = isSuccess; //1 代表成功，0 代表失败;
|
|
send_Cmd.Buff_Data_Length = strlen(buf);
|
|
WrapInCmdAndSend(send_Cmd, buf, sendWay, send_Handler);
|
|
}
|
|
|
|
void send_received_data_to_upper_computer(uint8_t *buffer, uint16_t length)
|
|
{
|
|
send_Cmd.CommadNum = 12; //定义12为设定Mqtt透传向上返回的数据
|
|
send_Cmd.Buff_Data_Length = length;
|
|
|
|
WrapInCmdAndSend(send_Cmd, buffer, 0, desulfurizer_message_UART_Handler);
|
|
|
|
}
|
|
|