/** Pack data into a control message */
static PyObject * pack_control(PyObject *self, PyObject *args){
float rudd, thro;
if (!PyArg_ParseTuple(args,"ff",&rudd,&thro)){
return NULL;
}
else{
// pack message and return the string...
char byt[MSG_CONTROL_LEN];
int8_t len = esp_pack_control((uint8_t*)byt,rudd,thro);
return Py_BuildValue("s#i",byt,len,len);
}
};
int test_control(int n){
int counter = 0;
for (int k = 0;k<n;k++){
uint8_t msg[256];
float t1 = gen_random(0.0,1.0),
h1 = gen_random(-1.0,1.0),
t=0,
h=0;
int8_t flag;
flag = esp_pack_control(msg,h1,t1);
printf("Raw: rudder = %g, speed = %g\n",h1,t1);
printf("Packed %d bytes into message\n",flag);
if (esp_unpack_control(msg,&h,&t) > 0){
printf("Message: rudder = %g, speed = %g\n",h,t);
counter++;
}
}
return counter;
}
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
}
}
}
}
//}
}
