< >
Home » Pixhawk源码笔记 » Pixhawk源码笔记-09.添加新的飞行模式

Pixhawk源码笔记-09.添加新的飞行模式

Pixhawk源码笔记-09.添加新的飞行模式

说明:

  • 本教程讲述如何向APM代码中添加新的飞行模式

  • 通过这里我们可以对飞行模式相关的几个文件有一个非常清晰的认识

第十部分 添加新的飞行模式

// Auto Pilot modes    
// ----------------

#define STABILIZE 0 // hold level position    
#define ACRO 1 // rate control    
#define ALT_HOLD 2 // AUTO control    
#define AUTO 3 // AUTO control    
#define GUIDED 4 // AUTO control    
#define LOITER 5 // Hold a single location    
#define RTL 6 // AUTO control    
#define CIRCLE 7 // AUTO control    
#define LAND 9 // AUTO control    
#define OF_LOITER 10 // Hold a single location using optical flow sensor    
#define DRIFT 11 // DRIFT mode (Note: 12 is no longer used)    
#define SPORT 13 // earth frame rate control    
#define FLIP 14 // flip the vehicle on the roll axis    
#define AUTOTUNE 15 // autotune the vehicle's roll and pitch gains    
#define POSHOLD 16 // position hold with manual override    
#define NEWFLIGHTMODE 17               // new flight mode description    
#define NUM_MODES 18
  • Step #2:类似于相似的飞行模式的control_stabilize.pde或者control_loiter.pde文件,创建新的飞行模式的.pde控制sketch文件。该文件中必须包含一个_init()初始化函数和_run()运行函数,类似于static bool althold_init(bool ignore_checks)和static void althold_run()
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// newflightmode_init - initialise flight mode
static bool newflightmode_init(bool ignore_checks)
{
   // do any required initialisation of the flight mode here
   // this code will be called whenever the operator switches into this mode 
   // return true initialisation is successful, false if it fails
   // if false is returned here the vehicle will remain in the previous flight mode
   return true;
}

 

// newflightmode_run - runs the main controller
// will be called at 100hz or more
static void newflightmode_run()
{
   // if not armed or throttle at zero, set throttle to zero and exit immediately
   if(!motors.armed() || g.rc_3.control_in <= 0) {
       attitude_control.relax_bf_rate_controller();
       attitude_control.set_yaw_target_to_current_heading();
       attitude_control.set_throttle_out(0, false);
       return;
   }

   // convert pilot input into desired vehicle angles or rotation rates
   //   g.rc_1.control_in : pilots roll input in the range -4500 ~ 4500
   //   g.rc_2.control_in : pilot pitch input in the range -4500 ~ 4500
   //   g.rc_3.control_in : pilot's throttle input in the range 0 ~ 1000
   //   g.rc_4.control_in : pilot's yaw input in the range -4500 ~ 4500
   // call one of attitude controller's attitude control functions like
   //  attitude_control.angle_ef_roll_pitch_rate_yaw(roll angle, pitch angle, yaw rate);
   // call position controller's z-axis controller or simply pass through throttle
   //  attitude_control.set_throttle_out(desired throttle, true);
}
  • Step #3:在文件flight_mode.pde文件的set_mode()函数中增加一个新飞行模式的case(C++中switch..case语法)选项,然后调用上面的_init()函数
// set_mode - change flight mode and perform any necessary initialisation
static bool set_mode(uint8_t mode)
{
   // boolean to record if flight mode could be set
   bool success = false;
   bool ignore_checks = !motors.armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
   // return immediately if we are already in the desired mode
   if (mode == control_mode) {
       return true;
   }
   switch(mode) {
       case ACRO:
           #if FRAME_CONFIG == HELI_FRAME
               success = heli_acro_init(ignore_checks);
           #else
               success = acro_init(ignore_checks);
           #endif
           break;

       case NEWFLIGHTMODE:
           success = newflightmode_init(ignore_checks);
           break;
   }
}
  • Step #4:在文件flight_mode.pde文件的update_flight_mode()函数中增加一个新飞行模式的case选项,然后调用上面的_run()函数
// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more

static void update_flight_mode()
{

   switch (control_mode) {
       case ACRO:
           #if FRAME_CONFIG == HELI_FRAME
               heli_acro_run();
           #else
               acro_run();
           #endif
           break;

       case NEWFLIGHTMODE:
           success = newflightmode_run();
           break;
   }
}
  • Step #5: 在文件flight_mode.pde文件的print_flight_mode()函数中增加可以输出新飞行模式字符串的case选项
    static void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
    {
       switch (mode) {
       case STABILIZE:
           port->print_P(PSTR("STABILIZE"));
           break;
    
       case NEWFLIGHTMODE:
           port->print_P(PSTR("NEWFLIGHTMODE"));
           break;
  • Step #6:在文件Parameters.pde中向FLTMODE1 ~ FLTMODE6参数中正确的增加你的新飞行模式到@Values列表中
   // @Param: FLTMODE1
   // @DisplayName: Flight Mode 1
   // @Description: Flight mode when Channel 5 pwm is 1230, <= 1360
   // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport,17:NewFlightMode
   // @User: Standard

   GSCALAR(flight_mode1, "FLTMODE1",              FLIGHT_MODE_1),

   // @Param: FLTMODE2
   // @DisplayName: Flight Mode 2
   // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
   // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport,17:NewFlightMode
   // @User: Standard

   GSCALAR(flight_mode2, "FLTMODE2",              FLIGHT_MODE_2),
  • Step #7:如果你想让你的新飞行模式出现的Mission Planner的平视显示器HUD和飞行模式组件中,你需要相应修改Mission Planner代码

  • 关于Mission Planner如何编译的问题,请参考我的另外一篇文章:http://blog.sina.com.cn/s/blog_402c071e0102v4kx.html

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: none