/** Read a GPS message
* @param[in] a packed GPS message
*/
static PyObject * unpack_gps(PyObject *self, PyObject *args){
const char* byt;
int len = MSG_GPS_LEN;
if (!PyArg_ParseTuple(args,"s#",(&byt),&len)){
return NULL;
}
else{
// unpack message
int32_t lat=0,lon=0;
float t = 0.0;
if ( len < MSG_GPS_LEN){
// return fail condition
return Py_BuildValue("llfi",lon,lat,t,-3);
}
int8_t len2 = esp_unpack_gps((uint8_t*)byt,&lon,&lat,&t);
return Py_BuildValue("llfi",lon,lat,t,len2);
}
}
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;
}
void commParser::handleMsg(){
//increment the counter
received_messages++;
//switch based on the message ID
switch (msg[2]){
case MSG_GPS:
float t;
int32_t lon,lat;
if (esp_unpack_gps(msg,&lon, &lat, &t) > 0){
//set status
status->gpsCmd.set(lat,lon,t);
}
else//increment the bad packet counter
bad_packets++;
break;
case MSG_CONTROL:/** Direct control of rudder/throttle */
float rudd,thro;
if (esp_unpack_control(msg,&rudd, &thro) > 0){
//set status rudder, throttle, and mode
status->control_rudder = rudd;
status->control_throttle = thro;
status->control_mode = CONTROL_MODE_DIRECT;
}
else//increment the bad packet counter
bad_packets++;
break;
case MSG_COMMAND:
break;
case MSG_SET_PID:
//uint8_t*msg,uint8_t* ch,float* KP, float* KI, float* KD
uint8_t ch;
float Kp, Kd, Ki;
if (esp_unpack_set_pid(msg,&ch,&Kp,&Ki,&Kd) > 0){
// set the gains in the status variable
status->Kp[ch] = Kp;
status->Ki[ch] = Ki;
status->Kd[ch] = Kd;
}
break;
}
}
int main(){
int monte = 1;
printf("%d/%d valid messages in test_gps\n",test_gps(monte),monte);
printf("%d/%d valid messages in test_control\n",test_control(monte),monte);
printf("%d/%d valid messages in test_command\n",test_command(monte),monte);
printf("%d/%d valid messages in test_pid\n",test_pid(monte),monte);
// weirdness
uint8_t msg[] = {0x7F,0x53,0x01,0xC0,0xAE,0x93,0xC6,0xC0,0x3D,0x40,0x12,0xEC,0x51,0x20,0x41,0x87};
int32_t lon=0,lat=0;
float t=0;
if (esp_unpack_gps(msg,&lon,&lat,&t) > 0){
printf("Hardcoded msg: lon = %ld, lat = %ld, t=%f\n",lon,lat,t);
}
printf("%ld\n",0xC0);
printf("%ld\n",0xAE<<8);
printf("%ld\n",0x93<<16);
printf("%ld\n",0xC6<<24);
printf("%ld",(0xC0) + (0xAE << 8) + (0x93<<16) + (0xC6 << 24));
return 0;
}
