void W_GPS_Y_FUNC(void)
{
Pointfp64 position;
position.y=MOSI_GPS_Y.f64_data;
position.x=Gps_List[0].position.x;
GPS_Init(position,Gps_List[0].radian);
}
void GPS_Clear(void)
{
Gyro_Total=0;
// Gyro_Last=Gyro_Now;
Encoder_Clear(0);
Encoder_Clear(1);
GPS_Init(Gps_Zero,0);
}
/*********************************************************************
** @fn :
**
** @brief :
**
** @param :
**
** @return :
*********************************************************************/
static void Module_Init(void)
{
EVL_Board_Init();
UART_Init();
Task_Init();
BuffPool_Init();
GPRS_Init();
GPS_Init();
NetFrame_Init();
Systick_Init();
}
/* Wrapped interface from higher level objects */
int Garmin_GPS_Init( wxString &port_name)
{
int ret;
#ifdef GPS_DEBUG0
// if (getenv("OPENCPN_GPS_ERROR") != NULL)
GPS_Enable_Error();
// if (getenv("OPENCPN_GPS_WARNING") != NULL)
GPS_Enable_Warning();
// if (getenv("OPENCPN_GPS_USER") != NULL)
GPS_Enable_User();
// if (getenv("OPENCPN_GPS_DIAGNOSE") != NULL)
GPS_Enable_Diagnose();
#endif
char m[1];
m[0] = '\0';
GPS_Error(m);
ret = GPS_Init(port_name.mb_str());
VerifyPortClosed();
return ret;
}
/**
* @brief 板级初始化
*
* @param none
* @retval none
*/
void BSP_Init (void)
{
LED_Init();
Flash_Read(); //读取参数
Flash_CheckFirstTime(false); //检查FLASH里是否存在有效数据，如果没有则写入默认数据
// cfg.mixerConfiguration = MIXERCONFIG;统一在调参软件里设置机型
Mixer_Init(); //动力初始化
Serial_Init(cfg.serial_baudrate);//上位机调参初始化
I2C2_Init(); //I2C2
Sensor_DetectAndInit();
IMU_Init();
Altitude_Init();
Actuator_Init();
RC_Init();
Daemon_Init();
GPS_Init(cfg.gps_baudrate);
Telemetry_Init();
#ifdef OPTICALFLOW
OpticalFlow_Init();
#endif
EasyCtrl_Init();
VisionLanding_Init();
Avoid_Init();
}
//*****************************************************************************
//
//! \brief something should do before the test execute of GPS test.
//!
//! \return None.
//
//*****************************************************************************
static void GPSSetup(void)
{
GPS_Init(9600);
}
int main(void)
{
// initial image transmit
Open_ImageFIFO(ImageFIFO);
// uart initial
fprintf(stderr, "This is a test for the Contex-A8...\n");
serial_initial("/dev/ttySAC1", &GsmDevice, 9600);
AtTransmitInit(&GsmDevice);
usleep(10000);
// buzzer init
// buzzer_init();
// buzzer_beep(100000);
// GPS uart init
GPS_Init();
// load the connection param
DevParam_Init();
SEQ_Init();
#ifdef _EN_INITIAL_GSM_
// config the gprs network
NetWork_Connection_Config(Param_IP(), Param_Port(), Param_APN());
#endif
// get the CSQ
if(GSM_GetCSQ(&CSQ)!=ERR_NONE){
fprintf(stderr, "Can't get the CSQ...\n");
} else{
fprintf(stderr, "CSQ = %d\n",CSQ);
}
sleep(5);
// create the thread
pthread_create(&Thread_ID[0], NULL, (void *)AsyncUpdate, NULL);
pthread_create(&Thread_ID[1], NULL, (void *)HeartBeat, NULL);
PositionMonitor_init();
pthread_create(&Thread_ID[2], NULL, (void *)PositionMonitor, NULL);
SpeedMonitor_init();
pthread_create(&Thread_ID[3], NULL, (void *)SpeedMonitor, NULL);
InZoneMonitor_init();
pthread_create(&Thread_ID[4], NULL, (void *)InZoneMonitor, NULL);
OutZoneMonitor_init();
pthread_create(&Thread_ID[5], NULL, (void *)OutZoneMonitor, NULL);
CrossTrackMonitor_init();
pthread_create(&Thread_ID[6], NULL, (void *)CrossTrackMonitor, NULL);
// // initial image transmit
// Open_ImageFIFO(ImageFIFO);
TypeD_Transmit_Init();
ImageTransmit_Init();
pthread_create(&Thread_ID[7], NULL, (void *)ImageTransmit_Thread, NULL);
// Emergency report
EmergencyMonitor_init();
pthread_create(&Thread_ID[8], NULL, (void *)EmergencyMonitor, NULL);
// Fatigue report
FatigueMonitor_init();
pthread_create(&Thread_ID[9], NULL, (void *)FatigueMonitor, NULL);
// GPS parse thread.
pthread_create(&GPS_Threadid, NULL, (void *)GPS_Parse_thread, NULL);
// try to login server
while( Try_Login() != 0 );
// buzzer_beep(100000);
while(1){
GSM_mutex_lock();
InputPacket_Process();
GPS_Infomation();
GSM_mutex_unlock();
if( isNeed_Updata_Position() ){
Position_update_after_Call();
}
if(is_lose_connection() == 1){
exit(-1);
}
sleep(1);
}
serial_close(&GsmDevice);
return 0;
}
int Garmin_GPS_Open( wxString &port_name )
{
return GPS_Init(port_name.mb_str());
}
int main(void)
{
int i;
// uart initial
fprintf(stderr, "This is a test for the Contex-A8...\n");
serial_initial("/dev/ttySAC1", &GsmDevice, 9600);
AtTransmitInit(&GsmDevice);
usleep(10000);
SEQ_Init();
// GPS uart init
GPS_Init();
// FIFO initial
Open_ImageFIFO(ImageFIFO);
#ifdef _EN_INITIAL_GSM_
// config the gprs network
NetWork_Connection_Config();
#endif
// printf the CSQ info
GSM_GetPacketInfo(&unread_sum, &packet_sum);
fprintf(stderr, "Unread packet sum = %d\n",unread_sum);
fprintf(stderr, "total packet sum = %d\n",packet_sum);
if(GSM_GetCSQ(&CSQ)!=ERR_NONE){
fprintf(stderr, "Can't get the CSQ...\n");
} else{
fprintf(stderr, "CSQ = %d\n",CSQ);
}
for(i=0;i<20;i++){
usleep(10000);
}
Login_Process(gRxBuff);
fprintf(stderr, "========= start transmit =========\n");
for(i=0;i<100;i++){
usleep(10000);
}
// =====================================
GPS_Debug.lat = MSEC2NDeg( (double)81678360 );
GPS_Debug.lon = MSEC2NDeg( (double)409628128 );
GPS_Debug.speed = 60;
GPS_InfoPrintf(&GPS_Debug);
// =====================================
// image updata process
//ImageTransmit_init("/home/plg/linux.jpg");
// packet process
fprintf(stderr,"\r\n****************************\r\n");
PositionUpdateRule_Initial(&RuleList);
InZoneCondition_Initial(&InZone_List);
OutZoneCondition_Initial(&OutZone_List);
TDSA_Condition_Initial(&TDSA_List);
Interval_Locate_Init(); // initial the position update process
ZoneInfo_Report_Init(); // initial the Zone information update process
Speed_Report_Init(); // initial the speed information update process
// task initial
Task_Init();
SetTimeOut(_Task_Heartbeat_, 6); // execute every 60 seconds
SetTimeOut(_Task_GPS_Display_, 6); // execute every 60 seconds
SetTimeOut(_Task_RE_Login_, 15); // if we did not receive the heart beat ack in 150 seconds, re login...
while(1){
GSM_GetPacketInfo(&unread_sum, &packet_sum);
G_PacketType = TypeX;
if(unread_sum>0){
memset(gRxBuff,'\0',1024);
UDP_ReceivePacket(&link_num, &data_index, &data_len, gRxBuff);
fprintf(stderr, "Receive Packet ...\n");
fprintf(stderr, "link num = %d\n",link_num);
fprintf(stderr, "data index = %d\n",data_index);
fprintf(stderr, "data length = %d\n",data_len);
Packet.length = data_len;
memcpy(&(Packet.Data[0]), gRxBuff, data_len);
res = TLP_PacketDevide(&Packet, &APP_Packet, &G_PacketType);
// A type input
if((res == ERROR_NONE)&&(G_PacketType == TypeA)){
InputCommandHandle(&APP_Packet);
ShowRules(&RuleList);
ShowCondition(&InZone_List);
ShowCondition(&OutZone_List);
ShowSpeedCondition(&TDSA_List);
}
// B type input
if(G_PacketType == TypeB){
ReportUpdata_Loop(0);
}
// D type input
if(G_PacketType == TypeD){
ImageTransmit_loop(0);
}
}
// check the fifo, if we got a new image, send it...
DrowsyImage_Check();
DrowsyImage_Send();
// if the image transmit is in working, the other update mesage is delay
if(Get_DT_State()==DT_Idle){
Interval_Locate_Check(&RuleList); // position update process
Interval_Locate_Updata(&GPS_Msg);
ZoneInfo_Condition_Check(&InZone_List, &OutZone_List, &GPS_Msg); // Zone information process
ZoneInfo_Update(&GPS_Msg);
Speed_Condition_Check(&TDSA_List,&GPS_Debug);
SpeedReport_Update(&GPS_Debug);
if( isTimeOut(_Task_Heartbeat_)==1 ){
HeartBeat_Request(NULL);
ClearTimeOut(_Task_Heartbeat_);
}
if( isTimeOut(_Task_RE_Login_)==1 ){
// reconfig the network
NetWork_Connection_Config();
Login_Process(gRxBuff);
ClearTimeOut(_Task_RE_Login_);
}
}
// GPS information
GPS_Read(&GPS_Msg);
if( isTimeOut(_Task_GPS_Display_)==1 ){
GPS_InfoPrintf(&GPS_Msg);
ClearTimeOut(_Task_GPS_Display_);
}
usleep(500000);
}
GSM_CloseConnection();
for(i=0;i<100;i++){
usleep(20000);
}
GSM_Reset();
serial_close(&GsmDevice);
return 0;
}
/**
* Tracking Module (Thread)
*/
THD_FUNCTION(moduleTRACKING, arg) {
// Print infos
module_params_t* parm = (module_params_t*)arg;
TRACE_INFO("TRAC > Startup module TRACKING MANAGER");
TRACE_INFO("TRAC > Module TRACKING MANAGER info\r\n"
"%s Cycle: %d sec",
TRACE_TAB, parm->cycle
);
uint32_t id = 1;
lastTrackPoint = &trackPoints[0];
systime_t time = chVTGetSystemTimeX();
while(true)
{
parm->lastCycle = chVTGetSystemTimeX(); // Watchdog timer
TRACE_INFO("TRAC > Do module TRACKING MANAGER cycle");
trackPoint_t* tp = &trackPoints[id % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Current track point
trackPoint_t* ltp = &trackPoints[(id-1) % (sizeof(trackPoints) / sizeof(trackPoint_t))]; // Last track point
// Search for GPS satellites
gpsFix_t gpsFix;
GPS_Init();
do {
gps_get_fix(&gpsFix);
} while(!isGPSLocked(&gpsFix) && chVTGetSystemTimeX() <= time + S2ST(parm->cycle-3)); // Do as long no GPS lock and within timeout, timeout=cycle-1sec (-1sec in order to keep synchronization)
if(isGPSLocked(&gpsFix)) { // GPS locked
// Switch off GPS
GPS_Deinit();
// Debug
TRACE_INFO("TRAC > GPS sampling finished GPS LOCK");
TRACE_GPSFIX(&gpsFix);
// Calibrate RTC
setTime(gpsFix.time);
// Take time from GPS
tp->time.year = gpsFix.time.year;
tp->time.month = gpsFix.time.month;
tp->time.day = gpsFix.time.day;
tp->time.hour = gpsFix.time.hour;
tp->time.minute = gpsFix.time.minute;
tp->time.second = gpsFix.time.second;
// Set new GPS fix
tp->gps_lat = gpsFix.lat;
tp->gps_lon = gpsFix.lon;
tp->gps_alt = gpsFix.alt;
tp->gps_lock = isGPSLocked(&gpsFix);
tp->gps_sats = gpsFix.num_svs;
} else { // GPS lost (keep GPS switched on)
// Debug
TRACE_WARN("TRAC > GPS sampling finished GPS LOSS");
// Take time from internal RTC
ptime_t time;
getTime(&time);
tp->time.year = time.year;
tp->time.month = time.month;
tp->time.day = time.day;
tp->time.hour = time.hour;
tp->time.minute = time.minute;
tp->time.second = time.second;
// Take GPS fix from old lock
tp->gps_lat = ltp->gps_lat;
tp->gps_lon = ltp->gps_lon;
tp->gps_alt = ltp->gps_alt;
// Mark gpsloss
tp->gps_lock = false;
tp->gps_sats = 0;
}
tp->id = id; // Serial ID
tp->gps_ttff = ST2S(chVTGetSystemTimeX() - time); // Time to first fix
// Power management
tp->adc_solar = getSolarVoltageMV();
tp->adc_battery = getBatteryVoltageMV();
tp->adc_charge = pac1720_getAverageChargePower();
tp->adc_discharge = pac1720_getAverageDischargePower();
bme280_t bmeInt;
bme280_t bmeExt;
// Atmosphere condition
if(BME280_isAvailable(BME280_ADDRESS_INT)) {
BME280_Init(&bmeInt, BME280_ADDRESS_INT);
tp->air_press = BME280_getPressure(&bmeInt, 256);
tp->air_hum = BME280_getHumidity(&bmeInt);
tp->air_temp = BME280_getTemperature(&bmeInt);
} else { // No internal BME280 found
TRACE_ERROR("TRAC > Internal BME280 not available");
tp->air_press = 0;
tp->air_hum = 0;
tp->air_temp = 0;
}
// Balloon condition
if(BME280_isAvailable(BME280_ADDRESS_EXT)) {
BME280_Init(&bmeExt, BME280_ADDRESS_EXT);
tp->bal_press = BME280_getPressure(&bmeExt, 256);
tp->bal_hum = BME280_getHumidity(&bmeExt);
tp->bal_temp = BME280_getTemperature(&bmeExt);
} else { // No external BME280 found
TRACE_WARN("TRAC > External BME280 not available");
tp->bal_press = 0;
tp->bal_hum = 0;
tp->bal_temp = 0;
}
// Trace data
TRACE_INFO( "TRAC > New tracking point available (ID=%d)\r\n"
"%s Time %04d-%02d-%02d %02d:%02d:%02d\r\n"
"%s Pos %d.%07d %d.%07d Alt %dm\r\n"
"%s Sats %d TTFF %dsec\r\n"
"%s ADC Vbat=%d.%03dV Vsol=%d.%03dV Pin=%dmW Pout=%dmW\r\n"
"%s Air p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%\r\n"
"%s Ball p=%6d.%01dPa T=%2d.%02ddegC phi=%2d.%01d%%",
tp->id,
TRACE_TAB, tp->time.year, tp->time.month, tp->time.day, tp->time.hour, tp->time.minute, tp->time.day,
TRACE_TAB, tp->gps_lat/10000000, tp->gps_lat%10000000, tp->gps_lon/10000000, tp->gps_lon%10000000, tp->gps_alt,
TRACE_TAB, tp->gps_sats, tp->gps_ttff,
TRACE_TAB, tp->adc_battery/1000, (tp->adc_battery%1000), tp->adc_solar/1000, (tp->adc_solar%1000), tp->adc_charge, tp->adc_discharge,
TRACE_TAB, tp->air_press/10, tp->air_press%10, tp->air_temp/100, tp->air_temp%100, tp->air_hum/10, tp->air_hum%10,
TRACE_TAB, tp->bal_press/10, tp->bal_press%10, tp->bal_temp/100, tp->bal_temp%100, tp->bal_hum/10, tp->bal_hum%10
);
// Switch last recent track point
lastTrackPoint = tp;
id++;
time = chThdSleepUntilWindowed(time, time + S2ST(parm->cycle)); // Wait until time + cycletime
}
}
void W_GPS_ANG_FUNC(void)
{
GPS_Init(Gps_List[0].position,MOSI_GPS_ANG.f64_data);
}
void TaskAuto (void *pdata)
{
//static struct Pointfp64 position;
//static fp64 angle;
uint8 err;
uint8 *msg;
pdata=pdata;
for(;;)
{
msg = OSMboxPend(AutoMbox,0,&err);
switch (msg[0])
{
case 1:
LCD_Clear();
GPS_Clear();
RouteForm=RouteStop;
PID_Clear();
GoRoute1();
case 2:
PID_Clear();
GoRoute2();
break;
case 3:
PID_Clear();
GoRoute3();
case 4:
PID_Clear();
GoRoute4();
case 5:
PID_Clear();
GoRoute5();
case 6:
rGPJDAT&=~0x1000; //¹Ø·äÃùÆ÷
LCD_Clear();
PID_Clear();
//GoRoute6();
Route_Num=6;
SetKeep(PointRoute[Route[6]].position,0.0);
while(RouteFinish)
{
if(((Fp64Abs(Aim_Angle-Gps_List[0].angle)<1)&&
(Getlength(AimPoint,Gps_List[0].position)<10))||(Route_Num==7))
{
RouteFinish=0;
RouteForm=RouteStop;
LCD_SetXY(0,2);
LCD_WriteString("route6 done");
}
OSTimeDly(1);
}
break;
case 7:
GPS_Init(PointRoute[Route[6]].position,0.0);
LCD_Clear();
PID_Clear();
GoRoute7();
break;
case 8:
GPS_Init(PointRoute[Route[7]+1].position,Gps_List[0].angle);
LCD_Clear();
PID_Clear();
GoRoute8();
Shootest();
break;
default:;
break;
}
OSTimeDly(1);
}
}