pixhawk 整体架构的认识

原文地址：http://blog.csdn.net/czyv587/article/details/51627787

此篇blog的目的是对px4工程有一个整体认识，对各个信号的流向有个了解，以及控制算法采用的控制框架。

PX4自动驾驶仪软件可分为三大部分：实时操作系统、中间件和飞行控制栈。

1.NuttX实时操作系统
提供POSIX-style的用户操作环境(如printf(), pthreads,/dev/ttyS1,open(),write(),poll(),ioctl())，进行底层的任务调度。
2.PX4中间件
PX4中间件运行于操作系统之上，提供设备驱动和一个微对象请求代理（micro object request broker，uORB)用于驾驶仪上运行的单个任务之间的异步通信。Px4被3DR开源后，整个代码结构被⼤改，原先的系统被摒弃，进而采用Nuttx，但是核心思想没变－为了简化开发而采用牺牲部分效率的消息传递机制，这是Px4 与ArduPilot 最本质的差别。
3.PX4飞行控制栈
飞行控制栈可以使用PX4的控制软件栈，也可以使用其他的控制软件，如APM:Plane、APM:Copter，但必须运行于PX4中间件之上。

此部分又可分为

决策导航部分：根据飞行器自身安全状态和接收到的命令，决定工作于什么模式，下一步应该怎么做。

位置姿态估计部分：根据传感器得到自身的位置和姿态信息，此部分算法含金量最高，算法也相当多。

位置姿态控制部分：根据期望位置和姿态设计控制结构，尽可能快、稳的达到期望位置和姿态。

控制器输出部分：mixer和执行器，pwm限幅。

算法在Firmware/src/modules，驱动在Firmware/src/drivers

px4原生固件模块列表：

系统命令程序

mavlink –通过串口发送和接收mavlink信息

sdlog2 –保存系统日志/飞行数据到SD卡

tests –测试系统中的测试程序

top –列出当前的进程和CPU负载

uORB – 微对象请求代理器-分发其他应用程序之间的信息

驱动

mkblctrl–blctrl电子模块驱动

esc_calib –ESC的校准工具

fmu –FMU引脚输入输出定义

gpio_led –GPIO LED驱动

gps –GPS接收器驱动

pwm –PWM的更新速率命令

sensors –传感器应用

px4io –px4io驱动

uavcan –uavcan驱动

飞行控制的程序

飞行安全和导航

commander –主要飞行安全状态机

navigator –任务，失效保护和RTL导航仪

估计姿态和位置

attitude_estimator_ekf –基于EKF的姿态估计

ekf_att_pos_estimator –基于EKF的姿态和位置估计

position_estimator_inav–惯性导航的位置估计

multirotor姿态和位置控制器

mc_att_control–multirotor姿态控制器

mc_pos_control –multirotor位置控制器

fixedwing姿态和位置控制器

fw_att_control –固定翼飞机的姿态控制

fw_pos_control_l1 –固定翼位置控制器

垂直起降姿态控制器

vtol_att_control –垂直起降姿态控制器

3.1决策

3.1.1 任务决策。以任务为导向，任务决策主要决定多旋翼下一步“去哪儿”，进一步，需要规划路径，使得整个过程能满足诸如：飞向航路点并沿航线飞，以及飞向航路点并避障等要求。
3.1.2 健康管理和失效保护。以安全为导向，失效保护主要决定多旋翼下一步“去哪儿”。多旋翼飞行器在飞行前或飞行中，可能会发生通信故障、传感器失效和动力系统异常等，这些意外会直接导致控制任务无法完成。这一部分包括安全问题的介绍、机载设备的健康评估、机载设备的健康监测、失效后的保护建议。

传感器校正在commander里面

对应的程序在Firmware/src/modules/commander和Firmware/src/modules/navigator

完成任务模式切换，同时考虑电池电量、GPS等传感器是否正常工作等信息。

[cpp] view plain copy

print ?

commander
orb_publish(ORB_ID(home_position), homePub, &home);
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(parameter_update), param_changed_sub,&param_update;
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
orb_copy(ORB_ID(safety), safety_sub, &safety);
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

commander

orb_publish(ORB_ID(home_position), homePub, &home);
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);

orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(parameter_update), param_changed_sub,&param_update;
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
orb_copy(ORB_ID(safety), safety_sub, &safety);
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

[cpp] view plain copy

print ?

navigator
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus)
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode)
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

navigator

orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);

orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus)
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) 
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

3.2位置估计和姿态估计

此部分需要大量理论知识，暂时还比较欠缺。

sensors是对陀螺仪、加速度计地磁计的一个处理

[cpp] view plain copy

print ?

sensors
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

sensors

orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);

orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

[cpp] view plain copy

print ?

ardrone_interface
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
batt_smbus
orb_publish(_batt_orb_id, _batt_topic, &new_report);
orb_copy(ORB_ID(battery_status), sub, &status)
bma180
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
GPS
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
hmc5883
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
hott
orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
l3gd20
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
lsm303d
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
mpu6000
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
mpu9250
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
ms5611
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
pwm_out_sim
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
px4flow
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
orb_publish(ORB_ID(subsystem_info), pub, &info);
px4fmu
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
orb_publish(ORB_ID(safety), _to_safety, &safety);
orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
px4io
orb_publish(ORB_ID(vehicle_command), pub, &cmd);
orb_publish(ORB_ID(safety), _to_safety, &safety);
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits);
orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
orb_copy(ORB_ID(actuator_controls_x), _t_actuator_controls_0, &controls);
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
stm32(adc)
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);

ardrone_interface
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);

#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); batt_smbus orb_publish(_batt_orb_id, _batt_topic, &new_report); orb_copy(ORB_ID(battery_status), sub, &status) bma180 orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); GPS orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); hmc5883 orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); hott orb_publish(ORB_ID(esc_status), _esc_pub, &esc); orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); orb_copy(ORB_ID(battery_status), _battery_sub, &battery); orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); orb_copy(ORB_ID(esc_status), _esc_sub, &esc); l3gd20 orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); lsm303d orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); mpu6000 orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); mpu9250 orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); ms5611 orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); pwm_out_sim orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa); px4flow orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); orb_publish(ORB_ID(subsystem_info), pub, &info); px4fmu orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); orb_publish(ORB_ID(safety), _to_safety, &safety); orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in); orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); px4io orb_publish(ORB_ID(vehicle_command), pub, &cmd); orb_publish(ORB_ID(safety), _to_safety, &safety); orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits); orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); orb_copy(ORB_ID(actuator_controls_x), _t_actuator_controls_0, &controls); orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); stm32(adc) orb_publish(ORB_ID(system_power), _to_system_power, &system_power);

3.3位置控制和姿态控制

3.4pwm输出

3.4.1mixer

官方地址(已失效)对应的内容

混合器输出定义控制器的输出是如何映射到电机和伺服输出。所有内置mixer文件位于ROM文件系统的/etc/mixers目录，并编译成固件。Mixer是一组独立的映射器，从控制读入写入，写入执行器输出。一个模块，结合一组根据预先定义的规则和参数的输入，产生一组输出。

(1)语法：
Mixer的定义是文本文件，用一个大写字母后跟一个冒号开头的行。所有其他的行都会被忽略，这意味着解释性文本可与定义相混合。每个文件可以定义多个Mixer。由一个Mixer产生的执行器输出的数量是特定的。
起始形式如下：
<tag>:<mixer arguments>
标签选择Mixer的类型，“M”为一种简单求和Mixer，“R”为多转子（多旋翼）Mixer等。
一个空的Mixer不消耗控制，并产生一个执行机构的输出，其值始终为零。通常一个空的Mixer被用作占位符Mixer的集合中，以实现制动器输出的特定模式。
空的Mixer定义形式：
Z:

(2)简单的Mixer
一个简单的Mixer的开始：
M: <control count>
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>

如果为零，则输出就为零，Mixer输出的固定值是受限制的。
第二行定义了输出输出定标器和标量参数。虽然计算作为浮点运算进行的，存储在所述定义文件中的值是由10000倍缩放，-0.5偏移被编码为-5000.

该定义继续描述控制输入和缩放项：
S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
这些值定义了控制组从那些定标器中读取数据，和该组内的值的偏移。是特定于设备读取Mixer的定义。

当用于混合车辆的控制，混合器组0是车辆姿态控制组，索引值从0到3通常分别是：roll，pitch，yaw，thrust。

具体详解见：https://pixhawk.org/dev/mixing?s[]=mixer

(3)多旋翼Mixer
在多旋翼混合器结合四个控制输入（roll，pitch，yaw，thrust）为一组用于驱动电机速度控制器制动输出。
Mixer定义的形式：
R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
参数解释：
机械：包括quad（4x，4+），hex（6x，6+），octo（8x，8+）
单独的roll，pitch和yaw控制因子
电机输出死区

此篇blog的目的是对px4工程有一个整体认识，对各个信号的流向有个了解，以及控制算法采用的控制框架。

PX4自动驾驶仪软件可分为三大部分：实时操作系统、中间件和飞行控制栈。

此部分又可分为

决策导航部分：根据飞行器自身安全状态和接收到的命令，决定工作于什么模式，下一步应该怎么做。

位置姿态估计部分：根据传感器得到自身的位置和姿态信息，此部分算法含金量最高，算法也相当多。

位置姿态控制部分：根据期望位置和姿态设计控制结构，尽可能快、稳的达到期望位置和姿态。

控制器输出部分：mixer和执行器，pwm限幅。

算法在Firmware/src/modules，驱动在Firmware/src/drivers

px4原生固件模块列表：

系统命令程序

mavlink –通过串口发送和接收mavlink信息

sdlog2 –保存系统日志/飞行数据到SD卡

tests –测试系统中的测试程序

top –列出当前的进程和CPU负载

uORB – 微对象请求代理器-分发其他应用程序之间的信息

驱动

mkblctrl–blctrl电子模块驱动

esc_calib –ESC的校准工具

fmu –FMU引脚输入输出定义

gpio_led –GPIO LED驱动

gps –GPS接收器驱动

pwm –PWM的更新速率命令

sensors –传感器应用

px4io –px4io驱动

uavcan –uavcan驱动

飞行控制的程序

飞行安全和导航

commander –主要飞行安全状态机

navigator –任务，失效保护和RTL导航仪

估计姿态和位置

attitude_estimator_ekf –基于EKF的姿态估计

ekf_att_pos_estimator –基于EKF的姿态和位置估计

position_estimator_inav–惯性导航的位置估计

multirotor姿态和位置控制器

mc_att_control–multirotor姿态控制器

mc_pos_control –multirotor位置控制器

fixedwing姿态和位置控制器

fw_att_control –固定翼飞机的姿态控制

fw_pos_control_l1 –固定翼位置控制器

垂直起降姿态控制器

vtol_att_control –垂直起降姿态控制器

3.1决策

传感器校正在commander里面

对应的程序在Firmware/src/modules/commander和Firmware/src/modules/navigator

完成任务模式切换，同时考虑电池电量、GPS等传感器是否正常工作等信息。

[cpp] view plain copy

print ?

commander
orb_publish(ORB_ID(home_position), homePub, &home);
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(parameter_update), param_changed_sub,&param_update;
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
orb_copy(ORB_ID(safety), safety_sub, &safety);
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

commander

orb_publish(ORB_ID(home_position), homePub, &home);
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);

orb_copy(ORB_ID(vehicle_status), state_sub, &state);
orb_copy(ORB_ID(parameter_update), param_changed_sub,&param_update;
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
orb_copy(ORB_ID(safety), safety_sub, &safety);
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

[cpp] view plain copy

print ?

navigator
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus)
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode)
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

navigator

orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);

orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus)
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) 
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

3.2位置估计和姿态估计

此部分需要大量理论知识，暂时还比较欠缺。

sensors是对陀螺仪、加速度计地磁计的一个处理

[cpp] view plain copy

print ?

sensors
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

sensors

orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);

orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);

[cpp] view plain copy

print ?

ardrone_interface
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
batt_smbus
orb_publish(_batt_orb_id, _batt_topic, &new_report);
orb_copy(ORB_ID(battery_status), sub, &status)
bma180
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
GPS
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
hmc5883
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
hott
orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
l3gd20
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
lsm303d
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
mpu6000
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
mpu9250
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
ms5611
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
pwm_out_sim
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
px4flow
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
orb_publish(ORB_ID(subsystem_info), pub, &info);
px4fmu
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
orb_publish(ORB_ID(safety), _to_safety, &safety);
orb_publish(ORB_ID(input_rc), _to_input_rc, &_rc_in);
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
px4io
orb_publish(ORB_ID(vehicle_command), pub, &cmd);
orb_publish(ORB_ID(safety), _to_safety, &safety);
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits);
orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
orb_copy(ORB_ID(actuator_controls_x), _t_actuator_controls_0, &controls);
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
stm32(adc)
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);

ardrone_interface
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);

pixhawk 整体架构的认识

猜你喜欢