Ejemplo n.º 1
0
/** 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);
  }
}
Ejemplo n.º 2
0
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;
  }
}
Ejemplo n.º 3
0
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;
}