Apm飞控学习笔记之如何添加自己的功能-Cxm
Posted CHENxiaomingming
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了Apm飞控学习笔记之如何添加自己的功能-Cxm相关的知识，希望对你有一定的参考价值。
官方给的参考
Scheduling Code to Run Intermittently — Dev documentation
https://ardupilot.org/dev/docs/code-overview-scheduling-your-new-code-to-run-intermittently.html贴出官方代码
十分简单大概原理就是 我写一个函数 然后添加到这里面 并给它运行速度和截至时间就可以了
/*
scheduler table - all regular tasks apart from the fast_loop()
should be listed here, along with how often they should be called
(in 10ms units) and the maximum time they are expected to take (in
microseconds)
*/
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM =
//第一个参数 update_GPS 函数名 第二个运行HZ, 第三个多如果到了900ms没回应就强行终止
update_GPS, 2, 900 ,
update_nav_mode, 1, 400 ,
medium_loop, 2, 700 ,
update_altitude, 10, 1000 ,
fifty_hz_loop, 2, 950 ,
run_nav_updates, 10, 800 ,
slow_loop, 10, 500 ,
gcs_check_input, 2, 700 ,
gcs_send_heartbeat, 100, 700 ,
gcs_data_stream_send, 2, 1500 ,
gcs_send_deferred, 2, 1200 ,
compass_accumulate, 2, 700 ,
barometer_accumulate, 2, 900 ,
super_slow_loop, 100, 1100 ,
my_new_function, 10, 200 ,
perf_update, 1000, 500
;我这边使用4.07的APM代码进行演示
开发IDE使用的是vscode
在ardupilot\\ArduCopter\\Copter.cpp文件内我们可以看到原生的代码是这样的
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_CLASS(OpticalFlow, &copter.optflow, update, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
#if TOY_MODE_ENABLED == ENABLED
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50),
#endif
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
#if RANGEFINDER_ENABLED == ENABLED
SCHED_TASK(read_rangefinder, 20, 100),
#endif
#if PROXIMITY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50),
#endif
#if BEACON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_VisualOdom, &copter.g2.visual_odom, update, 400, 50),
#endif
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
#if MODE_SMARTRTL_ENABLED == ENABLED
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100),
#endif
#if SPRAYER_ENABLED == ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90),
#endif
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75),
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90),
#if AC_FENCE == ENABLED
SCHED_TASK_CLASS(AC_Fence, &copter.fence, update, 10, 100),
#endif
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(fourhundred_hz_logging,400, 50),
#endif
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(check_vibration, 10, 50),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(standby_update, 100, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550),
#if MOUNT == ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75),
#endif
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update_trigger, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
#if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 40, 200),
#endif
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, 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
#if AC_TERRAIN == ENABLED
SCHED_TASK(terrain_update, 10, 100),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75),
#endif
#if WINCH_ENABLED == ENABLED
SCHED_TASK(winch_update, 10, 50),
#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
#if BUTTON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.g2.button, update, 5, 100),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100),
#endif
#if OSD_ENABLED == ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
;这里面使用了非常多的条件编译,并且格式和官方给出的也有些不一样,但是其实也是差不多的
这边我们先写一个自己的功能函数,简单的让串口持续输出 hahahhaha
这里我们要先看以下apm的串口定义
官方文档里面引导我们去查看
ardupilot\\libraries\\AP_HAL\\examples\\UART_test\\UART_test.cpp文件
我们直接打开 看注释
/*
simple test of UART interfaces
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_OS_POSIX_IO
#include <stdio.h>
#endif
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
/*
setup one UART at 57600
*/
static void setup_uart(AP_HAL::UARTDriver *uart, const char *name)
if (uart == nullptr)
// that UART doesn't exist on this platform
return;
//定义波特率为57600
uart->begin(57600);
void setup(void)
/*
start all UARTs at 57600 with default buffer sizes
*/
hal.scheduler->delay(1000); //Ensure that the uartA can be initialized
//这里可以看见他吧每一个串口的波特率都设置成了 57600
setup_uart(hal.uartA, "uartA"); // console
setup_uart(hal.uartB, "uartB"); // 1st GPS
setup_uart(hal.uartC, "uartC"); // telemetry 1
setup_uart(hal.uartD, "uartD"); // telemetry 2
setup_uart(hal.uartE, "uartE"); // 2nd GPS
static void test_uart(AP_HAL::UARTDriver *uart, const char *name)
if (uart == nullptr)
// that UART doesn't exist on this platform
return;
uart->printf("Hello on UART %s at %.3f seconds\\n",
name, (double)(AP_HAL::millis() * 0.001f));
void loop(void)
test_uart(hal.uartA, "uartA");
test_uart(hal.uartB, "uartB");
test_uart(hal.uartC, "uartC");
test_uart(hal.uartD, "uartD");
test_uart(hal.uartE, "uartE");
// also do a raw printf() on some platforms, which prints to the
// debug console
#if HAL_OS_POSIX_IO
::printf("Hello on debug console at %.3f seconds\\n", (double)(AP_HAL::millis() * 0.001f));
#endif
hal.scheduler->delay(1000);
AP_HAL_MAIN();
从这里面我们可以知道 hal.uartA是那个串口的调用方式 而uart->begin(57600); 是设置波特率
void Copter::Cxm_uart(void)
hal.scheduler->delay(1000); //初始化一下确保可以正确打开串口
hal.uartA->begin(9600);
hal.uartA->printf("****hahha****\\r\\n");
还要在头文件里面声明不要忘记了
ardupilot\\ArduCopter\\Copter.h 中
// ArduCopter.cpp
void fast_loop();
void rc_loop();
void throttle_loop();
void update_batt_compass(void);
void Cxm_uart(void);
void fourhundred_hz_logging();
void ten_hz_logging_loop();
void twentyfive_hz_logging();
void three_hz_loop();
void one_hz_loop();
void update_GPS(void);
void init_simple_bearing();
void update_simple_mode(void);
void update_super_simple_bearing(bool force_update);
void read_AHRS(void);
void update_altitude();然后添
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),
SCHED_TASK(Cxm_uart, 1, 75), //添加到这里
#if OPTFLOW == ENABLED
SCHED_TASK_CLASS(OpticalFlow, &copter.optflow, update, 200, 160),
#endif
SCHED_TASK(update_batt_compass, 10, 120),
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50),
SCHED_TASK(arm_motors_check, 10, 50),
#if TOY_MODE_ENABLED == ENABLED
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50),
#endif
SCHED_TASK(auto_disarm_check, 10, 50),
SCHED_TASK(auto_trim, 10, 75),
#if RANGEFINDER_ENABLED == ENABLED
SCHED_TASK(read_rangefinder, 20, 100),
#endif
#if PROXIMITY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50),
#endif
#if BEACON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_VisualOdom, &copter.g2.visual_odom, update, 400, 50),
#endif
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),
#if MODE_SMARTRTL_ENABLED == ENABLED
SCHED_TASK_CLASS(ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100),
#endif
#if SPRAYER_ENABLED == ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90),
#endif
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75),
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90),
#if AC_FENCE == ENABLED
SCHED_TASK_CLASS(AC_Fence, &copter.fence, update, 10, 100),
#endif
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(fourhundred_hz_logging,400, 50),
#endif
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(check_vibration, 10, 50),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(standby_update, 100, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550),
#if MOUNT == ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75),
#endif
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update_trigger, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50),
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
#if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 40, 200),
#endif
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, 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
#if AC_TERRAIN == ENABLED
SCHED_TASK(terrain_update, 10, 100),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75),
#endif
#if WINCH_ENABLED == ENABLED
SCHED_TASK(winch_update, 10, 50),
#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
#if BUTTON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.g2.button, update, 5, 100),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100),
#endif
#if OSD_ENABLED == ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
;加进我们的数组里面然后 打开终端 ./waf copter 开始编译
编译成功没有问题
接下来是烧录和实验
ardupilot\\build\\fmuv2\\bin\\arducopter.apj 这个文件我们就可以烧录进去
打开地面站 Mission Planner
打开之后更着操作步骤安装就可以了
然后我们打开一个串口调试工具设置好串口和我们设置的波特率9600然后就可以啦
为什么会有乱码那是因为飞控的mavlink协议本身的串口输出 因为波特率不对所以乱码
第一章就到这里啦 喜欢就收藏吧 我会出更多详细的过程
以上是关于Apm飞控学习笔记之如何添加自己的功能-Cxm的主要内容，如果未能解决你的问题，请参考以下文章