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

原创 2017年01月03日 16:00:33
给飞控添加新的模块,通常的做法是写驱动文件,然后用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;



版权声明:本文为博主原创文章,未经博主允许不得转载。

apm-rover主程序分析

apm-rover主函数分析
  • lifeng245112
  • lifeng245112
  • 2015年04月12日 20:27
  • 2060

超声波模块HC-SR04调试经验分享

这段时间要用超声波做一个演示实验,就是使用超声波和舵机结合,做一个自动壁障演示实验。 就是将超声波接到舵机上,通过转动舵机来获取各个方向到小车的距离,从而控制小车运动,避开障碍物,并寻找最佳路径。整...
  • liucheng5037
  • liucheng5037
  • 2016年03月25日 23:36
  • 14892

触摸屏驱动程序分析及其在BSP上的添加

注:我的硬件平台是Tiny6410,触摸屏部分把默认的一线触摸改成了标准的四线触摸,硬件上就是把屏幕背面四个零欧电阻R34/R35/R36/R37取了,把取了的零欧电阻焊接到R28/R29/R30/R...
  • bingqingsuimeng
  • bingqingsuimeng
  • 2012年11月27日 14:15
  • 1338

超声模块 C51 程序

  • 2012年09月25日 17:01
  • 1.09MB
  • 下载

错误程序分析

  • 2014年11月11日 16:56
  • 1KB
  • 下载

C++程序分析及运行结果

  • 2013年10月29日 22:04
  • 86KB
  • 下载

Android模仿乐淘的应用程序分析源码

  • 2013年04月10日 13:51
  • 3.21MB
  • 下载

经典C程序分析

  • 2012年09月26日 08:46
  • 84KB
  • 下载

C_复习题——程序分析题问答题

  • 2013年03月27日 16:51
  • 68KB
  • 下载

tiidf算法程序分析

  • 2014年11月22日 22:43
  • 56KB
  • 下载
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:APM添加超声模块及定高程序分析
举报原因:
原因补充:

(最多只允许输入30个字)