void gpsTask(void *unused)
{
gpsData.serial = serialOpen(USART1, 9600);
gpsData.mode = MODE_NMEA; // NMEA
while (1) {
CoTickDelay(25);
while (uartAvailable(gpsData.serial)) {
switch (gpsData.mode) {
case MODE_NMEA: // NMEA
gpsData.validFrames += gpsNewFrameNMEA(uartRead(gpsData.serial));
break;
case MODE_UBX: // UBX
gpsData.validFrames += gpsNewFrameUBLOX(uartRead(gpsData.serial));
break;
case MODE_MTK: // MTK
break;
case MODE_PASSTHROUGH: // GPS -> UART bridge
// TODO
// usbSerialWrite(uartRead(gpsData.serial));
break;
}
}
}
}
int main(void){
uint16_t byteCount = 0;
char buffer0[32];
HAL_Init();
SYS_TimerInit();
PHY_Init();
NWK_Init();
//SYS_INIT()
timer3Init();
uartInit();
sei();
while (1){
SYS_TaskHandler();
byteCount = uartAvailable();
if(byteCount > 0){
HAL_UartBytesReceived(byteCount);
sprintf(buffer0, "Bytes to send: %i\n", byteCount);
uartPuts(buffer0);
}
APP_TaskHandler();
}
}
static void getPos(void)
{
bool valid_pos = false;
uint32_t timeout = millis();
do
{
if (uartAvailable())
valid_pos = gpsDecode((char)uartRead());
} while((millis() - timeout < VALID_POS_TIMEOUT) && !valid_pos);
}
int main(void)
{
unsigned char BufC;
uint16_t BufW;
/* 系统初始化*/
systemInit();
delay_ms(300);
printf("LEADIY-M3 TEST V2\r\n");
while (1)
{
if(uartAvailable()) //检测是否收到LEADIY-M3数据
{
BufC = uartRead(); //读一个字节
if(BufC==0xA7){ //判断是否为帧 头
BufC = uartRead(); //读一个字节
if (BufC==0x7A) { //判断是否为帧头
BufC = uartRead(); //读一个字节
switch(BufC) //帧类型判断
{
case 0x70: //标识为角速度帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
Gyro[0] = ReadW(); //X轴
Gyro[1] = ReadW(); //Y轴
Gyro[2] = ReadW(); //Z轴
/*GYRO的量程为2000度每秒*/
/* 得到以"dps" ("度/秒")为单位的角速度值*/
GyroDPS[0] = (float)Gyro[0]*4/16.4;
GyroDPS[1] = (float)Gyro[1]*4/16.4;
GyroDPS[2] = (float)Gyro[2]*4/16.4;
printf("GYRO X：%d Y：%d Z：%d\r\n", Gyro[0], Gyro[1], Gyro[2]);
printf("GyroDPS X: %.2f, Y: %.2f, Z: %.2f\r\n", GyroDPS[0], GyroDPS[1], GyroDPS[2]);
break;
case 0x71: //标识为加速度帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
Acc[0] = ReadW(); //X轴
Acc[1] = ReadW(); //Y轴
Acc[2] = ReadW(); //Z轴
/*ACC的量程为8G*/
/*得到以"g"为单位的加速度*/
AccG[0] = (float)Acc[0] / 4096;
AccG[1] = (float)Acc[1] / 4096;
AccG[2] = (float)Acc[2] / 4096;
printf("ACC X：%d Y：%d Z：%d\r\n", Acc[0], Acc[1], Acc[2]);
printf("AccG X：%.2f Y：%.2f Z：%.2f\r\n", AccG[0], AccG[1], AccG[2]);
break;
case 0x72: //标识为姿态帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
Angle[0] = ReadW(); //X轴角度(横滚)
Angle[1] = ReadW(); //Y轴角度(俯仰)
Angle[2] = ReadW(); //Z轴角度(偏航)
AngleDeg[0] = (float)Angle[0] / 100;
AngleDeg[1] = (float)Angle[1] / 100;
AngleDeg[2] = (float)Angle[2] / 100;
if (AngleDeg[2]<0) AngleDeg[2] += 360; //将航向值转化到0---360度区间
printf("ANGLE X：%d Y：%d Z：%d \r\n", Angle[0], Angle[1], Angle[2]);
printf("AngleDeg X：%.2f Y：%.2f Z：%.2f \r\n", AngleDeg[0], AngleDeg[1], AngleDeg[2]);
break;
case 0x73: //标识为 地磁帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
Mag[0] = ReadW(); //X轴
Mag[1] = ReadW(); //Y轴
Mag[2] = ReadW(); //Z轴
/*地磁设置为2.5Ga*/
/*得到以"Gauss"为单位的地磁*/
MagGauss[0] = (float)Mag[0] / 660;
MagGauss[1] = (float)Mag[1] / 660;
MagGauss[2] = (float)Mag[2] / 660;
printf("Mag X：%d Y：%d Z：%d \r\n", Mag[0], Mag[1], Mag[2]);
printf("MagGauss X：%.2f Y：%.2f Z：%.2f \r\n", MagGauss[0], MagGauss[1], MagGauss[2]);
break;
case 0x74: //标识为温度、气压帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
Temper = ReadW() / 10; //X轴
BufW = ReadW(); //气压低16位
Pressure = (int32_t)(((uint32_t)(ReadW() << 16))|BufW);
printf("Temperature(Degree)：%.2f Pressure(Pa)：%d \r\n", Temper, Pressure);
break;
case 0x75: // 标识为高度帧
BufC = uartRead(); //帧长度，可不用
BufC = uartRead(); //效验位
BufW = ReadW(); //高度低16位
/*得到以"CM"为单位的海拔高度*/
Altitude = (int32_t)(((uint32_t)(ReadW() << 16))|BufW);
printf("Altitude(cm)：%d \r\n", Altitude);
break;
default: break;
}
}
}
}
}
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
uint32_t currentTime;
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32f3xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f4xx.c file
*/
systemReady = false;
systemInit();
systemReady = true;
//evrPush(EVR_StartingMain, 0);
/* Setup SysTick Timer for 1 msec interrupts.
------------------------------------------
1. The SysTick_Config() function is a CMSIS function which configure:
- The SysTick Reload register with value passed as function parameter.
- Configure the SysTick IRQ priority to the lowest value (0x0F).
- Reset the SysTick Counter register.
- Configure the SysTick Counter clock source to be Core Clock Source (HCLK).
- Enable the SysTick Interrupt.
- Start the SysTick Counter.
2. You can change the SysTick Clock source to be HCLK_Div8 by calling the
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8) just after the
SysTick_Config() function call. The SysTick_CLKSourceConfig() is defined
inside the misc.c file.
3. You can change the SysTick IRQ priority by calling the
NVIC_SetPriority(SysTick_IRQn,...) just after the SysTick_Config() function
call. The NVIC_SetPriority() is defined inside the core_cm4.h file.
4. To adjust the SysTick time base, use the following formula:
Reload Value = SysTick Counter Clock (Hz) x Desired Time base (s)
- Reload Value is the parameter to be passed for SysTick_Config() function
- Reload Value should not exceed 0xFFFFFF
*/
#if 1
if (APRS_SLOT >= 0)
{
do
{
/*
while (!uartAvailable())
sleep();
*/
} while (!gpsDecode((char)uartRead()));
next_aprs = millis() + 1000 * (APRS_PERIOD - (gps_seconds + APRS_PERIOD - APRS_SLOT) % APRS_PERIOD);
}
else
{
next_aprs = millis();
}
#endif
while (1)
{
//evrCheck();
// Wait for sampling clock to overflow
if (TIM_GetFlagStatus(TIM2, TIM_FLAG_Update) != RESET)
{
;
}
#if 1
if ((int32_t)(millis() - next_aprs) >= 0)
{
getPos();
aprsSend();
next_aprs += APRS_PERIOD * 1000L;
while(afskBusy())
;
//sleep();
#if DEBUG_MODEM
afskDebug();
#endif
}
//sleep();
#else
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
#if 1
while (uartAvailable())
{
gpsDecode((char)uartRead());
}
#endif
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
cliCom();
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true) {
ledToggle(LED0);
#ifdef DEBUG_MODEM
afskDebug();
#endif
}
if (execUp == false)
execUpCount++;
if ((execUpCount == 5) && (execUp == false))
{
execUp = true;
ledOFF(LED0);
}
executionTime1Hz = micros() - currentTime;
}
#endif
}
}
uint8_t uartReadPoll(void)
{
while (!uartAvailable()); // wait for some bytes
return uartRead();
}
void serialCom(void)
{
uint8_t c;
static uint8_t offset;
static uint8_t dataSize;
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state = IDLE;
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
while (uartAvailable()) {
c = uartRead();
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE)
evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state == HEADER_START) {
c_state = (c == 'M') ? HEADER_M : IDLE;
} else if (c_state == HEADER_M) {
c_state = (c == '<') ? HEADER_ARROW : IDLE;
} else if (c_state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state = IDLE;
continue;
}
dataSize = c;
offset = 0;
checksum = 0;
indRX = 0;
checksum ^= c;
c_state = HEADER_SIZE; // the command is to follow
guiConnected = true;
} else if (c_state == HEADER_SIZE) {
cmdMSP = c;
checksum ^= c;
c_state = HEADER_CMD;
} else if (c_state == HEADER_CMD && offset < dataSize) {
checksum ^= c;
inBuf[offset++] = c;
} else if (c_state == HEADER_CMD && offset >= dataSize) {
if (checksum == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state = IDLE;
}
}
if (!cliMode && !uartAvailable() && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
return;
}
}
void serialCom(void)
{
uint8_t c;
bool HaveSomePacket = false;
static uint8_t NumberSignCNT = 0, RcharCNT = 0, RetCNT = 0;
// START Common Stuff for serial protocols
if (cfg.rssicut && rssi <= cfg.rssicut) rssi = 0;
// END Common Stuff for serial protocols
if (cliMode) // in cli mode, all uart stuff goes to here. enter cli mode by sending #####
{
cliProcess();
return;
}
if (f.ARMED) // Change Protocol according to armed state
{
switch(cfg.tele_prot) // 0=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
{
case 0:
Currentprotocol = PROTOCOL_MWII21;
break;
case 1: // Do Frsky here
sendFRSKYTelemetry();
break;
case 2:
case 3:
Currentprotocol = PROTOCOL_MAVLINK;
break;
}
}
else // Not armed engage autosensing here
{
if (!BlockProtocolChange && ((currentTimeMS - LastValidProtocolTimestampMS) > 4000))
Currentprotocol = PROTOCOL_AUTOSENSE; // Set Protocol to unknown after 4 Sek of garbage or no inputdata
}
switch(Currentprotocol)
{
case PROTOCOL_MAVLINK:
baseflight_mavlink_send_updates(); // Sends Heartbeat as well
break;
case PROTOCOL_AUTOSENSE:
baseflight_mavlink_send_1Hzheartbeat(); // It will time itself so no doubleheartbeat from sendupdates are here
default: // Mwii not specially done here because it just sends when asked to (see below)
break;
}
while (uartAvailable())
{
c = uartRead();
if (Currentprotocol == PROTOCOL_AUTOSENSE) // Check for extra Data when no protocol active (that is only in disarmed state possible)
{
if (c == '#')
{
NumberSignCNT++;
if (NumberSignCNT > 2) cliProcess(); // This is a one way - reset - street
}else NumberSignCNT = 0;
if (c == 'R')
{
RcharCNT++;
if (RcharCNT > 2) systemReset(true); // This is a one way - reset - street
}else RcharCNT = 0;
}
if (!f.ARMED && c == '\r') // Check for 3 Times Return in a row
{
RetCNT++;
if (RetCNT > 2) cliProcess();
}else RetCNT = 0;
switch(Currentprotocol)
{
case PROTOCOL_AUTOSENSE: // Current is Unsure
if(baseflight_mavlink_receive(c))Currentprotocol = PROTOCOL_MAVLINK;
else if(mwii_receive(c)) Currentprotocol = PROTOCOL_MWII21;
if(Currentprotocol != PROTOCOL_AUTOSENSE) HaveSomePacket = true; // Some protocol found
break;
case PROTOCOL_MWII21: // Current is Mwii
HaveSomePacket = mwii_receive(c);
break;
case PROTOCOL_MAVLINK: // Current is Mavlink
HaveSomePacket = baseflight_mavlink_receive(c);
break;
}
if(HaveSomePacket) LastValidProtocolTimestampMS = currentTimeMS;
}
}
void cliProcess(void)
{
if (!cliMode) {
cliMode = 1;
uartPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
}
while (uartAvailable()) {
uint8_t c = uartRead();
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
int i = bufferIndex;
for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0))
continue;
if (!pstart)
pstart = cmd;
pend = cmd;
}
if (pstart) { /* Buffer matches one or more commands */
for (; ; bufferIndex++) {
if (pstart->name[bufferIndex] != pend->name[bufferIndex])
break;
if (!pstart->name[bufferIndex]) {
/* Unambiguous -- append a space */
cliBuffer[bufferIndex++] = ' ';
break;
}
cliBuffer[bufferIndex] = pstart->name[bufferIndex];
}
}
if (!bufferIndex || pstart != pend) {
/* Print list of ambiguous matches */
uartPrint("\r\033[K");
for (cmd = pstart; cmd <= pend; cmd++) {
uartPrint(cmd->name);
uartWrite('\t');
}
cliPrompt();
i = 0; /* Redraw prompt */
}
for (; i < bufferIndex; i++)
uartWrite(cliBuffer[i]);
} else if (!bufferIndex && c == 4) {
cliExit(cliBuffer);
return;
} else if (c == 12) {
// clear screen
uartPrint("\033[2J\033[1;1H");
cliPrompt();
} else if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed
clicmd_t *cmd = NULL;
clicmd_t target;
uartPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name) + 1);
else
uartPrint("ERR: Unknown command, try 'help'");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
// 'exit' will reset this flag, so we don't need to print prompt again
if (!cliMode)
return;
cliPrompt();
} else if (c == 127) {
// backspace
if (bufferIndex) {
cliBuffer[--bufferIndex] = 0;
uartPrint("\010 \010");
}
} else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
if (!bufferIndex && c == 32)
continue;
cliBuffer[bufferIndex++] = c;
uartWrite(c);
}
}
}
void serialCom(bool singlestep) // Singlestep means you can restrict to decoding just one dataset per run
{ // Reason: Reduce priority when other tasks need more.
uint8_t c;
bool HaveSomePacket = false, Skipnow = false;
static uint8_t NumberSignCNT = 0, RcharCNT = 0, RetCNT = 0, SingleStepCnt;
if (singlestep) SingleStepCnt = min(SingleStepCnt+1, 4);
else SingleStepCnt = 0;
if (SingleStepCnt > 3) singlestep = false; // Limit singlestep to max 3. in a row
// START Common Stuff for serial protocols
if (cfg.rssicut && rssi <= cfg.rssicut) rssi = 0;
// END Common Stuff for serial protocols
if (f.ARMED) // Change Protocol according to armed state
{
switch(cfg.tele_prot) // 0=Keep Multiwii @CurrentUSB Baud, 1=Frsky @9600Baud, 2=Mavlink @CurrentUSB Baud, 3=Mavlink @57KBaud (like stock minimOSD wants it)
{
case 0:
Currentprotocol = PROTOCOL_MWII21;
break;
case 1: // Do Frsky here
sendFRSKYTelemetry();
break;
case 2:
case 3:
Currentprotocol = PROTOCOL_MAVLINK;
break;
}
}
else // Not armed engage autosensing here
{
if (AllowProtocolAutosense && ((currentTimeMS - LastValidProtocolTimestampMS) > 4000))
Currentprotocol = PROTOCOL_AUTOSENSE; // Set Protocol to unknown after 4 Sek of garbage or no inputdata
}
switch(Currentprotocol)
{
case PROTOCOL_MAVLINK:
baseflight_mavlink_send_updates(); // Sends Heartbeat as well
break;
case PROTOCOL_AUTOSENSE:
baseflight_mavlink_send_1Hzheartbeat(); // It will time itself so no doubleheartbeat from sendupdates are here
default: // Mwii not specially done here because it just sends when asked to (see below)
break;
}
while (uartAvailable() && !Skipnow)
{
c = uartRead();
if (Currentprotocol == PROTOCOL_AUTOSENSE) // Check for extra Data when no protocol active (that is only in disarmed state possible)
{
if (c == '#')
{
NumberSignCNT++;
if (NumberSignCNT > 2) cliProcess(); // This is a one way - reset - street
}else NumberSignCNT = 0;
if (c == 'R')
{
RcharCNT++;
if (RcharCNT > 2) systemReset(true); // This is a one way - reset - street
}else RcharCNT = 0;
}
if (!f.ARMED && c == '\r') // Check for 3 Times Return in a row
{
RetCNT++;
if (RetCNT > 2) cliProcess();
}else RetCNT = 0;
switch(Currentprotocol)
{
case PROTOCOL_AUTOSENSE: // Current is Unsure
if(baseflight_mavlink_receive(c))Currentprotocol = PROTOCOL_MAVLINK;
else if(mwii_receive(c)) Currentprotocol = PROTOCOL_MWII21;
if(Currentprotocol != PROTOCOL_AUTOSENSE) HaveSomePacket = true; // Some protocol found
break;
case PROTOCOL_MWII21: // Current is Mwii
HaveSomePacket = mwii_receive(c);
break;
case PROTOCOL_MAVLINK: // Current is Mavlink
HaveSomePacket = baseflight_mavlink_receive(c);
break;
}
if(HaveSomePacket)
{
LastValidProtocolTimestampMS = currentTimeMS;
if (singlestep) Skipnow = true;
}
}
}
int HardwareSerial::available(void)
{
return uartAvailable(_uart);
}
