commander模式切换

 根深方能叶茂 在等待的日子里，刻苦读书，谦卑做人，养得深根，日后才能枝叶茂盛。 --Better/阿木社区

Commder是Px4源码中的飞行控制命令切换模块，比如由自稳模式切换到，定高，定点，自动模式。所有模式能不能成功切换，都在这个模块中做了相应的飞行条件检查。自稳模式的切换，不需要太多的飞行条件。如果是定高，定点，自动模式，需要很多的传感器数据有效的条件，这就需要很多的检查，只有条件满足才能够成功切换。我们拿光流定点的例子来分析，光流定点都需要哪些传感器的条件有效，光流定点才能成功切换。

一 主体框架

 

模式切换就是MODE_SW ,就是Commander完成的内容，最终能不能成功的切换就看各种模式需要的传感器条件。

二 Commander发布的消息

我们知道分析一个模块有哪些功能，最重要的就是看，这个模块有哪些订阅和发布的消息。一般来讲一个模块订阅的消息(msg)就是这个模块要处理的数据，比如姿态控制模块会定义大量的传感器数据，来反馈控制姿态。一个模块要发布的数据，就是这个模块最终会影响系统的状态。同样Commander发布了vehicle_control_mode这个消息，从字面理解这个消息就是”载具控制模式”就是指飞行器的飞行控制模式消息。通过改变并且发布这个消息成员函数，还切换系统的飞行模式。通过订阅这个消息，让系统知道，目前系统应该处于什么飞行模式，是否需要飞行模式切换。这个消息（vehicle_control_mode)一下的模块订阅:

 

也就是说这个commander发布的vehicle_control_mode消息会影响飞行器的姿态控制和位置控制，比如Mc_att_contrlo_main(多旋翼姿态控制)订阅了这个消息。

我们在看看这个vehicle_control_mode消息有哪些成员。

 

他的成员是很多标志位，比如手动模式使能标志位，自动模式使能标志位，自动模式使能标志位，姿态控制模块订阅了这些消息成员，就知道飞机应该处于什么状态，从而实现了飞机的状态切换。

三 Commander的订阅消息

上面的消息是Commander影响其他模块的消息发布，同时他要订阅vehicle_command消息来处理响应状态切换，比如遥控器发射的飞行状态指令切换，和地面站通过mavlink协议发来的飞行状态指令切换。也就是说遥控器和mavlink协议模块会(因为飞机状态切换，就是靠遥控器和地面站发来的消息指令)发布vehicle_command消息，而Commander会订阅这个消息。

vehicle_command消息成员:

 

有飞机的指令ID，目标系统，指令参数等等.这个Commander进程要不断的去检测这个vehicle_command消息是否有更新，有更新就代表有飞行器的状态切换。

功能简图如下:                                 

vehicle_command(订阅命令切换消息)-----> Commander( (handle_command)函数解析消息，并且做是否允许模式切换检查------>发布vehicle_control_mode消息------>

姿态控制模块订阅vehicle_control_mode------>改变飞行器的模式状态。

 

流程函数很清楚，里面有个比较重要的地方是”否允许模式切换检查”，为什么要检查呢？比如你要切换到无人机的定点飞行模式，定点飞行需要GPS和气压计数据来提供经纬度和高度，这时候就需要飞机的GPS信号和气压计信号非常良好，如果飞机的GPS信号和气压计信号不好，飞机就不能执行定点飞行，当然用光流也可以实现飞机的定点飞行。所以要做是否允许模式切换检查。如果检查不通过，飞机会给地面站发送拒绝执行模式切换的消息，并且保持当前状态不做改变。同样切换到自动飞行，定高飞行，离线模式，都需要做检查。

 

我们在671行可以看到，具体的指令条件检查函数。我们拿定点功能切换为例，具体分析飞控是怎么做检查的。

四 定点模式切换分析

 

main_state_transition这个函数主要是做状态判断的函数

 

 

这个就是具体的判断，可以注意到status_flags 状态标志位

// This is a struct used by the commander internally.

struct status_flags_s {

bool condition_calibration_enabled;

bool condition_system_sensors_initialized;

bool condition_system_prearm_error_reported; // true if errors have already been reported

bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over

bool condition_system_returned_to_home;

bool condition_auto_mission_available;

bool condition_global_position_valid; // set to true by the commander app if the quality of t               

     bool condition_home_position_valid; //

bool condition_local_position_valid;

bool condition_local_altitude_valid;

bool condition_airspeed_valid;

bool usb_connected; // status of the USB power supply

bool circuit_breaker_engaged_power_check;

bool circuit_breaker_engaged_airspd_check;

bool circuit_breaker_engaged_enginefailure_check;

bool circuit_breaker_engaged_gpsfailure_check;

bool circuit_breaker_flight_termination_disabled;

bool circuit_breaker_engaged_usb_check;

bool offboard_control_signal_found_once;

bool offboard_control_signal_lost;

bool offboard_control_signal_weak;

bool offboard_control_set_by_command;

bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time

bool rc_signal_found_once;

bool rc_signal_lost_cmd; // true if RC lost mode is commanded

bool rc_input_blocked; // set if RC input should be ignored temporarily

bool data_link_lost_cmd; // datalink to GCS lost mode commanded

bool vtol_transition_failure; // Set to true if vtol transition failed

bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded

bool gps_failure; // Set to true if a gps failure is detected

bool gps_failure_cmd; // Set to true if a gps failure mode is commanded

bool barometer_failure; // Set to true if a barometer failure is detected

};

这个结构体有大量的标志位，来记录飞控的状态，比如condition_local_position_valid;

这个布尔类型的，就是判断本地位置估计是否有效，bool condition_home_position_valid;

这个布尔类型是表示家的位置是否有效。

case commander_state_s::MAIN_STATE_POSCTL:

/* need at minimum local position estimate */

if (status_flags->condition_local_position_valid ||

     status_flags->condition_global_position_valid)

{

ret = TRANSITION_CHANGED;

}

break;

这句的判断就是，决定着是否可以切换进入光流定点模式。如果大家发现，飞控拒绝进入光流定点模式。那么就跟踪一下status_flags->condition_local_position_valid或者status_flags->condition_global_position_valid这个标志位是否被打开。

在commander_thread_main(int argc, char *argv[])这个主线程中，订阅了很多消息，比如本地位置消息等等。

 

    orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);这个就是订阅了姿态数据，位置数据里面有水平数据因子和垂直数据因子，这几个因子要小于某个阈值，condition_local_position_valid才会有效。注意在GPS模式下，如果GPS数据比较好  ，这个condition_global_position_valid标志位是自动打开的。那么光流的数据会直接影响到 ondition_local_position_valid。我们去分析本地位置估计里面的代码，也就是分析这个vehicle_local_position消息发布的地方。如果光流的数据不好，或者相应的地面站参数设置不对，都会导致位置数据里面有水平数据因子和垂直数据因子不好。这部分不好的话，就有可能涉及到修改光流固件的问题，至于光流的算法，在图像识别中，也是相当成熟了。具体可以先看看图像识别opencv里面的光流部分算法，在结合光流固件的源码，来修改优化光流模块的参数。

从我们接触的光流模块的数据来看，官方的光流固件比较不可用，不稳定。调的稳定，还是有自己修改代码。