Chinaunix首页 | 论坛 | 博客
  • 博客访问: 1706679
  • 博文数量: 98
  • 博客积分: 667
  • 博客等级: 上士
  • 技术积分: 1631
  • 用 户 组: 普通用户
  • 注册时间: 2009-04-27 15:59
个人简介

一沙一世界 一树一菩提

文章分类

全部博文(98)

文章存档

2021年(8)

2020年(16)

2019年(8)

2017年(1)

2016年(11)

2015年(17)

2014年(9)

2013年(4)

2012年(19)

2011年(1)

2009年(4)

分类: 嵌入式

2019-12-25 23:37:08

## 2016-12-8 启动部分
 apm的启动脚本在/ardupilot/mk/PX4/ROMFS/init.d/目录下。这个脚本先执行,然后会继续执行同目录下的rc.APM脚本。
同时,在/ardupilot/mk/PX4/ROMFS目录下,还有px4的bootloader,px4io。在rc.APM最后启动ArduPilot,具体命令
如下:if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start。


在同一文件中,定义deviceA deviceC deviceD:


set deviceA /dev/ttyACM0
    set deviceC /dev/ttyS1
    set deviceD /dev/ttyS2


   整个代码的入口在ardupilot/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp文件。


void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) const函数中下面的这部分对应脚本
里的ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start这一行:


  for (i=0; i         if (strcmp(argv[i], "start") == 0) {
            if (thread_running) {
                printf("%s already running\n", SKETCHNAME);
                /* this is not an error */
                exit(0);
            }


            uartADriver.set_device_path(deviceA);
            uartCDriver.set_device_path(deviceC);
            uartDDriver.set_device_path(deviceD);
            uartEDriver.set_device_path(deviceE);
            printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\n",
                   SKETCHNAME, deviceA, deviceC, deviceD, deviceE);


            _px4_thread_should_exit = false;
            daemon_task = px4_task_spawn_cmd(SKETCHNAME,
                                             SCHED_FIFO,
                                             APM_MAIN_PRIORITY,
                                             APM_MAIN_THREAD_STACK_SIZE,
                                             main_loop,
                                             NULL);
            exit(0);
    }


  上面代码中的start就是脚本里的start。
  在arducopter.cpp文件的最下面有一行:


AP_HAL_MAIN_CALLBACKS(&copter);
   而AP_HAL_MAIN_CALLBACKS定义在libraries/AP_HAL/ AP_HAL_Main.h中
#define AP_MAIN __EXPORT ArduPilot_main


    #define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, CALLBACKS); \
        return 0; \
    } \
    }


所以,这句可以修改为如下形式:


int __EXPORT ArduPilot_main(int argc, char* const argv[]);
int __EXPORT ArduPilot_main(int argc, char* const argv[])
{
         hal.run(argc, argv, &copter);
}
copter,hal都为全局变量,hal在hal_px4_class.cpp文件最下面实例化,copter在
在ArduCopter/Copter.cpp文件中定义并初始化,copter和hal都在copter.h文件中说明为extern。
然后AP_HAL_Main.h中的 hal.run(argc, argv, CALLBACKS)实际为:
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks)函数,并且callbacks就是copter。


在rc.APM中执行的命令ArduPilot-d $deviceA -d2 $deviceC -d3 $deviceD start,其实就是执行hal.run(argc,argv, &copter);
px4的hal类定义在libraries/ AP_HAL_PX4/ HAL_PX4_Class.cpp文件中,HAL_PX4::run相当于main函数。
通过此页的usage函数,可以看到此命令可以带4个参数,-d 第一个电台使用串口,-d2第二个电台使用串口,-d3 第三个电台使用串口,-d4第二个GPS使用串口。命令start后,通过px4_task_spawn_cmd创建了一个任务,其入口函数为main_loop,相当于单片机中的main函数。main_loop函数先初始化,包括copter的setup函数。然后循环执行copter的loop函数。
也就是相当于单片机的while(1)循环。


另外,如果我们想运行某一个例子程序,如libraries/ap_gps/examples/gps_auto_test.cpp这个例子。
在这个代码的最开始有一句:


const AP_HAL::HAL& hal = AP_HAL::get_HAL();
这里取到的hal引用是libraries/ap_hal_px4/hal_px4_class.cpp中的hal_px4这个类。
在gps_auto_test.cpp这个文件,最下面的一句:


    AP_HAL_MAIN();
执行的是libraries/ap_hal/ap_hal_main.h中的下面内容:


    #define AP_HAL_MAIN() \
    AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
    extern "C" {                               \
    int AP_MAIN(int argc, char* const argv[]); \
    int AP_MAIN(int argc, char* const argv[]) { \
        hal.run(argc, argv, &callbacks); \
        return 0; \
    } \
    }


即用gps_auto_test.cpp中的setup和loop定义一个 AP_HAL::HAL::FunCallbacks结构。然后调用hal_px4这个类的run函数。
进而调用hal_px4_class.cpp中的main_loop函数。在这个函数里调用初始化部分调用gps_auto_test.cpp的setup函数,在
while循环中调用gps_auto_test.cpp的loop函数,这样,gps_auto_test.cpp这个测试例子就运行起来了,setup进行初始化。然后循环执行loop函数。我们就可以看到不断的打印gps的信息了。


下面我们再看启动以后,明天继续上传。
阅读(1266) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~