594 lines
19 KiB
C++
594 lines
19 KiB
C++
#include <string.h>
|
|
|
|
#include "tick.h"
|
|
|
|
#include "ori.h"
|
|
|
|
#include "uart.h"
|
|
#include "gps.h"
|
|
|
|
#include "pwm.h"
|
|
|
|
#include "quat.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);
|
|
|
|
UART2_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);
|
|
}
|
|
|
|
extern Vec3 test_iner;
|
|
|
|
//?????????? ??????? ????????? ??????????. ?????????? ??????? ? ???????, ????????? ????????? InertialInfo
|
|
void InertialInfo()
|
|
{
|
|
//????? ???????? ? ??????
|
|
HeaderBegin* begin;
|
|
PayloadInertialInfo& info = *(PayloadInertialInfo*)CreateMessageHeaders(begin, sizeof(PayloadInertialInfo), MESSAGES_ID::InertialInfo);
|
|
|
|
info.x = test_iner.x;
|
|
info.y = test_iner.y;
|
|
info.z = test_iner.z;
|
|
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();
|
|
|
|
UART2_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 = UART2_Recv(&byte, 1);
|
|
if (size) MainUpdate(byte);
|
|
}
|