首页 > 代码库 > PPZ之串口获取IMU

PPZ之串口获取IMU



1,rotocraft/main函数
 main_init()
  (1)各设备初始化
  (2)register timers for the periodic functions
   其中有telemetry_tid
 while(1){
  handle_periodic_tasks()
   其中,该函数的实现里有:
    if (sys_time_check_and_ack_timer(telemetry_tid))
     telemetry_periodic();
   即检测到telemetry_tid计时器到期之后,执行telemetry_periodic()
    该telemetry_periodic()函数的实现里有
     periodic_telemetry_send_Main();
      ?????//查看#include "generated/periodic_telemetry.h"
  main_event()
   重点看了包含的下面两个函数
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
   DatalinkEvent();
 }
2,DatalinkEvent()
 check&parse,发送前的准备
 (1)在subsystems/datalink/datalink.h文件中
 
  EXTERN void dl_parse_msg(void);
  EXTERN bool_t dl_msg_available;
  /** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/

  EXTERN uint16_t datalink_time;

  #define MSG_SIZE 128
  EXTERN uint8_t dl_buffer[MSG_SIZE]  __attribute__ ((aligned));

  EXTERN void dl_parse_msg(void);
  /** Should be called when chars are available in dl_buffer */

  /** Check for new message and parse */
  #define DlCheckAndParse() {   \
    if (dl_msg_available) {      \
   dl_parse_msg();            \
   dl_msg_available = FALSE;  \
    }                            \
  }
  
  #if defined DATALINK && DATALINK == PPRZ

   #define DatalinkEvent() {                       \
    PprzCheckAndParse(PPRZ_UART, pprz_tp);      \
    DlCheckAndParse();                          \
     }

  #elif defined DATALINK && DATALINK == XBEE

   #define DatalinkEvent() {                       \
    XBeeCheckAndParse(XBEE_UART, xbee_tp);      \
    DlCheckAndParse();                          \
     }
  宏定义DatalinkEvent()调用PprzCheckAndParse()与DlCheckAndParse()
  其中,DlCheckAndParse()Check后调用dl_parse_msg()
 (2)在/datalink/pprz_transport.h文件中
  PprzCheckAndParse() 调用ReadPprzBuffer()读取缓存并调用parse_pprz()解析数据、检查校验码,
  最后调用pprz_parse_payload()将pprz_transport中的payload数据复制到dl_buffer中。
  #define PprzBuffer(_dev) TransportLink(_dev,ChAvailable())
  #define ReadPprzBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received))
            parse_pprz(&(_trans),TransportLink(_dev,Getch())); }
  #define PprzCheckAndParse(_dev,_trans) {  \
    if (PprzBuffer(_dev)) {                 \
   ReadPprzBuffer(_dev,_trans);          \
   if (_trans.trans.msg_received) {      \
     pprz_parse_payload(&(_trans));      \
     _trans.trans.msg_received = FALSE;  \
   }                                     \
    }                                       \
  }
  static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) {
    switch (t->status) {
    case UNINIT:
   if (c == STX)
     t->status++;
   break;
    case GOT_STX:
   if (t->trans.msg_received) {
     t->trans.ovrn++;
     goto error;
   }
   t->trans.payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
   t->ck_a = t->ck_b = c;
   t->status++;
   t->payload_idx = 0;
   break;
    case GOT_LENGTH:
   t->trans.payload[t->payload_idx] = c;
   t->ck_a += c; t->ck_b += t->ck_a;
   t->payload_idx++;
   if (t->payload_idx == t->trans.payload_len)
     t->status++;
   break;
    case GOT_PAYLOAD:
   if (c != t->ck_a)
     goto error;
   t->status++;
   break;
    case GOT_CRC1:
   if (c != t->ck_b)
     goto error;
   t->trans.msg_received = TRUE;
   goto restart;
    default:
   goto error;
    }
    return;
   error:
    t->trans.error++;
   restart:
    t->status = UNINIT;
    return;
  }

  static inline void pprz_parse_payload(struct pprz_transport * t) {
    uint8_t i;
    for(i = 0; i < t->trans.payload_len; i++)
   dl_buffer[i] = t->trans.payload[i];
    dl_msg_available = TRUE;
  }
 (3)在/firmwares/rotorcraft/datalink.c中
  dl_parse_msg()获取msgID,根据该ID调用相应DOWNLINK_SEND_PONG()
  void dl_parse_msg(void) {

    datalink_time = 0;

    uint8_t msg_id = IdOfMsg(dl_buffer);
    switch (msg_id) {

    case  DL_PING:
   {
     DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
   }
   break;
   ..............
   
   }
  }
 
3,ImuEvent()
 (1)第一步imu_aspirin2_event()最终调用mpu60x0_spi_periodic()读取Imu数据,然后调用各handler执行如ahrs等后处理
 static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
   imu_aspirin2_event();
   if (imu_aspirin2.gyro_valid) {
  imu_aspirin2.gyro_valid = FALSE;
  _gyro_handler();
   }
   if (imu_aspirin2.accel_valid) {
  imu_aspirin2.accel_valid = FALSE;
  _accel_handler();
   }
   if (imu_aspirin2.mag_valid) {
  imu_aspirin2.mag_valid = FALSE;
  _mag_handler();
   }
 }
 在/subsystems/imu/imu_aspirin_2_spi.c文件中
 void imu_periodic(void)
 {
   mpu60x0_spi_periodic(&imu_aspirin2.mpu);
 }
 在/peripherals/mpu60x0_spi.h文件里
 /// convenience function: read or start configuration if not already initialized
 static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) {
   if (mpu->config.initialized)
  mpu60x0_spi_read(mpu);
   else
  mpu60x0_spi_start_configure(mpu);
 }
 (2)//下面为获得IMU数据后执行的处理
 static inline void on_accel_event( void ) {
   ImuScaleAccel(imu);

   if (ahrs.status != AHRS_UNINIT) {
  ahrs_update_accel();
   }
 }

 static inline void on_gyro_event( void ) {

   ImuScaleGyro(imu);

   if (ahrs.status == AHRS_UNINIT) {
  ahrs_aligner_run();
  if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
    ahrs_align();
   }
   else {
  ahrs_propagate();
 #ifdef SITL
  if (nps_bypass_ahrs) sim_overwrite_ahrs();
 #endif
  ins_propagate();
   }
 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_imu_available();
 #endif
 }
 static inline void on_mag_event(void) {
   ImuScaleMag(imu);

 #if USE_MAGNETOMETER
   if (ahrs.status == AHRS_RUNNING) {
  ahrs_update_mag();
   }
 #endif

 #ifdef USE_VEHICLE_INTERFACE
   vi_notify_mag_available();
 #endif
 }
4,各种Message的分类及区别,确定地面站接收到的Message的类型
?????

PPZ之串口获取IMU