功能描述:仿照大疆飞机,遥控器开关快速拨动三次触发校准磁罗盘任务
原理: 遥控器配置第六通道,在commander模块添加订阅遥控器通道值根据通道值变化判断进入磁罗盘校准的逻辑
编程逻辑:
修改commander文件,添加遥控器通道订阅,在主函数循环中定义计数位根据开关拨动计数,三秒内计数六次发送校准磁罗盘的指令
在主程序的循环里添加以下代码
//PX4_INFO("_rc_channels.channels[5]:%3.6f\r\n",(double)_rc_channels.channels[5]);
if (_rc_channel_sub.updated()){
_rc_channel_sub.copy(&_rc_channels);
//3秒后重新计数 --我们要求必须在三秒内连续来回拨动3次开关遥杆
if((hrt_absolute_time()/1e6 - time_cla_msg_start) > 3 )
cla_count = 0;
switch (cla_count){
case 0:
if(_rc_channels.channels[5] > 0)
{
cla_count++;
time_cla_msg_start = hrt_absolute_time()/ 1e6;
PX4_INFO("cla_count :%d \r\n",cla_count);
}
break;
case 1:
if(_rc_channels.channels[5] < 0)
cla_count++;
PX4_INFO("cla_count :%d \r\n",cla_count);
break;
case 2:
if(_rc_channels.channels[5] > 0)
cla_count++;
PX4_INFO("cla_count :%d \r\n",cla_count);
break;
case 3:
if(_rc_channels.channels[5] < 0)
cla_count++;
PX4_INFO("cla_count :%d \r\n",cla_count);
break;
case 4:
if(_rc_channels.channels[5] > 0)
cla_count++;
PX4_INFO("cla_count :%d \r\n",cla_count);
break;
case 5:
if(_rc_channels.channels[5] < 0)
{
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f);//发送校准命令
cla_count = 0;
PX4_INFO("cla_count :%d \r\n",cla_count);
}
break;
default:
break;
}
}