首页 > 代码库 > 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