Ardupilot前后端基于NEMA协议GPS解包过程
我是李锡龙，在生日的这天给大家带来ardupilot使用NEMA类型GPS，从应用层前端调用，到后端的GPS解包协议过程。
这里我们先从ArduCopter任务调度器开始：
前端的调用：
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(rc_loop, 100, 130),
SCHED_TASK(throttle_loop, 50, 75),
SCHED_TASK(update_GPS, 50, 200),
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK(read_aux_switches, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_proximity, 100, 50),
SCHED_TASK(update_beacon, 400, 50),
SCHED_TASK(update_visual_odom, 400, 50),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
SCHED_TASK(smart_rtl_save_position, 3, 100),
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK(compass_accumulate, 100, 100),
SCHED_TASK(barometer_accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(fourhundred_hz_logging,400, 50),
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(update_my_led, 2, 90),
SCHED_TASK(update_my_telem2, 10, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK(dataflash_periodic, 400, 300),
SCHED_TASK(perf_update, 0.1, 75),
SCHED_TASK(read_receiver_rssi, 10, 75),
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100),
#endif
SCHED_TASK(terrain_update, 10, 100),
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK(gripper_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
SCHED_TASK(button_update, 5, 100),
SCHED_TASK(stats_update, 1, 100),
};
上面任务调度器源自ArduCopter.cpp，关于任务调度器我将会在另外一篇博文中给大家带来介绍，在这里我们先简单的了解下，第一项是这个任务的内容，比如在OPTFLOW使能后，update_optical_flow代表更新光流，可进一步追踪，第二项数字代表每多少HZ会被调用一次，第三项数字代表这个任务所能使用的最大时间，单位是ms。
此时再将目光移到咱的update_GPS，可以看到它是每50HZ调用一次进行GPS更新的，接着追踪
// called at 50hz
void Copter::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
bool gps_updated = false;
gps.update();
// log after every gps message
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
// log GPS message
if (should_log(MASK_LOG_GPS) && !ahrs.have_ekf_logging()) {
DataFlash.Log_Write_GPS(gps, i);
}
gps_updated = true;
}
}
if (gps_updated) {
// set system time if necessary
set_system_time_from_GPS();
#if CAMERA == ENABLED
camera.update();
#endif
}
}
gps.update实际上是AP_GPS::update()的前端调用
如何进入后端：
在最近写自己的飞控才真的体会到这样一种前后端的魅力所在，尤其是现在各项任务的分工更加明确，每个团队负责的地方不同，接口的统一显得更加重要，我们以后想添加自己的传感器，只需要在后端添加相应的驱动程序，然后按照接口导入数据，就可以不用修改前端的代码，这样一来大大提高了开发项目的效率，以及代码的可用性，说远了，言归正传进入libraries中的AP_GPS，进入其中的update函数如下(AP_GPS.cpp):
/*
update all GPS instances
*/
void AP_GPS::update(void)
{
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
update_instance(i);
}
// calculate number of instances
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (state[i].status != NO_GPS) {
num_instances = i+1;
}
}
// if blending is requested, attempt to calculate weighting for each GPS
if (_auto_switch == 2) {
_output_is_blended = calc_blend_weights();
// adjust blend health counter
if (!_output_is_blended) {
_blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
} else if (_blend_health_counter > 0) {
_blend_health_counter--;
}
// stop blending if unhealthy
if (_blend_health_counter >= 50) {
_output_is_blended = false;
}
} else {
_output_is_blended = false;
_blend_health_counter = 0;
}
if (_output_is_blended) {
// Use the weighting to calculate blended GPS states
calc_blended_state();
// set primary to the virtual instance
primary_instance = GPS_BLENDED_INSTANCE;
} else {
// use switch logic to find best GPS
uint32_t now = AP_HAL::millis();
if (_auto_switch >= 1) {
// handling switching away from blended GPS
if (primary_instance == GPS_BLENDED_INSTANCE) {
primary_instance = 0;
for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
// choose GPS with highest state or higher number of satellites
if ((state[i].status > state[primary_instance].status) ||
((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
primary_instance = i;
_last_instance_swap_ms = now;
}
}
} else {
// handle switch between real GPSs
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
if (i == primary_instance) {
continue;
}
if (state[i].status > state[primary_instance].status) {
// we have a higher status lock, or primary is set to the blended GPS, change GPS
primary_instance = i;
_last_instance_swap_ms = now;
continue;
}
bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
bool another_gps_has_2_or_more_sats = (state[i].num_sat
最低0.47元/天 解锁文章
5543
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
