/** Pack data into a GPS message */
static PyObject * pack_gps(PyObject *self, PyObject *args){
int32_t lat=0,lon=0;
float t = 0.0;
if (!PyArg_ParseTuple(args,"llf",&lon,&lat,&t)){
return NULL;
}
else{
// pack message and return the string...
char byt[MSG_GPS_LEN];
int8_t len = esp_pack_gps((uint8_t*)byt,lon,lat,t);
return Py_BuildValue("s#i",byt,len,len);
}
};
void commParser::misc_tasks(uint32_t millis){
// check the time since last message and set the status appropriately
if (millis-last_message_millis < TIMEOUT_WARNING_MILLIS)
status->comm_status = COMM_STATUS_HEALTHY;
// if we're in a non-healthy comm status mode, set the control mode to passive and wait to see if things improve
if (millis-last_message_millis > TIMEOUT_WARNING_MILLIS & millis-last_message_millis < TIMEOUT_LOST_MILLIS){
status->comm_status = COMM_STATUS_WARNING;
status->control_mode = CONTROL_MODE_PASSIVE;
}
if (millis-last_message_millis > TIMEOUT_LOST_MILLIS){
status->comm_status = COMM_STATUS_LOST;
status->control_mode = CONTROL_MODE_PASSIVE;
}
// reset the send buffer position
send_buffer_counter = 0;
send_buffer_counter_helper = 0;
//if( status->comm_status == COMM_STATUS_HEALTHY){
// send periodic messages
for(int k = 0;k<2;k++){
if (next_stream_time_millis[k] <= millis){
if (k == 0){ // GPS stream, send current GPS
next_stream_time_millis[k] = millis + STREAM_PERIOD_GPS;
if (status->gpsNow.init){
// send GPS if initialized
if (esp_pack_gps(&send_buffer[send_buffer_counter],status->gpsNow.lon,status->gpsNow.lat,status->gpsNow.t) > 0)
send_buffer_counter+=MSG_GPS_LEN;
}
}
if (k == 1){ // control-related message stream
next_stream_time_millis[k] = millis + STREAM_PERIOD_CONTROL;
if (status->control_mode == CONTROL_MODE_DIRECT){// in DIRECT mode, echo the rudder and throttle commands
if (esp_pack_control(&send_buffer[send_buffer_counter],status->control_rudder,status->control_throttle) > 0)
send_buffer_counter+=MSG_CONTROL_LEN;
}
else if(status->control_mode == CONTROL_MODE_INDIRECT){// in INDIRECT mode, send the current rudder and throttle settings
continue;//TODO stream messages after we add this functionality
}
}
}
}
//}
}
int test_gps(int n){
int counter = 0;
for (int k = 0;k<n;k++){
uint8_t msg[256];
int32_t lon1 = int32_t(1e7*gen_random(-180.0,180.0)),
lat1 = int32_t(1e7*gen_random(-180.0,180.0)),
lon=0,lat=0;
float t1 = gen_random(0.0,1.0e6),
t=0;
int8_t flag;
flag = esp_pack_gps(msg,lon1,lat1,t1);
printf("Raw: lon = %ul, lat = %ul, t=%f\n",lon1,lat1,t1);
printf("Packed %d bytes into message\n",flag);
if (esp_unpack_gps(msg,&lon,&lat,&t) > 0){
printf("Message: lon = %dl, lat = %dl, t=%f\n",lon,lat,t);
counter++;
}
}
return counter;
}
