给飞控添加新的模块，通常的做法是写驱动文件，然后用uORB订阅消息，这种方法已经有文章介绍了，下面介绍另一种更加简洁的方法。
硬件连接: UARTD（ ttyS2）。
超声
Boudrate -> 9600，数据格式：R025，三位数字表示距离，单位cm。
飞控程序
UARTD
默认的波特率配置是57600，需要将其修改为9600。
打开HAL_PX4_Class.cpp文件，找到
main_loop函数，在这里
初始化了串口，
将
uartD波特率
改为9600。
static int main_loop(int argc, char **argv)
{
extern void setup(void);
extern void loop(void);
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(9600); //bruce 57600
hal.uartE->begin(57600);
}
实际测试发现只有刚开始一瞬间能读到数据，后面就没有了。原因是程序里还有个地方把波特率又改回默认值了。
打开systerm.pde文件，找到如下代码，将其注释掉即可。
打开systerm.pde文件，找到如下代码，将其注释掉即可。
#if MAVLINK_COMM_NUM_BUFFERS > 2
if (g.serial2_protocol == SERIAL2_FRSKY_DPORT ||
g.serial2_protocol == SERIAL2_FRSKY_SPORT) {
frsky_telemetry.init(hal.uartD, g.serial2_protocol);
} else {
gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128);
}
#endif
读取超声数据：
将该段代码放到GCS_Mavlink.pde文件的
void gcs_check_input(void)函数里，用串口5打印就可以看到数据了。
static uint16_t sonar_data; //bruce
static void gcs_check_input(void)
{
uint8_t single_data = 0;
char buffer[3] = "";
single_data = hal.uartD->read();
if(single_data == 'R'){
for(int num = 0;num <3;++num){
single_data = hal.uartD->read();
buffer[num] = single_data;
single_data = 0;
}
sonar_data = atoi(buffer);
//memset(buffer,'0',sizeof(buffer));
//printf("uart data is %d\n",sonar_data);
}
}
设定目标高度：
_pos_target.z =
_inav.get_altitude();
PID定高：
Control_althold.pde
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
_pos_target.z += climb_rate_cms * dt;
pos_control.update_z_controller();
AC_PosControl.cpp
pos_to_rate_z();
curr_alt = _inav.get_altitude(); //获取当前高度
_pos_error.z = _pos_target.z - curr_alt; //计算高度误差
_vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance)); //计算Z轴目标速度
rate_to_accel_z();
curr_vel = _inav.get_velocity(); //获取当前Z轴速度
_vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z);
//计算Z轴速度误差
accel_to_throttle(float accel_target_z)
_accel_error.z = _accel_error_filter.apply(constrain_float(accel_target_z - z_accel_meas, -32000, 32000));
//计算Z轴加速度误差
PID=(p+i+d); //计算PID值
set_throttle_out();
AP_InertialNav.cpp
check_baro()；
correct_with_baro(_baro.get_altitude()*100.0f, dt);
_position_error.z = baro_alt-baro_alt_ground - (hist_position_base_z + _position_correction.z);
AP_InertialNav::update(float dt)；
_position = _position_base + _position_correction;
_velocity += velocity_increase;
