ArduPilot开源飞控之AP_ExternalAHRS_VectorNav
- 1. 源由
- 2. 框架代码
- 2.1 启动代码
- 2.2 任务代码
- 3. 重要例程
- 3.1 AP_ExternalAHRS_VectorNav
- 3.2 update_thread
- 3.3 update
- 4. 串行协议
- 4.1 check_uart
- 4.2 process_packet1
- 4.3 process_packet2
- 4.4 process_packet_VN_100
- 5. 参考资料
1. 源由
ArduPilot开源飞控之AP_AHRS这里既然已经提及外部AHRS传感器，就接着这个话题，把AP_ExternalAHRS_VectorNav驱动研读一下。
2. 框架代码
2.1 启动代码
Copter::init_ardupilot
└──> Copter::startup_INS_ground
└──> AP_AHRS::init
└──> AP_ExternalAHRS::init
└──> new AP_ExternalAHRS_VectorNav
└──> AP_ExternalAHRS_VectorNav::update_thread //thread_create
2.2 任务代码
FAST_TASK(read_AHRS)
└──> Copter::read_AHRS
└──> AP_AHRS::update
└──> AP_ExternalAHRS::update
└──> AP_ExternalAHRS_VectorNav::update
3. 重要例程
3.1 AP_ExternalAHRS_VectorNav
应用的变量初始化
AP_ExternalAHRS_VectorNav
│
│ /********************************************************************************
│ * Find Serial Port *
│ ********************************************************************************/
├──> auto &sm = AP::serialmanager()
├──> uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0)
├──> <!uart>
│ ├──> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS no UART")
│ └──> return
│
│ /********************************************************************************
│ * Application Configurations Initialization *
│ ********************************************************************************/
├──> baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0)
├──> port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0)
├──> bufsize = MAX(MAX(VN_PKT1_LENGTH, VN_PKT2_LENGTH), VN_100_PKT1_LENGTH)
├──> pktbuf = new uint8_t[bufsize]
├──> last_pkt1 = new VN_packet1
├──> last_pkt2 = new VN_packet2
├──> <!pktbuf || !last_pkt1 || !last_pkt2>
│ └──> AP_BoardConfig::allocation_error("ExternalAHRS")
├──> <!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_VectorNav::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)>
│ └──> AP_HAL::panic("Failed to start ExternalAHRS update thread")
└──> GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised")
3.2 update_thread
启动驱动总线数据更新线程。
AP_ExternalAHRS_VectorNav::update_thread
│
│ /********************************************************************************
│ * Serial Port Initialization *
│ ********************************************************************************/
│ // Open port in the thread
├──> uart->begin(baudrate, 1024, 512)
│
│ // Stop NMEA Async Outputs (this UART only)
├──> nmea_printf(uart, "$VNWRG,6,0")
│
│ // Detect version, Read Model Number Register, ID 1
├──> wait_register_responce(1)
│
│ /********************************************************************************
│ * AHRS Model Classification *
│ ********************************************************************************/
│ // Setup for messages respective model types (on both UARTs)
├──> <strncmp(model_name, "VN-100", 6) == 0>
│ │ // VN-100
│ ├──> type = TYPE::VN_100
│ │
│ │ // This assumes unit is still configured at its default rate of 800hz
│ └──> nmea_printf(uart, "$VNWRG,75,3,%u,14,073E,0004", unsigned(800/get_rate()))
├──> < else >
│ │ // Default to Setup for VN-300 series
│ │ // This assumes unit is still configured at its default rate of 400hz
│ ├──> nmea_printf(uart, "$VNWRG,75,3,%u,34,072E,0106,0612", unsigned(400/get_rate()))
│ └──> nmea_printf(uart, "$VNWRG,76,3,80,4E,0002,0010,20B8,0018")
├──> setup_complete = true
│
│ /********************************************************************************
│ * UART Receive Procedure *
│ ********************************************************************************/
└──> <while (true)>
└──> <!check_uart()>
└──> hal.scheduler->delay(1)
3.3 update
这里就比较奇怪，也许是为了保险，在进行AHRS之前在此进行以下数据同步。
AP_ExternalAHRS_VectorNav::update
└──> check_uart
4. 串行协议
4.1 check_uart
目前支持以下三种报文头格式：
static const uint8_t vn_pkt1_header[] { 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 }
static const uint8_t vn_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x00 }
static const uint8_t vn_100_pkt1_header[] { 0x14, 0x3E, 0x07, 0x04, 0x00 }
/*
check the UART for more data
returns true if the function should be called again straight away
*/
#define SYNC_BYTE 0xFA
AP_ExternalAHRS_VectorNav::check_uart
│
│ /********************************************************************************
│ * Initilization Complete Check *
│ ********************************************************************************/
├──> <!setup_complete>
│ └──> return false
│
│ /********************************************************************************
│ * Serial Data Avaliable Check *
│ ********************************************************************************/
├──> WITH_SEMAPHORE(state.sem)
├──> uint32_t n = uart->available()
├──> <n == 0>
│ └──> return false
├──> <pktoffset < bufsize>
│ ├──> ssize_t nread = uart->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset)))
│ ├──> <nread <= 0>
│ │ └──> return false
│ └──> pktoffset += nread
│
│ /********************************************************************************
│ * Prepare For Data Parsing *
│ ********************************************************************************/
├──> bool match_header1 = false
├──> bool match_header2 = false
├──> bool match_header3 = false
│
│ // Sync flag check
├──> <pktbuf[0] != SYNC_BYTE>
│ └──> goto reset
│
│ // Header type check
├──> <type == TYPE::VN_300>
│ ├──> match_header1 = (0 == memcmp(&pktbuf[1], vn_pkt1_header, MIN(sizeof(vn_pkt1_header), unsigned(pktoffset-1))))
│ └──> match_header2 = (0 == memcmp(&pktbuf[1], vn_pkt2_header, MIN(sizeof(vn_pkt2_header), unsigned(pktoffset-1))))
├──> < else >
│ └──> match_header3 = (0 == memcmp(&pktbuf[1], vn_100_pkt1_header, MIN(sizeof(vn_100_pkt1_header), unsigned(pktoffset-1))))
├──> <!match_header1 && !match_header2 && !match_header3>
│ └──> goto reset
│
│ // VN_PKT1
├──> <case: match_header1 && pktoffset >= VN_PKT1_LENGTH>
│ ├──> uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT1_LENGTH-1, 0)
│ ├──> <crc == 0> // got pkt1
│ │ ├──> process_packet1(&pktbuf[sizeof(vn_pkt1_header)+1])
│ │ ├──> memmove(&pktbuf[0], &pktbuf[VN_PKT1_LENGTH], pktoffset-VN_PKT1_LENGTH)
│ │ └──> pktoffset -= VN_PKT1_LENGTH
│ └──> < else >
│ └──> goto reset
│
│ // VN_PKT2
├──> <case: match_header2 && pktoffset >= VN_PKT2_LENGTH>
│ ├──> uint16_t crc = crc16_ccitt(&pktbuf[1], VN_PKT2_LENGTH-1, 0)
│ ├──> <crc == 0> // got pkt2
│ │ ├──> process_packet2(&pktbuf[sizeof(vn_pkt2_header)+1])
│ │ ├──> memmove(&pktbuf[0], &pktbuf[VN_PKT2_LENGTH], pktoffset-VN_PKT2_LENGTH)
│ │ └──> pktoffset -= VN_PKT2_LENGTH
│ └──> < else >
│ └──> goto reset
│
│ // VN_100_PKT1
├──> <case: match_header3 && pktoffset >= VN_100_PKT1_LENGTH>
│ ├──> uint16_t crc = crc16_ccitt(&pktbuf[1], VN_100_PKT1_LENGTH-1, 0)
│ ├──> <crc == 0> // got VN-100 pkt1
│ │ ├──> process_packet_VN_100(&pktbuf[sizeof(vn_100_pkt1_header)+1])
│ │ ├──> memmove(&pktbuf[0], &pktbuf[VN_100_PKT1_LENGTH], pktoffset-VN_100_PKT1_LENGTH)
│ │ └──> pktoffset -= VN_100_PKT1_LENGTH
│ └──> < else >
│ └──> goto reset
└──> return true
/********************************************************************************
* Reset Procedure for goto instruction *
********************************************************************************/
reset:
├──> uint8_t *p = (uint8_t *)memchr(&pktbuf[1], SYNC_BYTE, pktoffset-1)
├──> <p>
│ ├──> uint8_t newlen = pktoffset - (p - pktbuf)
│ ├──> memmove(&pktbuf[0], p, newlen)
│ └──> pktoffset = newlen
├──> < else >
│ └──> pktoffset = 0
└──> return true
4.2 process_packet1
/*
process packet type 1
*/
AP_ExternalAHRS_VectorNav::process_packet1
│
│ /********************************************************************************
│ * Prepare Parsing *
│ ********************************************************************************/
├──> const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)b
├──> const struct VN_packet2 &pkt2 = *last_pkt2
├──> last_pkt1_ms = AP_HAL::millis()
├──> *last_pkt1 = pkt1
│
│ /********************************************************************************
│ * GYRO & ACC & Velocity & Location update *
│ ********************************************************************************/
├──> const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU)
├──> WITH_SEMAPHORE(state.sem)
├──> <use_uncomp>
│ ├──> state.accel = Vector3f{pkt1.uncompAccel[0], pkt1.uncompAccel[1], pkt1.uncompAccel[2]}
│ └──> state.gyro = Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}
├──> < else >
│ ├──> state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}
│ └──> state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}
├──> state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}
├──> state.have_quaternion = true
├──> state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}
├──> state.have_velocity = true
├──> state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7),
│ int32_t(pkt1.positionLLA[1] * 1.0e7),
│ int32_t(pkt1.positionLLA[2] * 1.0e2),
│ Location::AltFrame::ABSOLUTE}
├──> state.have_location = true
│
│ /********************************************************************************
│ * External Baro *
│ ********************************************************************************/
├──> <AP_BARO_EXTERNALAHRS_ENABLED>
│ ├──> AP_ExternalAHRS::baro_data_message_t baro
│ ├──> baro.instance = 0
│ ├──> baro.pressure_pa = pkt1.pressure*1e3
│ ├──> baro.temperature = pkt2.temp
│ └──> AP::baro().handle_external(baro)
│
│ /********************************************************************************
│ * External Compass *
│ ********************************************************************************/
├──> <AP_COMPASS_EXTERNALAHRS_ENABLED>
│ ├──> AP_ExternalAHRS::mag_data_message_t mag
│ ├──> mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}
│ ├──> mag.field *= 1000 // to mGauss
│ └──> AP::compass().handle_external(mag)
│
│ /********************************************************************************
│ * AHRS update *
│ ********************************************************************************/
├──> AP_ExternalAHRS::ins_data_message_t ins
├──> ins.accel = state.accel
├──> ins.gyro = state.gyro
├──> ins.temperature = pkt2.temp
├──> AP::ins().handle_external(ins)
│
│ // @LoggerMessage: EAH1
│ // @Description: External AHRS data
│ // @Field: TimeUS: Time since system startup
│ // @Field: Roll: euler roll
│ // @Field: Pitch: euler pitch
│ // @Field: Yaw: euler yaw
│ // @Field: VN: velocity north
│ // @Field: VE: velocity east
│ // @Field: VD: velocity down
│ // @Field: Lat: latitude
│ // @Field: Lon: longitude
│ // @Field: Alt: altitude AMSL
│ // @Field: UXY: uncertainty in XY position
│ // @Field: UV: uncertainty in velocity
│ // @Field: UR: uncertainty in roll
│ // @Field: UP: uncertainty in pitch
│ // @Field: UY: uncertainty in yaw
│
└──> AP::logger().WriteStreaming("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
"sdddnnnDUmmnddd", "F000000GG000000",
"QffffffLLffffff",
AP_HAL::micros64(),
pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
float(pkt1.positionLLA[2]),
pkt1.posU, pkt1.velU,
pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0])
4.3 process_packet2
/*
process packet type 2
*/
AP_ExternalAHRS_VectorNav::process_packet2
│
│ /********************************************************************************
│ * Prepare Parsing *
│ ********************************************************************************/
├──> const struct VN_packet2 &pkt2 = *(struct VN_packet2 *)b
├──> const struct VN_packet1 &pkt1 = *last_pkt1
├──> last_pkt2_ms = AP_HAL::millis()
├──> *last_pkt2 = pkt2
├──> AP_ExternalAHRS::gps_data_message_t gps
│
│ /********************************************************************************
│ * GPS Data *
│ ********************************************************************************/
│ // get ToW in milliseconds
├──> gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL)
├──> gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL)
├──> gps.fix_type = pkt2.GPS1Fix
├──> gps.satellites_in_view = pkt2.numGPS1Sats
│
├──> gps.horizontal_pos_accuracy = pkt1.posU
├──> gps.vertical_pos_accuracy = pkt1.posU
├──> gps.horizontal_vel_accuracy = pkt1.velU
│
├──> gps.hdop = pkt2.GPS1DOP[4]
├──> gps.vdop = pkt2.GPS1DOP[3]
│
├──> gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7
├──> gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7
├──> gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2
│
├──> gps.ned_vel_north = pkt2.GPS1velNED[0]
├──> gps.ned_vel_east = pkt2.GPS1velNED[1]
├──> gps.ned_vel_down = pkt2.GPS1velNED[2]
│
├──> <gps.fix_type >= 3 && !state.have_origin>
│ ├──> WITH_SEMAPHORE(state.sem)
│ ├──> state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7),
│ │ int32_t(pkt2.GPS1posLLA[1] * 1.0e7),
│ │ int32_t(pkt2.GPS1posLLA[2] * 1.0e2),
│ │ Location::AltFrame::ABSOLUTE}
│ └──> state.have_origin = true
└──> <AP::gps().get_first_external_instance(instance)>
└──> AP::gps().handle_external(gps, instance)
4.4 process_packet_VN_100
/*
process VN-100 packet type 1
*/
AP_ExternalAHRS_VectorNav::process_packet_VN_100
│
│ /********************************************************************************
│ * Prepare Parsing *
│ ********************************************************************************/
├──> const struct VN_100_packet1 &pkt = *(struct VN_100_packet1 *)b
├──> last_pkt1_ms = AP_HAL::millis()
│
│ /********************************************************************************
│ * GYRO & ACC & Velocity & Location update *
│ ********************************************************************************/
├──> const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU)
├──> WITH_SEMAPHORE(state.sem)
├──> <use_uncomp>
│ ├──> state.accel = Vector3f{pkt.uncompAccel[0], pkt.uncompAccel[1], pkt.uncompAccel[2]}
│ └──> state.gyro = Vector3f{pkt.uncompAngRate[0], pkt.uncompAngRate[1], pkt.uncompAngRate[2]}
├──> < else >
│ ├──> state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}
│ └──> state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}
├──> state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}
├──> state.have_quaternion = true
│
│ /********************************************************************************
│ * External Baro *
│ ********************************************************************************/
├──> <AP_BARO_EXTERNALAHRS_ENABLED>
│ ├──> AP_ExternalAHRS::baro_data_message_t baro
│ ├──> baro.instance = 0
│ ├──> baro.pressure_pa = pkt.pressure*1e3
│ ├──> baro.temperature = pkt.temp
│ └──> AP::baro().handle_external(baro)
│
│ /********************************************************************************
│ * External Compass *
│ ********************************************************************************/
├──> <AP_COMPASS_EXTERNALAHRS_ENABLED>
│ ├──> AP_ExternalAHRS::mag_data_message_t mag
│ ├──> <use_uncomp>
│ │ └──> mag.field = Vector3f{pkt.uncompMag[0], pkt.uncompMag[1], pkt.uncompMag[2]}
│ ├──> < else >
│ │ └──> mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}
│ ├──> mag.field *= 1000 // to mGauss
│ └──> AP::compass().handle_external(mag)
│
│ /********************************************************************************
│ * AHRS update *
│ ********************************************************************************/
│ ├──> AP_ExternalAHRS::ins_data_message_t ins
│ ├──> ins.accel = state.accel
│ ├──> ins.gyro = state.gyro
│ ├──> ins.temperature = pkt.temp
│ └──> AP::ins().handle_external(ins)
│
│ // @LoggerMessage: EAH3
│ // @Description: External AHRS data
│ // @Field: TimeUS: Time since system startup
│ // @Field: Temp: Temprature
│ // @Field: Pres: Pressure
│ // @Field: MX: Magnetic feild X-axis
│ // @Field: MY: Magnetic feild Y-axis
│ // @Field: MZ: Magnetic feild Z-axis
│ // @Field: AX: Acceleration X-axis
│ // @Field: AY: Acceleration Y-axis
│ // @Field: AZ: Acceleration Z-axis
│ // @Field: GX: Rotation rate X-axis
│ // @Field: GY: Rotation rate Y-axis
│ // @Field: GZ: Rotation rate Z-axis
│ // @Field: Q1: Attitude quaternion 1
│ // @Field: Q2: Attitude quaternion 2
│ // @Field: Q3: Attitude quaternion 3
│ // @Field: Q4: Attitude quaternion 4
└──> AP::logger().WriteStreaming("EAH3", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ,Q1,Q2,Q3,Q4",
"sdPGGGoooEEE----", "F000000000000000",
"Qfffffffffffffff",
AP_HAL::micros64(),
pkt.temp, pkt.pressure*1e3,
use_uncomp ? pkt.uncompMag[0] : pkt.mag[0],
use_uncomp ? pkt.uncompMag[1] : pkt.mag[1],
use_uncomp ? pkt.uncompMag[2] : pkt.mag[2],
state.accel[0], state.accel[1], state.accel[2],
state.gyro[0], state.gyro[1], state.gyro[2],
state.quat[0], state.quat[1], state.quat[2], state.quat[3])
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
