void paramTask(void * prm)
{
crtpInitTaskQueue(CRTP_PORT_PARAM);
while(1) {
crtpReceivePacketBlock(CRTP_PORT_PARAM, &p);
if (p.channel==TOC_CH)
paramTOCProcess(p.data[0]);
else if (p.channel==READ_CH)
paramReadProcess(p.data[0]);
else if (p.channel==WRITE_CH)
paramWriteProcess(p.data[0], &p.data[1]);
}
}
void logTask(void * prm)
{
crtpInitTaskQueue(CRTP_PORT_LOG);
while(1) {
crtpReceivePacketBlock(CRTP_PORT_LOG, &p);
xSemaphoreTake(logLock, portMAX_DELAY);
if (p.crtp_channel==TOC_CH)
logTOCProcess(p.crtp_data[0]);
if (p.crtp_channel==CONTROL_CH)
logControlProcess();
xSemaphoreGive(logLock);
}
}
void logTask(void * prm)
{
crtpInitTaskQueue(CRTP_PORT_LOG);
while(1) {
crtpReceivePacketBlock(CRTP_PORT_LOG, &p);
//xSemaphoreTake(logLock, portMAX_DELAY);
rt_mutex_take(logLock, RT_WAITING_FOREVER);
if (p.channel==TOC_CH)
logTOCProcess(p.data[0]);
if (p.channel==CONTROL_CH)
logControlProcess();
//xSemaphoreGive(logLock);
rt_mutex_release(logLock);
}
}
void memTask(void * param)
{
crtpInitTaskQueue(CRTP_PORT_MEM);
while(1)
{
crtpReceivePacketBlock(CRTP_PORT_MEM, &p);
switch (p.channel)
{
case MEM_SETTINGS_CH:
memSettingsProcess(p.data[0]);
break;
default:
break;
}
}
}
void paramTask(void * prm)
{
crtpInitTaskQueue(CRTP_PORT_PARAM);
while(1) {
crtpReceivePacketBlock(CRTP_PORT_PARAM, &p);
if (p.channel==TOC_CH)
paramTOCProcess(p.data[0]);
else if (p.channel==READ_CH)
paramReadProcess(p.data[0]);
else if (p.channel==WRITE_CH)
paramWriteProcess(p.data[0], &p.data[1]);
else if (p.channel==MISC_CH) {
if (p.data[0] == MISC_SETBYNAME) {
int i, nzero = 0;
char *group;
char *name;
uint8_t type;
void * valPtr;
int error;
// If the packet contains at least 2 zeros in the first 28 bytes
// The packet decoding algorithm will not crash
for (i=0; i<CRTP_MAX_DATA_SIZE; i++) {
if (p.data[i] == '\0') nzero++;
}
if (nzero < 2) return;
group = (char*)&p.data[1];
name = (char*)&p.data[1+strlen(group)+1];
type = p.data[1+strlen(group)+1+strlen(name)+1];
valPtr = &p.data[1+strlen(group)+1+strlen(name)+2];
error = paramWriteByNameProcess(group, name, type, valPtr);
p.data[1+strlen(group)+1+strlen(name)+1] = error;
p.size = 1+strlen(group)+1+strlen(name)+1+1;
crtpSendPacket(&p);
}
}
}
}
void pidCrtlTask(void *param)
{
CRTPPacket p;
extern PidObject pidRollRate;
extern PidObject pidPitchRate;
extern PidObject pidYawRate;
extern PidObject pidRoll;
extern PidObject pidPitch;
extern PidObject pidYaw;
struct pidValues
{
uint16_t rateKpRP;
uint16_t rateKiRP;
uint16_t rateKdRP;
uint16_t attKpRP;
uint16_t attKiRP;
uint16_t attKdRP;
uint16_t rateKpY;
uint16_t rateKiY;
uint16_t rateKdY;
uint16_t attKpY;
uint16_t attKiY;
uint16_t attKdY;
} __attribute__((packed));
struct pidValues *pPid;
while (TRUE)
{
if (crtpReceivePacketBlock(6, &p) == pdTRUE)
{
PIDCrtlNbr pidNbr = p.crtp_channel;
switch (pidNbr)
{
case pidCtrlValues:
pPid = (struct pidValues *)p.crtp_data;
{
pidSetKp(&pidRollRate, (float)pPid->rateKpRP/100.0);
pidSetKi(&pidRollRate, (float)pPid->rateKiRP/100.0);
pidSetKd(&pidRollRate, (float)pPid->rateKdRP/100.0);
pidSetKp(&pidRoll, (float)pPid->attKpRP/100.0);
pidSetKi(&pidRoll, (float)pPid->attKiRP/100.0);
pidSetKd(&pidRoll, (float)pPid->attKdRP/100.0);
pidSetKp(&pidPitchRate, (float)pPid->rateKpRP/100.0);
pidSetKi(&pidPitchRate, (float)pPid->rateKiRP/100.0);
pidSetKd(&pidPitchRate, (float)pPid->rateKdRP/100.0);
pidSetKp(&pidPitch, (float)pPid->attKpRP/100.0);
pidSetKi(&pidPitch, (float)pPid->attKiRP/100.0);
pidSetKd(&pidPitch, (float)pPid->attKdRP/100.0);
pidSetKp(&pidYawRate, (float)pPid->rateKpY/100.0);
pidSetKi(&pidYawRate, (float)pPid->rateKiY/100.0);
pidSetKd(&pidYawRate, (float)pPid->rateKdY/100.0);
pidSetKp(&pidYaw, (float)pPid->attKpY/100.0);
pidSetKi(&pidYaw, (float)pPid->attKiY/100.0);
pidSetKd(&pidYaw, (float)pPid->attKdY/100.0);
}
break;
default:
break;
}
}
}
}
