概述
void Copter::loop()
{
......
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
}本文以GPS数据为代表，分析数据如何从硬件驱动层慢慢的流到主函数算法应用层(其它传感器数据都类似于GPS数据)，内容有点复杂，有些地方可能定位定错了，但也是并列的层，将就的算跑通了传感器数据流动过程。也麻烦看到错误的同学提醒楼主一下，以免误导大家。
经过之前pixhawkArduPilot_main启动与运行分析：
setup()函数在板子启动的时候被调用一次，它实际的调用来自每块板子的HAL，所有main函数是在HAL里的，其后就是loop()函数的调用，sketch的主要工作体现在loop()函数里。
setup、loop函数可以在ArduCopter.cpp中分别找到，其中setup函数里有scheduler.init(&scheduler_tasks[0],ARRAY_SIZE(scheduler_tasks));
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
/*
scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
(in 2.5ms units) and the maximum time they are expected to take (in
microseconds)
1 = 400hz
2 = 200hz
4 = 100hz
8 = 50hz
20 = 20hz
40 = 10hz
133 = 3hz
400 = 1hz
4000 = 0.1hz
*/
constAP_Scheduler::TaskCopter::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(update_altitude, 10, 140),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_thr_average, 100, 90),
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, 50, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
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(fifty_hz_logging_loop, 50, 110),
SCHED_TASK(full_rate_logging_loop,400, 100),
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(adsb_update, 1, 100),
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 75),
#endif
#if EPM_ENABLED == ENABLED
SCHED_TASK(epm_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
};
void Copter::setup()
|
|
__scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));void Copter::loop()
|
|
___scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);void AP_Scheduler::run(uint32_t time_available)
{
uint32_t run_started_usec = AP_HAL::micros();
uint32_t now = run_started_usec;
if (_debug > 3 && _perf_counters == nullptr) {
_perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
if (_perf_counters != nullptr) {
for (uint8_t i=0; i<_num_tasks; i++) {
_perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name);
}
}
}
for (uint8_t i=0; i<_num_tasks; i++) {
uint16_t dt = _tick_counter - _last_run[i];
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
if (interval_ticks < 1) {
interval_ticks = 1;
}
if (dt >= interval_ticks) {
// this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros;
if (dt >= interval_ticks*2) {
// we've slipped a whole run of this task!
if (_debug > 1) {
::printf("Scheduler slip task[%u-%s] (%u/%u/%u)n",
(unsigned)i,
_tasks[i].name,
(unsigned)dt,
(unsigned)interval_ticks,
(unsigned)_task_time_allowed);
}
}
if (_task_time_allowed <= time_available) {
// run it
_task_time_started = now;
current_task = i;
if (_debug > 3 && _perf_counters && _perf_counters[i]) {
hal.util->perf_begin(_perf_counters[i]);
}
_tasks[i].function();
if (_debug > 3 && _perf_counters && _perf_counters[i]) {
hal.util->perf_end(_perf_counters[i]);
}
current_task = -1;
// record the tick第一：运行相应的任务函数，_tasks[i].function();可以跳转到相应的函数里
struct Task {
task_fn_t function;
const char *name;
float rate_hz;
uint16_t max_time_micros;
};FUNCTOR_TYPEDEF(task_fn_t, void);#define FUNCTOR_TYPEDEF(name, rettype, ...)
typedef Functor<rettype, ## __VA_ARGS__> namevoid Copter::loop()
|
__scheduler.tick();
|
___tick_counter++;void AP_Scheduler::run(uint32_t time_available)
{
......
uint16_t dt = _tick_counter - _last_run[i];
//_last_run[i]是上次运行完该任务记下的循环圈数，_tick_counter 是此时循环了的圈数，所以dt 就是上次运行到现在隔的循环圈数
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
//_loop_rate_hz 是循环的频率，这里应该是50Hz，_tasks[i].rate_hz是本任务的频率，假如是20Hz，那么interval_ticks =2.5，那就应该是运行了2圈后再进来运行任务时就该执行，即中间要间隔2.5圈
......
if (dt >= interval_ticks)//只有当实际运行间隔圈数大于需要间隔的圈数，才运行任务函数
{
_tasks[i].function();
}
......
_last_run[i] = _tick_counter;//几路上次任务执行后的loop循环的圈数
}void Copter::loop()
{
......
uint32_t timer = micros();//当前时间
// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// tell the scheduler one tick has passed
scheduler.tick();
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
//可用的时间=(进入loop()时的时间+主循环周期2.5ms)-此时的时间=主循环的时间-已经运行的程序花的时间=剩下的时间
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
}
void AP_Scheduler::run(uint32_t time_available)
{
uint32_t now = run_started_usec;
......
_task_time_allowed = _tasks[i].max_time_micros;//任务所允许花的最大的时间
if (_task_time_allowed <= time_available) {//任务所需的时间<剩余的时间
{
_task_time_started = now;//任务开始时的时间
......
_tasks[i].function();
......
now = AP_HAL::micros();//执行完任务的时间
uint32_t time_taken = now - _task_time_started;//任务所花的时间
if (time_taken > _task_time_allowed)
{......}
if (time_taken >= time_available) {
goto update_spare_ticks;
}
time_available -= time_taken;//剩余的时间-各个任务所花的时间的和
}
}(2017-08-30更改)
此段程序是fastloop之外的主程序调用，分别是任务函数、调用时间间隔（1代表1×2.5ms）、最长运行时间
进入AP_Scheduler
/*
A task scheduler for APM main loops
Sketches should call scheduler.init() on startup, then call
scheduler.tick() at regular intervals (typically every 10ms).
To run tasks use scheduler.run(), passing the amount of time that
the scheduler is allowed to use before it must return
*/
APM主循环的任务调度
应首先调用scheduler.init()启动初始化，每隔一段时间调用scheduler.tick()(比如10ms)，调用scheduler.run()运行任务程序（但必须在最长时间内）
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle.h>
classAP_Scheduler
{
public:
// constructor
AP_Scheduler(void);
FUNCTOR_TYPEDEF(task_fn_t, void);
struct Task {
task_fn_t function;
const char *name;
floatrate_hz;
uint16_t max_time_micros;
};
// initialise scheduler
void init(const Task *tasks, uint8_t num_tasks);就是上面的scheduler.init()
这个函数就是主程序void Copter::setup()里面的
scheduler.init(&scheduler_tasks[0],ARRAY_SIZE(scheduler_tasks));
voidAP_Scheduler::init(constAP_Scheduler::Task *tasks, uint8_t num_tasks)
{
_tasks = tasks;
_num_tasks = num_tasks;
_last_run = new uint16_t[_num_tasks];
memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks);
_tick_counter = 0;
}
应该是给每个任务开启相应大小的空间
// call when one tick has passed
void tick(void); 就是上面的 scheduler.tick()
这个函数就是主程序void Copter::loop()里面的scheduler.tick();
voidAP_Scheduler::tick(void)
{
_tick_counter++;
}
// run the tasks. Call this once per'tick'.
// time_available is the amount of time available to run
// tasks in microseconds
void run(uint16_t time_available); 就是上面的 scheduler.run()
这个函数就是主程序void Copter::loop()里面的scheduler.run(time_available);
voidAP_Scheduler::run(uint16_t time_available)
{
uint32_t run_started_usec = AP_HAL::micros();
uint32_t now = run_started_usec;
for (uint8_t i=0; i<_num_tasks; i++) {
uint16_tdt = _tick_counter - _last_run[i];
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
if (interval_ticks< 1) {
interval_ticks = 1;
}
if (dt>= interval_ticks) {
// this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros;
if (dt>= interval_ticks*2) {
// we've slipped a whole run of this task!
if (_debug > 1) {
hal.console->printf("Scheduler slip task[%u-%s] (%u/%u/%u)n",
(unsigned)i,
_tasks[i].name,
(unsigned)dt,
(unsigned)interval_ticks,
(unsigned)_task_time_allowed);
}
}
if (_task_time_allowed<= time_available) {
// run it
_task_time_started = now;
current_task = i;
_tasks[i].function();这句话执行任务函数
current_task = -1;
// record the tick counter when we ran. This drives
// when we next run the event
_last_run[i] = _tick_counter;
// work out how long the event actually took
now = AP_HAL::micros();
uint32_t time_taken = now - _task_time_started;
if (time_taken> _task_time_allowed) {
// the event overran!
if (_debug > 2) {
hal.console->printf("Scheduler overrun task[%u-%s] (%u/%u)n",
(unsigned)i,
_tasks[i].name,
(unsigned)time_taken,
(unsigned)_task_time_allowed);
}
}
if (time_taken>= time_available) {
gotoupdate_spare_ticks;
}
time_available -= time_taken;
}
}
}
// update number of spare microseconds
_spare_micros += time_available;
update_spare_ticks:
_spare_ticks++;
if (_spare_ticks == 32) {
_spare_ticks /= 2;
_spare_micros /= 2;
}
}
使得每个任务在指定的时间（多个tick）内运行。这个函数是计算总共多少时间、用了多少时间、还剩多少时间可用、再通过_tasks[i].function();跳到相应的任务函数地址中，由此开始各种任务！
// return the number of microseconds available for the current task
uint16_t time_available_usec(void);
// return debug parameter
uint8_t debug(void) { return _debug; }
// return load average, as a number between 0 and 1. 1 means
// 100% load. Calculated from how much spare time we have at the
// end of a run()下载进度，返回值为0~1，从完成任务剩余时间中算出
floatload_average(uint32_t tick_time_usec) const;
// get the configured main loop rate
uint16_t get_loop_rate_hz(void) const {
return _loop_rate_hz;
}
staticconststructAP_Param::GroupInfovar_info[];
// current running task, or -1 if none. Used to debug stuck tasks
static int8_t current_task;
private:
// used to enable scheduler debugging
AP_Int8 _debug;
// overall scheduling rate in Hz
AP_Int16 _loop_rate_hz;
// progmem list of tasks to run
conststruct Task *_tasks;
// number of tasks in _tasks list
uint8_t _num_tasks;
// number of 'ticks' that have passed (number of times that
// tick() has been called
uint16_t _tick_counter;
// tick counter at the time we last ran each task
uint16_t *_last_run;
// number of microseconds allowed for the current task
uint32_t _task_time_allowed;
// the time in microseconds when the task started
uint32_t _task_time_started;
// number of spare microseconds accumulated
uint32_t _spare_micros;
// number of ticks that _spare_micros is counted over
uint8_t _spare_ticks;
};
任务间的时间控制，任务调度，现在得出的结论是基于tick的计数，控制在指定时间内，具体过程，暂不清楚。
经上述分析，函数通过_voidCopter::loop() 里面的scheduler.run(time_available)函数中的tasks[i].function()语句进入各任务，以SCHED_TASK(update_GPS,50, 200)为例
进入 SCHED_TASK
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter, func, rate_hz, max_time_micros)
进入SCHED_TASK_CLASS
/*
useful macro for creating scheduler task table建立调度程序任务列表的宏
*/
#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) {
.function = FUNCTOR_BIND(classptr, &classname::func, void),调用&classname::func就是&Copter::update_GPS
AP_SCHEDULER_NAME_INITIALIZER(func)
.rate_hz = _rate_hz,
.max_time_micros = _max_time_micros
}
#define FUNCTOR_BIND(obj, func, rettype, ...)
Functor<rettype, ## __VA_ARGS__>::bind<std::remove_reference<decltype(*obj)>::type, func>(obj)
从这里开始具体实现过程看不懂了，主要是通过某种方式(指针或者其他的方式)将程序跳到相应的任务地址。
那么进入update_GPS函数
// called at 50hz
void Copter::update_GPS(void)
{
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
boolgps_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)) {
DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
}
gps_updated = true;
}
}
if (gps_updated) {
// set system time if necessary
set_system_time_from_GPS();
// checks to initialise home and take location based pictures
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
#if CAMERA == ENABLED
if (camera.update_location(current_loc, copter.ahrs) == true) {
do_take_picture();
}
#endif
}
}
}这段程序主要做的是跟新GPS信息、log GPS信息、对获取的GPS信息判断并处理（只是为了有逻辑的运行，并没必要详细阅读）。重点是gps.update();
进入gps.update();/*
update all GPS instances
*/
void
AP_GPS::update(void)
{
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
update_instance(i);
}
// work out which GPS is the primary, and how many sensors we have
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
if (state[i].status != NO_GPS) {
num_instances = i+1;
}
if (_auto_switch) {
if (i == primary_instance) {
continue;
}
if (state[i].status > state[primary_instance].status) {
// we have a higher status lock, change GPS
primary_instance = i;
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) {
uint32_t now = AP_HAL::millis();
bool another_gps_has_2_or_more_sats = (state[i].num_sats>= state[primary_instance].num_sats + 2);
if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a
// position shift to the controllers.
primary_instance = i;
_last_instance_swap_ms = now;
}
}
} else {
primary_instance = 0;
}
}
// update notify with gps status. We always base this on the primary_instance
AP_Notify::flags.gps_status = state[primary_instance].status;
AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
}
这段代码数据跟新是通过
for (uint8_t i=0; i<GPS_MAX_INSTANCES;i++) {
update_instance(i);
}
实现的，接下来的是一个选取哪个GPS作为起作用的GPS的算法。
/*
update one GPS instance. This should be called at 10Hz or greater
*/
void
AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_HIL) {
// in HIL, leave info alone
return;
}
if (_type[instance] == GPS_TYPE_NONE) {
// not enabled
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
return;
}
if (locked_ports & (1U<<instance)) {
// the port is locked by another driver
return;
}
if (drivers[instance] == NULL || state[instance].status == NO_GPS) {
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance(instance);
return;
}
send_blob_update(instance);
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = AP_HAL::millis();
// if we did not get a message, and the idle timer of 2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > 2000) {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = NULL;
memset(&state[instance], 0, sizeof(state[instance]));
state[instance].instance = instance;
state[instance].status = NO_GPS;
state[instance].hdop = 9999;
timing[instance].last_message_time_ms = tnow;
}
} else {
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
}
}这段代码bool result =drivers[instance]->read();是实现数据跟新的函数，之后有一个计时，如果2s没收到数据则重新初始化GPS。
再跟踪read函数
class AP_GPS_Backend
{
public:
AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// we declare a virtual destructor so that GPS drivers can
// override with a custom destructor if need be.
virtual ~AP_GPS_Backend(void) {}
// The read() method is the only one needed in each driver. It
// should return true when the backend has successfully received a
// valid packet from the GPS.
virtual bool read() = 0;
// Highest status supported by this GPS.
// Allows external system to identify type of receiver connected.
virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }
virtual bool is_configured(void) { return true; }
virtual void inject_data(uint8_t *data, uint8_t len) { return; }
//MAVLink methods
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }
virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan) { return ; }
virtual void broadcast_configuration_failure_reason(void) const { return ; }
protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance
// common utility functions
int32_t swap_int32(int32_t v) const;
int16_t swap_int16(int16_t v) const;
/*
fill in 3D velocity from 2D components
*/
void fill_3d_velocity(void);
/*
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
*/
void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds);
};
跟新数据的是这个函数virtual bool read() = 0;至此无法直接跟踪下去。
这些接口都被定义成了 virtual函数，于是脑补了下C++关于virtual函数的语法。
C++纯虚函数和抽象类在C++中，可以将成员函数声明为纯虚函数，语法格式为：
virtual 函数返回类型 函数名 (函数参数)= 0;
纯虚函数没有函数体，只有函数声明，在虚函数声明结尾加上=0，表明此函数为纯虚函数。
最后的=0并不表示函数返回值为0，它只起形式上的作用，告诉编译系统“这是纯虚函数”。
包含纯虚成员函数的类称为 抽象类（AbstractClass） 。之所以说它抽象，是因为它无法实例化，也就是无法创建对象。原因很明显，纯虚函数没有函数体，不是完整的函数，无法调用，也无法为其分配内存空间。抽象类通常是作为基类，让派生类去实现纯虚函数。派生类必须实现纯虚函数才能被实例化。
纯虚函数使用举例：
#include <iostream>
using namespace std;
//线
class Line{
protected:
float len;
public:
Line(float len): len(len){}
virtual float area() = 0;
virtual float volume() = 0;
};
//矩形
class Rec: public Line{
protected:
float width;
public:
Rec(float len, float width): Line(len),width(width){}
float area(){ return len*width; }
};
//长方体
class Cuboid: public Rec{
protected:
float height;
public:
Cuboid(float len, float width, float height): Rec(len, width), height(height){}
float area(){ return 2*(len*width + len*height + width*height); }
float volume(){ return len*width*height; }
};
//正方体
class Cube: public Cuboid{
public:
Cube(float len): Cuboid(len, len, len){}
float area(){ return 6*len*len; }
float volume(){ return len*len*len; }
};
int main(){
Line *p = new Cuboid(10, 20, 30);
cout<<"The area of Cuboid is "<<p->area()<<endl;
cout<<"The volume of Cuboid is "<<p->volume()<<endl;
p = new Cube(15);
cout<<"The area of Cube is "<<p->area()<<endl;
cout<<"The volume of Cube is "<<p->volume()<<endl;
return 0;
}
运行结果：
The area of Cuboid is 2200
The volume of Cuboid is 6000
The area of Cube is 1350
The volume of Cube is 3375
本例中定义了四个类，它们的继承关系为：Line --> Rec --> Cuboid --> Cube。
Line 是一个抽象类，也是最顶层的基类，在 Line类中定义了两个纯虚函数 area() 和 volume()。
在 Rec 类中，实现了 area() 函数；所谓实现，就是定义了纯虚函数的函数体。但这时 Rec 仍不能被实例化，因为它没有实现继承来的 volume() 函数，volume() 仍然是纯虚函数，所以 Rec 也仍然是抽象类。
直到 Cuboid 类，才实现了 volume() 函数，才是一个完整的类，才可以被实例化。
可以发现，Line 类表示“线”，没有面积和体积，但它仍然定义了 area() 和 volume() 两个纯虚函数。这样的用意很明显：Line 类不需要被实例化，但是它为派生类提供了“约束条件”，派生类必须要实现这两个函数，完成计算面积和体积的功能，否则就不能实例化。
在实际开发中，你可以定义一个抽象基类，只完成部分功能，未完成的功能交给派生类去实现（谁派生谁实现）。这部分未完成的功能，往往是基类不需要的，或者在基类中无法实现的。虽然抽象基类没有完成，但是却强制要求派生类完成，这就是抽象基类的“霸王条款”。
抽象基类除了约束派生类的功能，还可以实现多态。请注意代码第 39行，指针 p 的类型是 Line，但是它却可以访问派生类中的 area() 和 volume() 函数，正是由于在 Line 类中将这两个函数定义为纯虚函数；如果不这样做，39行后面的代码都是错误的。我想，这或许才是C++提供纯虚函数的主要目的。
熟悉C++的肯定都知道这表示什么。所以我们只要在子类中重写这些virtual函数应用程序就会调用到我们自己写的函数，如果你不写，那自然就调用父类的。
由此可类推
class AP_GPS_Backend相当于classLine，所以里面只有virtual函数的声明
virtual bool read() = 0;相当于virtualfloat area() = 0;
所以需要查找virtual的实例化
class Rec: public Line是classLine的实例化的类，float area(){ return len*width; }是classLine中virtual float area()的实例化的方法
类比，因此要先找class AP_GPS_Backend实例化的类，通过Ctrl+H全局搜索public AP_GPS_Backend
bool read();就是class AP_GPS_Backend中virtualbool read()的实例化的方法
由于直接进入read函数进不去，所以进一步搜索AP_GPS_PX4::read
终于找到了boolresult =drivers[instance]->read();的下一层，read的实例化。
顺便看下module.pre.o.map文件，复制了部分代码。
Memory Configuration
Name Origin Length Attributes
*default* 0x00000000 0xffffffff
Linker script and memory map
LOAD c:/ardupilot/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp.o
LOAD libraries/AP_ADC/AP_ADC.cpp.o
LOAD libraries/AP_ADC/AP_ADC_ADS1115.cpp.o
LOAD libraries/AP_ADC/AP_ADC_ADS7844.cpp.o
LOAD libraries/AP_AHRS/AP_AHRS.cpp.o
LOAD libraries/AP_AHRS/AP_AHRS_DCM.cpp.o
LOAD libraries/AP_AHRS/AP_AHRS_NavEKF.cpp.o
LOAD libraries/AP_Airspeed/AP_Airspeed.cpp.o
LOAD libraries/AP_Airspeed/AP_Airspeed_I2C.cpp.o
.interp
*(.interp)
.note.gnu.build-id
*(.note.gnu.build-id)
.hash
*(.hash)
.gnu.hash
*(.gnu.hash)
.dynsym
*(.dynsym)
.dynstr
*(.dynstr)
.gnu.version
*(.gnu.version)
.text._ZN10AP_GPS_PX44readEv
0x00000000 0x1b0 libraries/AP_GPS/AP_GPS_PX4.cpp.o
0x00000000 AP_GPS_PX4::read ()
.text._ZN10AP_GPS_PX4C2ER6AP_GPSRNS0_9GPS_StateEPN6AP_HAL10UARTDriverE
0x00000000 0x20
.text._ZN10AP_GPS_PX4C2ER6AP_GPSRNS0_9GPS_StateEPN6AP_HAL10UARTDriverE
0x00000000 0x20 libraries/AP_GPS/AP_GPS_PX4.cpp.o
0x00000000 AP_GPS_PX4::AP_GPS_PX4(AP_GPS&, AP_GPS::GPS_State&, AP_HAL::UARTDriver*)
0x00000000 AP_GPS_PX4::AP_GPS_PX4(AP_GPS&, AP_GPS::GPS_State&, AP_HAL::UARTDriver*)
.text._ZN10AP_GPS_SBF24highest_supported_statusEv
0x00000000 0x4
.text._ZN10AP_GPS_SBF24highest_supported_statusEv
0x00000000 0x4 libraries/AP_GPS/AP_GPS_SBF.cpp.o
0x00000000 AP_GPS_SBF::highest_supported_status()
.text._ZN10AP_GPS_SBFD2Ev
0x00000000 0xc
.text._ZN10AP_GPS_SBFD2Ev
0x00000000 0xc libraries/AP_GPS/AP_GPS_SBF.cpp.o
0x00000000 AP_GPS_SBF::~AP_GPS_SBF()
0x00000000 AP_GPS_SBF::~AP_GPS_SBF()
.text._ZN10AP_GPS_SBFD0Ev
0x00000000 0x14
.text._ZN10AP_GPS_SBFD0Ev
0x00000000 0x14 libraries/AP_GPS/AP_GPS_SBF.cpp.o
0x00000000 AP_GPS_SBF::~AP_GPS_SBF()
.text._ZN10AP_GPS_SBF11inject_dataEPhh
0x00000000 0x28
.text._ZN10AP_GPS_SBF11inject_dataEPhh
0x00000000 0x28 libraries/AP_GPS/AP_GPS_SBF.cpp.o
0x00000000 AP_GPS_SBF::inject_data(unsigned char*, unsigned char)
.text._ZN10AP_GPS_SBFC2ER6AP_GPSRNS0_9GPS_StateEPN6AP_HAL10UARTDriverE
0x00000000 0x54
.text._ZN10AP_GPS_SBFC2ER6AP_GPSRNS0_9GPS_StateEPN6AP_HAL10UARTDriverE
0x00000000 0x54 libraries/AP_GPS/AP_GPS_SBF.cpp.o
0x00000000 AP_GPS_SBF::AP_GPS_SBF(AP_GPS&, AP_GPS::GPS_State&, AP_HAL::UARTDriver*)
0x00000000 AP_GPS_SBF::AP_GPS_SBF(AP_GPS&, AP_GPS::GPS_State&, AP_HAL::UARTDriver*)
这个相当于DSP里面的.map文件，就是显示控制器内存如何分配的
DSP中是通过CMD文件生成.map文件，pixhawk中module.pre.o.map应该是通过bulitin_commands生成的。(此处是猜测)AP_GPS_PX4::read函数主体为
// update internal state if new GPS information is available
bool
AP_GPS_PX4::read(void)
{
bool updated = false;
orb_check(_gps_sub, &updated);
if (updated) {
if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) {
state.last_gps_time_ms = AP_HAL::millis();
state.status = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX);
state.num_sats = _gps_pos.satellites_used;
state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f);
if (_gps_pos.fix_type >= 2) {
state.location.lat = _gps_pos.lat;
state.location.lng = _gps_pos.lon;
state.location.alt = _gps_pos.alt/10;
state.ground_speed = _gps_pos.vel_m_s;
state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100);
state.hdop = _gps_pos.eph*100;
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine
// raw week information (or APM switches to Posix epoch ;-) )
uint64_t epoch_ms = uint64_t(_gps_pos.time_utc_usec/1000.+.5);
uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014;
state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK));
state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK);
if (_gps_pos.time_utc_usec == 0) {
// This is a work-around for https://github.com/PX4/Firmware/issues/1474
// reject position reports with invalid time, as APM adjusts it's clock after the first lock has been aquired
state.status = AP_GPS::NO_FIX;
}
}
if (_gps_pos.fix_type >= 3) {
state.have_vertical_velocity = _gps_pos.vel_ned_valid;
state.velocity.x = _gps_pos.vel_n_m_s;
state.velocity.y = _gps_pos.vel_e_m_s;
state.velocity.z = _gps_pos.vel_d_m_s;
state.speed_accuracy = _gps_pos.s_variance_m_s;
state.have_speed_accuracy = true;
}
else {
state.have_vertical_velocity = false;
}
}
}
return updated;
}
orb_check(_gps_sub, &updated);是通过orb机制检测是否gps已经有新的数据，具体如何实现另立一节
if (OK == orb_copy(ORB_ID(vehicle_gps_position),_gps_sub, &_gps_pos))这一句才是gps的信息来源，之后的都是将数据处理下赋值给其它变量。
进入orb_copy()
/**
* Fetch data from a topic.
*
* This is the only operation that will reset the internal marker that
* indicates that a topic has been updated for a subscriber. Once poll
* or check return indicating that an updaet is available, this call
* must be used to update the subscription.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param handle A handle returned from orb_subscribe.
* @param buffer Pointer to the buffer receiving the data, or NULL
* if the caller wants to clear the updated flag without
* using the data.
* @return OK on success, ERROR otherwise with errno set accordingly.
*/
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);
}
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
int ret;
ret = read(handle, buffer, meta->o_size);
if (ret < 0) {
return ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return ERROR;
}
return OK;
}
进入ret = read(handle, buffer, meta->o_size);
ssize_t read(int fd, FAR void *buf, size_t nbytes)
{
/* Did we get a valid file descriptor? */
#if CONFIG_NFILE_DESCRIPTORS > 0
if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
{
/* No.. If networking is enabled, read() is the same as recv() with
* the flags parameter set to zero.
*/
#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
return recv(fd, buf, nbytes, 0);
#else
/* No networking... it is a bad descriptor in any event */
set_errno(EBADF);
return ERROR;
#endif
}
/* The descriptor is in a valid range to file descriptor... do the read */
#if CONFIG_NFILE_DESCRIPTORS > 0
return file_read(fd, buf, nbytes);
#endif
}
进file_read(fd, buf, nbytes);
/****************************************************************************
* Private Functions
****************************************************************************/
#if CONFIG_NFILE_DESCRIPTORS > 0
static inline ssize_t file_read(int fd, FAR void *buf, size_t nbytes)
{
FAR struct filelist *list;
int ret = -EBADF;
/* Get the thread-specific file list */
list = sched_getfiles();
if (!list)
{
/* Failed to get the file list */
ret = -EMFILE;
}
/* Were we given a valid file descriptor? */
else if ((unsigned int)fd < CONFIG_NFILE_DESCRIPTORS)
{
FAR struct file *this_file = &list->fl_files[fd];
FAR struct inode *inode = this_file->f_inode;
/* Yes.. Was this file opened for read access? */
if ((this_file->f_oflags & O_RDOK) == 0)
{
/* No.. File is not read-able */
ret = -EACCES;
}
/* Is a driver or mountpoint registered? If so, does it support
* the read method?
*/
else if (inode && inode->u.i_ops && inode->u.i_ops->read)
{
/* Yes.. then let it perform the read. NOTE that for the case
* of the mountpoint, we depend on the read methods bing
* identical in signature and position in the operations vtable.
*/
ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);
}
}
/* If an error occurred, set errno and return -1 (ERROR) */
if (ret < 0)
{
set_errno(-ret);
return ERROR;
}
/* Otherwise, return the number of bytes read */
return ret;
}
#endif
/* This structure is provided by devices when they are registered with the
* system. It is used to call back to perform device specific operations.
*/
struct file;
struct pollfd;
struct file_operations
{
/* The device driver open method differs from the mountpoint open method */
int (*open)(FAR struct file *filp);
/* The following methods must be identical in signature and position because
* the struct file_operations and struct mountp_operations are treated like
* unions.
*/
int (*close)(FAR struct file *filp);
ssize_t (*read)(FAR struct file *filp, FAR char *buffer, size_t buflen);
ssize_t (*write)(FAR struct file *filp, FAR const char *buffer, size_t buflen);
off_t (*seek)(FAR struct file *filp, off_t offset, int whence);
int (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);
#ifndef CONFIG_DISABLE_POLL
int (*poll)(FAR struct file *filp, struct pollfd *fds, bool setup);
#endif
/* The two structures need not be common after this point */
};
这种结构是由设备注册系统时提供的。它是用来调用执行特定于设备的操作。猜测应该是通过一些具体的操作比如某个地方选择px4，接下来到具体芯片的驱动层了
于是搜索file_operations，因为知道px4的底层驱动的路径，于是直接进来看stm32的驱动文件
进入stm32_read()
static ssize_t stm32_read(struct file *filep, char *buffer, size_t buflen)
{
if (sem_wait(&g_rngdev.rd_devsem) != OK)
{
return -errno;
}
else
{
/* We've got the semaphore. */
/* Initialize semaphore with 0 for blocking until the buffer is filled from
* interrupts.
*/
sem_init(&g_rngdev.rd_readsem, 0, 1);
g_rngdev.rd_buflen = buflen;
g_rngdev.rd_buf = buffer;
/* Enable RNG with interrupts */
stm32_enable();
/* Wait until the buffer is filled */
sem_wait(&g_rngdev.rd_readsem);
/* Free RNG for next use */
sem_post(&g_rngdev.rd_devsem);
return buflen;
}
}
/* Initialize semaphore with 0 for blockinguntil the buffer is filled from
*interrupts.
*/
由此注释可看出buffer是由中断填满的，由stm32_enable();使能中断，进入中断
static int stm32_interrupt(int irq, void *context)
{
uint32_t rngsr;
uint32_t data;
rngsr = getreg32(STM32_RNG_SR);
if ((rngsr & (RNG_SR_SEIS | RNG_SR_CEIS)) /* Check for error bits */
|| !(rngsr & RNG_SR_DRDY)) /* Data ready must be set */
{
/* This random value is not valid, we will try again. */
return OK;
}
data = getreg32(STM32_RNG_DR);
/* As required by the FIPS PUB (Federal Information Processing Standard
* Publication) 140-2, the first random number generated after setting the
* RNGEN bit should not be used, but saved for comparison with the next
* generated random number. Each subsequent generated random number has to be
* compared with the previously generated number. The test fails if any two
* compared numbers are equal (continuous random number generator test).
*/
if (g_rngdev.rd_first)
{
g_rngdev.rd_first = false;
g_rngdev.rd_lastval = data;
return OK;
}
if (g_rngdev.rd_lastval == data)
{
/* Two subsequent same numbers, we will try again. */
return OK;
}
/* If we get here, the random number is valid. */
g_rngdev.rd_lastval = data;
if (g_rngdev.rd_buflen >= 4)
{
g_rngdev.rd_buflen -= 4;
*(uint32_t*)&g_rngdev.rd_buf[g_rngdev.rd_buflen] = data;
}
else
{
while (g_rngdev.rd_buflen > 0)
{
g_rngdev.rd_buf[--g_rngdev.rd_buflen] = (char)data;
data >>= 8;
}
}
if (g_rngdev.rd_buflen == 0)
{
/* Buffer filled, stop further interrupts. */
stm32_disable();
sem_post(&g_rngdev.rd_readsem);
}
return OK;
}
data = getreg32(STM32_RNG_DR);这一句是数据来源
#define STM32_RNG_DR (STM32_RNG_BASE+STM32_RNG_DR_OFFSET)#define STM32_RNG_DR_OFFSET 0x0008 /* RNG Data Register */至此终于知道GPS数据来自于0x0008这个地址。
如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!
最后
以上就是从容大船为你收集整理的pixhawk 从main开始分析传感器数据如何流动起来,以GPS为例的全部内容，希望文章能够帮你解决pixhawk 从main开始分析传感器数据如何流动起来,以GPS为例所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复