APM添加超声模块及定高程序分析

给飞控添加新的模块，通常的做法是写驱动文件，然后用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;