589 lines
20 KiB
C++
589 lines
20 KiB
C++
#include <string.h>
|
||
|
||
#include "tick.h"
|
||
|
||
#include "ori.h"
|
||
|
||
#include "uart.h"
|
||
#include "gps.h"
|
||
|
||
#include "pwm.h"
|
||
|
||
#include "prot.h"
|
||
#include "prot_head.h"
|
||
|
||
Point PointGPS{0,0,0};
|
||
|
||
MAIN_FLY_MODE MainFlyMode=MAIN_FLY_MODE::WAIT;
|
||
|
||
STATUS_MODE Main_StatusMode;
|
||
SYS_MODE Main_SysMode;
|
||
|
||
const unsigned char SRC_ID = 1;
|
||
const unsigned char DST_ID = 254;
|
||
|
||
bool HeaderBegin::CheckCRC()
|
||
{
|
||
unsigned char test = 0;
|
||
for (unsigned short a = 0; a < len1; a++) test ^= data[a];
|
||
return crc == test;
|
||
}
|
||
|
||
void HeaderBegin::SetCRC()
|
||
{
|
||
unsigned char test = 0;
|
||
for (unsigned short a = 0; a < len1; a++) test ^= data[a];
|
||
crc = test;
|
||
}
|
||
|
||
enum class DataMode : unsigned char { Begin, Head, Data };
|
||
|
||
static unsigned char ProtoData[MaxLength];
|
||
static DataMode Mode = DataMode::Begin;
|
||
static unsigned long Length, Index;
|
||
|
||
static const unsigned long Wait = 100; // ms
|
||
static unsigned long Timer;
|
||
|
||
static void ProcessingMessage(HeaderMessages*);
|
||
|
||
bool MainUpdate(unsigned char byte)
|
||
{
|
||
const unsigned long tick = TICK_GetCount();
|
||
|
||
//if ((tick - Timer > 100)) // Момент долгой обработки
|
||
//{
|
||
// printf("------------> Warning: %d\n", tick - Timer);
|
||
//}
|
||
|
||
if ((Mode != DataMode::Begin) && (tick - Timer > Wait))
|
||
{
|
||
Mode = DataMode::Begin;
|
||
//printf("Error: timeout\n");
|
||
}
|
||
|
||
Timer = tick;
|
||
|
||
switch (Mode)
|
||
{
|
||
case DataMode::Begin:
|
||
{
|
||
if (byte != Global_stx) return true;
|
||
|
||
Index = 0;
|
||
ProtoData[Index++] = byte;
|
||
Length = sizeof(HeaderBegin);
|
||
Mode = DataMode::Head;
|
||
|
||
return true;
|
||
}
|
||
|
||
case DataMode::Head:
|
||
{
|
||
ProtoData[Index++] = byte;
|
||
if (Index < Length) return true;
|
||
|
||
HeaderBegin& begin = *(HeaderBegin*)ProtoData;
|
||
|
||
if ((begin.len1 != begin.len2) || (begin.len1 > MaxLength)
|
||
|| (begin.len1 < sizeof(HeaderMessages)) || Index < sizeof(HeaderBegin))
|
||
{
|
||
//printf("Error: HeaderBegin -> size=%d\n", begin.len1);
|
||
Mode = DataMode::Begin;
|
||
return true;
|
||
}
|
||
|
||
Mode = DataMode::Data;
|
||
Length += begin.len1;
|
||
|
||
return true;
|
||
}
|
||
|
||
case DataMode::Data:
|
||
{
|
||
ProtoData[Index++] = byte;
|
||
if (Index < Length) return true;
|
||
|
||
HeaderBegin& begin = *(HeaderBegin*)ProtoData;
|
||
Mode = DataMode::Begin;
|
||
if (!begin.CheckCRC())
|
||
{
|
||
//printf("Error: HeaderBegin -> CRC\n");
|
||
return true;
|
||
}
|
||
ProcessingMessage((HeaderMessages*)(ProtoData + sizeof(HeaderBegin)));
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
void SendMes(HeaderBegin* begin)
|
||
{
|
||
begin->SetCRC();
|
||
//SendData(begin, sizeof(HeaderBegin) + begin->len1);
|
||
|
||
LPUART1_Send(begin, sizeof(HeaderBegin) + begin->len1);
|
||
}
|
||
//Создает основные заголовки сообщения
|
||
|
||
static unsigned char MessageData[MaxLength];
|
||
|
||
void* CreateMessageHeaders(HeaderBegin*& begin, unsigned long size, MESSAGES_ID msg_id)
|
||
{
|
||
|
||
begin = (HeaderBegin*)MessageData;
|
||
begin->len1 = begin->len2 = sizeof(HeaderMessages) + size;
|
||
begin->stx = Global_stx;
|
||
HeaderMessages& message = *(HeaderMessages*)begin->data;
|
||
message.msgId = msg_id;
|
||
message.srcId = SRC_ID;
|
||
message.dstId = DST_ID;
|
||
message.len = size;
|
||
message.timeusec = TICK_GetCount();
|
||
|
||
return message.data;
|
||
}
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение BatteryInfo
|
||
void BatteryInfo()
|
||
{
|
||
//Опрос датчиков и систем
|
||
HeaderBegin* begin;
|
||
PayloadBatteryInfo& info = *(PayloadBatteryInfo*) CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::BatteryInfo);
|
||
|
||
info.perCharge = 95;
|
||
info.voltage = 14.5f;
|
||
info.amperage = 20.8f;
|
||
info.temp = 36.6f;
|
||
info.totalPower = 100.0f;
|
||
info.timeRemaining = 3600;
|
||
//printf("MessageTelemetry: BatteryInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
|
||
NAV_SYS_MODE PROT_Nav=NAV_SYS_MODE::gps;
|
||
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение SysInfo
|
||
void SysInfo()
|
||
{
|
||
//Опрос датчиков и систем
|
||
unsigned int count_engine = 4;
|
||
HeaderBegin* begin;
|
||
PayloadSysInfo& info = *(PayloadSysInfo*)CreateMessageHeaders(begin, sizeof(PayloadSysInfo) + count_engine * sizeof(EnginePowerInfo), MESSAGES_ID::SysInfo);
|
||
|
||
info.mode = Main_SysMode; // !!!!!!!!!!!!
|
||
info.navSys = PROT_Nav;
|
||
info.pressureBaro = DataORI.Bar.Bar;
|
||
info.statusMode = Main_StatusMode; // !!!!!!!!!!
|
||
info.engineStatus = PWM_Enable ? ENGINE_STATUS::enable : ENGINE_STATUS::disable; // !!!!!!!!!!!
|
||
info.engineCount = count_engine;
|
||
info.tempBaro = DataORI.Temp.Bar;
|
||
|
||
for (int i = 0; i < info.engineCount; i++)
|
||
{
|
||
EnginePowerInfo& pow = info.enginePower[i];
|
||
pow.power = 50;
|
||
pow.current = 1.0;
|
||
pow.engineSpeed = 100;
|
||
pow.voltage = 12.5;
|
||
pow.temp = 34.5;
|
||
}
|
||
//printf("MessageTelemetry: SysInfo -> Send\n");
|
||
|
||
SendMes(begin);
|
||
}
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение GyroInfo
|
||
void GyroInfo()
|
||
{
|
||
//Опрос датчиков и систем
|
||
HeaderBegin* begin;
|
||
PayloadGyroInfo& info = *(PayloadGyroInfo*)CreateMessageHeaders(begin, sizeof(PayloadGyroInfo), MESSAGES_ID::GyroInfo);
|
||
|
||
info.pitchGyroVel = DataORI.Gyr.X;
|
||
info.rollGyroVel = DataORI.Gyr.Y;
|
||
info.yawGyroVel = DataORI.Gyr.Z;
|
||
//printf("MessageTelemetry: GyroInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение AccelInfo
|
||
void AccelInfo()
|
||
{
|
||
//Опрос датчиков и систем
|
||
HeaderBegin* begin;
|
||
PayloadAccelInfo& info = *(PayloadAccelInfo*)CreateMessageHeaders(begin, sizeof(PayloadAccelInfo), MESSAGES_ID::AccelInfo);
|
||
|
||
info.aX = DataORI.Acc.X;
|
||
info.aY = DataORI.Acc.Y;
|
||
info.aZ = DataORI.Acc.Z;
|
||
info.pitchAccelVel = 0.0;
|
||
info.rollAccelVel = 0.0;
|
||
info.yawAccelVel = 0.0;
|
||
info.tempAccel = DataORI.Temp.Acc;
|
||
//printf("MessageTelemetry: AccelInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
|
||
static GPS_BaseInfo Main_GPS;
|
||
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение GpsInfo
|
||
void GpsInfo()
|
||
{
|
||
|
||
GPS_GetBaseInfo(Main_GPS);
|
||
|
||
//Опрос датчиков и систем
|
||
HeaderBegin* begin;
|
||
PayloadGpsInfo& info = *(PayloadGpsInfo*)CreateMessageHeaders(begin, sizeof(PayloadGpsInfo), MESSAGES_ID::GpsInfo);
|
||
|
||
info.lat = Main_GPS.lat;
|
||
info.lon = Main_GPS.lon;
|
||
info.absAlt = Main_GPS.absAlt;
|
||
info.realAlt = Main_GPS.realAlt;
|
||
info.fixType = Main_GPS.fixType;
|
||
info.hdop = Main_GPS.hdop;
|
||
info.pdop = Main_GPS.pdop;
|
||
info.vdop = Main_GPS.vdop;
|
||
info.satUsed = Main_GPS.satUsed;
|
||
info.satVisible = Main_GPS.satVisible;
|
||
info.noise = Main_GPS.noise;
|
||
info.jamming = 0.0;
|
||
info.speed = Main_GPS.speed;
|
||
info.timeUTC = Main_GPS.timeUTC;
|
||
//printf("MessageTelemetry: GpsInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение InertialInfo
|
||
void InertialInfo()
|
||
{
|
||
//Опрос датчиков и систем
|
||
HeaderBegin* begin;
|
||
PayloadInertialInfo& info = *(PayloadInertialInfo*)CreateMessageHeaders(begin, sizeof(PayloadInertialInfo), MESSAGES_ID::InertialInfo);
|
||
|
||
info.x = Main_GPS.LocalXYZ[0];
|
||
info.y = Main_GPS.LocalXYZ[1];
|
||
info.z = Main_GPS.LocalXYZ[2];
|
||
info.baroAlt = DataORI.Bar.Alt;
|
||
info.pitch = DataORI.Pitch;
|
||
info.yaw = DataORI.Yaw;
|
||
info.roll = DataORI.Roll;
|
||
info.headingDeg = DataORI.Yaw;
|
||
info.speed = Main_GPS.speed;
|
||
info.pitchVel = 1.0;
|
||
info.yawVel = 1.0;
|
||
info.rollVel = 1.0;
|
||
//printf("MessageTelemetry: InertialInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
//Формирует ответ на команду по полезной нагрузке ответа
|
||
void CommandAck(PayloadCommandAck& ack)
|
||
{
|
||
HeaderBegin* begin;
|
||
PayloadCommandAck& info = *(PayloadCommandAck*)CreateMessageHeaders(begin, sizeof(PayloadCommandAck), MESSAGES_ID::Ack);
|
||
info = ack;
|
||
//printf("MessageCommandACK: Send\n");
|
||
SendMes(begin);
|
||
}
|
||
//Обработчик комманды приземления
|
||
static void ProcessingCommandLand()
|
||
{
|
||
MainFlyMode=MAIN_FLY_MODE::STOP;
|
||
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::Land;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды остановки полета (зависание)
|
||
static void ProcessingCommandPause()
|
||
{
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::Pause;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды возврата в точку старта
|
||
static void ProcessingCommandGoHome()
|
||
{
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::GoHome;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды продолжения полета
|
||
static void ProcessingCommandContinue()
|
||
{
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::Continue;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды запуска моторов
|
||
static void ProcessingCommandStartEngine()
|
||
{
|
||
if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::START;
|
||
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::StartEngine;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды остановки моторов
|
||
static void ProcessingCommandStopEngine()
|
||
{
|
||
MainFlyMode=MAIN_FLY_MODE::WAIT;
|
||
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::StopEngine;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды изменения скорости полета
|
||
static void ProcessingCommandChangeSpeed(PayloadCommandChangeSpeed* payload)
|
||
{
|
||
// Выполняет команду...
|
||
//payload->speed;
|
||
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::ChangeSpeed;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды смены навигационной системы
|
||
static void ProcessingCommandChangeNav(PayloadCommandChangeNav* payload)
|
||
{
|
||
PROT_Nav=(NAV_SYS_MODE)payload->nav_id;
|
||
// Выполняет команду...
|
||
//payload->nav_id;
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::ChangeNav;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
}
|
||
//Обработчик комманды перемещения в точку по глобальным координатам
|
||
static void ProcessingCommandGoToGlobal(PayloadCommandGoToGlobal* payload)
|
||
{
|
||
PointGPS.Latitude=payload->lat;
|
||
PointGPS.Longitude=payload->lon;
|
||
PointGPS.Altitude=payload->abs_alt;
|
||
|
||
if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::FLY;
|
||
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::GoToGlobal;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
//Обработчик комманды перемещения в точку по локальным координатам
|
||
static void ProcessingCommandGoToLocal(PayloadCommandGoToLocal* payload)
|
||
{
|
||
// Выполняет команду...
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::GoToLocal;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
CommandAck(info);
|
||
|
||
}
|
||
|
||
//
|
||
static void ProcessingCommandSetParameter(PayloadCommandSetParameter* payload)
|
||
{
|
||
unsigned char cat_id = payload->cat_id; // ID категории
|
||
unsigned char param_id = payload->param_id; // ID параметра
|
||
DATA_TYPES param_type = payload->param_type_code; // Тип данных
|
||
unsigned char param_len = payload->param_len; // Размер массива данных
|
||
//payload->param_data; // Массив байт параметра
|
||
|
||
PayloadCommandAck info;
|
||
info.commandId = COMMANDS_NAME::SetParameter;
|
||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
CommandAck(info);
|
||
}
|
||
|
||
//Общий обработчик сообщения с командой
|
||
static void ProcessingCommand(CommandObj* commandObject) {
|
||
switch (commandObject->commandId)
|
||
{
|
||
case COMMANDS_NAME::Land: ProcessingCommandLand(); return;
|
||
case COMMANDS_NAME::Pause: ProcessingCommandPause(); return;
|
||
case COMMANDS_NAME::GoHome: ProcessingCommandGoHome(); return;
|
||
case COMMANDS_NAME::Continue: ProcessingCommandContinue(); return;
|
||
case COMMANDS_NAME::StartEngine: ProcessingCommandStartEngine(); return;
|
||
case COMMANDS_NAME::StopEngine: ProcessingCommandStopEngine(); return;
|
||
case COMMANDS_NAME::ChangeSpeed: ProcessingCommandChangeSpeed((PayloadCommandChangeSpeed*)commandObject->data); return;
|
||
case COMMANDS_NAME::ChangeNav: ProcessingCommandChangeNav((PayloadCommandChangeNav*)commandObject->data); return;
|
||
case COMMANDS_NAME::GoToGlobal: ProcessingCommandGoToGlobal((PayloadCommandGoToGlobal*)commandObject->data); return;
|
||
case COMMANDS_NAME::GoToLocal: ProcessingCommandGoToLocal((PayloadCommandGoToLocal*)commandObject->data); return;
|
||
case COMMANDS_NAME::SetParameter: ProcessingCommandSetParameter((PayloadCommandSetParameter*)commandObject->data); return;
|
||
}
|
||
}
|
||
//Обработчик сообщения тестирования связи
|
||
static void ConnectionTest()
|
||
{
|
||
HeaderBegin* begin;
|
||
CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::ConnectionTest);
|
||
|
||
begin->SetCRC();
|
||
|
||
LPUART1_Send(begin, sizeof(HeaderBegin) + begin->len1);
|
||
}
|
||
//Обработчик сообщения запроса статуса команды
|
||
static void StatusCommand(PayloadStatusCommand* payload)
|
||
{
|
||
// payload->commandId; Тут должна быть какая-то проверка текущего состояния выполнения команды по ее ID
|
||
HeaderBegin* begin;
|
||
PayloadStatusCommand& info = *(PayloadStatusCommand*)CreateMessageHeaders(begin, sizeof(PayloadStatusCommand), MESSAGES_ID::StatusCommand);
|
||
info.commandId = payload->commandId;
|
||
info.executionCode = EXEC_CODE_COMMAND::completed;
|
||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||
SendMes(begin);
|
||
}
|
||
|
||
static void CountMenuCategories()
|
||
{
|
||
//
|
||
unsigned char count_categories = 4;
|
||
unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 };
|
||
HeaderBegin* begin;
|
||
PayloadCountMenuCategories& info = *(PayloadCountMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadCountMenuCategories) + count_categories * sizeof(unsigned char), MESSAGES_ID::CountMenuCategories);
|
||
|
||
info.countCategories = count_categories;
|
||
for (int i = 0; i < info.countCategories; i++) {
|
||
info.data[i] = categoriesNamesLen[i];
|
||
}
|
||
//printf("MessageTelemetry: SysInfo -> Send\n");
|
||
SendMes(begin);
|
||
}
|
||
|
||
static void MenuCategories(PayloadMenuCategories* payload)
|
||
{
|
||
const char* names[4] = { "Main1","PID_S1","MOTOR", "ACCELER"}; // Имена категорий
|
||
unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 }; // Длины имен категорий (имена в кодировке utf-8)
|
||
unsigned char req_count = payload->countCategories;
|
||
unsigned char total_len_names = 0;
|
||
for (int i = 0; i < req_count; i++)
|
||
{
|
||
unsigned char cat_id = (payload->categories)[i];
|
||
unsigned char cat_name_len = categoriesNamesLen[(payload->categories)[i]];
|
||
total_len_names += cat_name_len;
|
||
}
|
||
HeaderBegin* begin;
|
||
PayloadMenuCategories& info = *(PayloadMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadMenuCategories) + req_count * sizeof(MenuCategori) + total_len_names * sizeof(unsigned char), MESSAGES_ID::CategoriesMenu);
|
||
info.countCategories = req_count;
|
||
MenuCategori* menu = (MenuCategori*)info.categories;
|
||
for (int i = 0; i < req_count; i++)
|
||
{
|
||
menu->id = (payload->categories)[i];
|
||
menu->count_param = 1;
|
||
menu->len_name = categoriesNamesLen[menu->id];
|
||
memcpy(menu->name, names[menu->id], menu->len_name);
|
||
menu = (MenuCategori*)(((char*)menu) + sizeof(MenuCategori) + menu->len_name);
|
||
}
|
||
SendMes(begin);
|
||
}
|
||
|
||
static void CategoriesParameters(PayloadCategoriesParameters* payload)
|
||
{
|
||
unsigned char cat_id = payload->cat_id; // id категории для поиска параметра
|
||
unsigned char param_id = payload->param_id; // id параметра для поиска параметра
|
||
// Получение параметра
|
||
const char* name = "PARAMETER1";
|
||
unsigned char len_name = 10;
|
||
float param1 = 0.231;
|
||
DATA_TYPES param_type = DATA_TYPES::dt_float_32;
|
||
//
|
||
|
||
HeaderBegin* begin;
|
||
PayloadCategoriesParameters& info = *(PayloadCategoriesParameters*)CreateMessageHeaders(begin, sizeof(PayloadCategoriesParameters) + len_name + sizeof(param1), MESSAGES_ID::CategoriesParameters);
|
||
info.cat_id = cat_id;
|
||
info.param_id = param_id;
|
||
info.param_name_len = len_name;
|
||
info.param_len = sizeof(param1);
|
||
info.param_type_code = param_type;
|
||
memcpy(info.param_data, name, len_name);
|
||
memcpy(&info.param_data[len_name], ¶m1, sizeof(param1));
|
||
SendMes(begin);
|
||
}
|
||
|
||
//void Ping(HeaderMessages* message)
|
||
//{
|
||
// HeaderBegin* begin;
|
||
// CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Ping);
|
||
// //---
|
||
// SendMes(begin);
|
||
// //---
|
||
// printf("Ping %d Pong %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
|
||
//}
|
||
//
|
||
//void Pong(HeaderMessages* message)
|
||
//{
|
||
// HeaderBegin* begin;
|
||
// CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Pong);
|
||
// //---
|
||
// SendMes(begin);
|
||
// //---
|
||
// printf("Pong %d Ping %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
|
||
//}
|
||
|
||
static void ProcessingMessage(HeaderMessages* message)
|
||
{
|
||
switch (message->msgId)
|
||
{
|
||
/*case MESSAGES_ID::Ping: Pong(message); return;
|
||
case MESSAGES_ID::Pong: Ping(message); return;*/
|
||
case MESSAGES_ID::BatteryInfo: BatteryInfo(); return;
|
||
case MESSAGES_ID::GyroInfo: GyroInfo(); return;
|
||
case MESSAGES_ID::AccelInfo: AccelInfo(); return;
|
||
case MESSAGES_ID::GpsInfo: GpsInfo(); return;
|
||
case MESSAGES_ID::InertialInfo: InertialInfo(); return;
|
||
case MESSAGES_ID::SysInfo: SysInfo(); return;
|
||
case MESSAGES_ID::Command: ProcessingCommand((CommandObj*)message->data); return;
|
||
case MESSAGES_ID::ConnectionTest: ConnectionTest(); return;
|
||
case MESSAGES_ID::StatusCommand: StatusCommand((PayloadStatusCommand*)message->data); return;
|
||
|
||
case MESSAGES_ID::CountMenuCategories: CountMenuCategories(); return;
|
||
case MESSAGES_ID::CategoriesMenu: MenuCategories((PayloadMenuCategories*)message->data); return;
|
||
case MESSAGES_ID::CategoriesParameters: CategoriesParameters((PayloadCategoriesParameters*)message->data); return;
|
||
}
|
||
}
|
||
|
||
//void TestPing()
|
||
//{
|
||
// HeaderMessages message;
|
||
// message.timeusec = GetTickCount();
|
||
// Pong(&message);
|
||
//}
|
||
|
||
void PROT_Update()
|
||
{
|
||
char byte;
|
||
unsigned char size = LPUART1_Recv(&byte, 1);
|
||
if (size) MainUpdate(byte);
|
||
}
|