关闭

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

标签: 超声定高
1406人阅读 评论(6) 收藏 举报
分类:
给飞控添加新的模块,通常的做法是写驱动文件,然后用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;



0
0

查看评论
* 以上用户言论只代表其个人观点,不代表CSDN网站的观点或立场
    个人资料
    • 访问:13308次
    • 积分:238
    • 等级:
    • 排名:千里之外
    • 原创:6篇
    • 转载:2篇
    • 译文:2篇
    • 评论:15条
    文章分类
    文章存档
    最新评论