/** Read a set_pid message
* @param[in] a packed set_pid message
* @param[out] ch channel (0=rudder,1=throttle)
* @param[out] Kp proportional gain
* @param[out] Ki integral gain
* @param[out] Kd derivative gain
* @param[out] len Number of bytes parsed; returns -3 if message is too short; -2 if message header wrong; -1 if message checksum wrong
*/
static PyObject * unpack_set_pid(PyObject *self, PyObject *args){
const char* byt;
int len = MSG_SET_PID_LEN;
if (!PyArg_ParseTuple(args,"s#",(&byt),&len)){
return NULL;
}
else{
// unpack message
uint8_t ch;
float Kp=0.0,Ki = 0.0,Kd = 0.0;
int8_t len2 = -3;
if ( len >= MSG_SET_PID_LEN){
len2 = esp_unpack_set_pid((uint8_t*)byt,&ch,&Kp,&Ki,&Kd);
}
return Py_BuildValue("Ifffi",ch,Kp,Ki,Kd,len2);
}
}
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 test_pid(int n){
int counter = 0;
for (int k = 0;k<n;k++){
uint8_t msg[256];
uint8_t ch1 = 0,ch = -1;
float kp1 = 0.8,
ki1 = .909812916276,
kd1 = -1.2324,
kp=0,
ki=0,
kd=0;
int8_t flag;
flag = esp_pack_set_pid(msg,ch1,kp1,ki1,kd1);
printf("Raw: ch=%d,kp=%g,ki=%g,kd=%g\n",ch1,kp1,ki1,kd1);
printf("Packed %d bytes into message\n",flag);
if (esp_unpack_set_pid(msg,&ch,&kp,&ki,&kd) > 0){
printf("Message: ch=%d,kp=%g,ki=%g,kd=%g\n",ch,kp,ki,kd);
counter++;
}
}
return counter;
}
