1. 停机巡检lua
https://download.csdn.net/download/weixin_43321489/88647127
2. 不停机巡检
2.1 在AP_RTC.cpp中增加一个函数
在AP_RTC.h中声明：
bool get_utc_usec32(float &usec,uint32_t &usec_h,uint32_t &usec_l) const;
在AP_RTC.cpp中定义：
bool AP_RTC::get_utc_usec32(float &usec,uint32_t &usec_h,uint32_t &usec_l) const
{
if (rtc_source_type == SOURCE_NONE) {
return false;
}
uint64_t usec64 = AP_HAL::micros64() + rtc_shift;
usec=(float)usec64;
usec_h = uint32_t (usec64/1000000);
usec_l = uint32_t (usec64-usec_h*1000000);
return true;
}
2.2 增加一个lua绑定
在bindings.desc中添加：
singleton AP_RTC method get_utc_usec32 boolean float'Null uint32_t'Null uint32_t'Null
2.3 lua脚本实现风机不停机巡检
- 脚本1，用于模拟雷达检测到的风叶数据：
local function get_time_str()
local time_curr = uint32_t(millis()/1000)
local gps_week_sec = uint32_t(gps:time_week_ms(0)/1000)
local year,mont,day,hour,min,sec,wee=rtc:clock_s_to_date_fields(gps_week_sec)
local timeStr = string.format("time:%d-%d-%d-%d-%d-%d ", year,mont,day,hour,min,sec)
local timeStr2 = string.format("time: %d-%d-%d-%d-%d-%d-%d ", year,mont,day,hour,min,sec,wee)
return timeStr
end
function update()
local HD = Parameter()
HD:init('LUAA_HD')
if HD:get()==0 then
if param:set_and_save('LUAA_HD',1) then
--gcs:send_text(0,get_time_str()..string.format(" set success:HD is %d ",HD:get()))
return update,3000 --1保持的时间
end
else
if param:set_and_save('LUAA_HD',0) then
--gcs:send_text(0,get_time_str()..string.format(" set success:HD is %d ",HD:get()))
return update,10000 --0保持的时间
end
end
end
return update, 10000
- 脚本2，用于实现风机检测算法：
-- global para
--雷达
local rotation_downward = 25
local rotation_forward = 0
local dist_min = 5 --飞机离桨叶的最小距离 m
local dist_max = 40 --飞机离桨叶的最大距离 m
--桨叶角速度
local detectNextTime_ms = 0 --检测到下个桨叶的平均时间
local lastDetectTime_ms = 0 --上次检测到桨叶的时间 ms
local detecTimeRange_ms = 5000 --检测到同一片桨叶的持续时间
--核心算法
local count = 0
local finished = 0 --结束标志
local blade_no = 0 --桨叶序号
local blade_no_last = 0 --上个桨叶序号
local detectPointNum = 3 --检测位置数，排除轮毂中心（2个，一前一后）、过渡点（两个）以外的点
local indexMax = 2 + 2*detectPointNum + 2 --航点个数：轮毂中心点 + 检测位置数 + 过渡点
local transitPos1_idx = detectPointNum+2 --过渡点1的下标
local transitPos2_idx = detectPointNum+3 --过渡点2的下标
local index = 1
local detectPointInfo = {}
local terminal_offset_left = 3.0 --左端点偏置 m
local terminal_offset_right = 3.0 --又端点偏置 m
--过程参数
local arrTime_ms = 0 --到达航点的时间 ms
local arrived = false
--风机参数
local bladeLen = 35 --桨叶长度 m
local hubAlt = 50 --桨毂安装高度 m
local distToHub_measr = 28 --飞机到桨毂的人工测量距离 m
local distToHub_dect = 0 --飞机到桨毂的雷达检测距离 m
--计算航向角
local distToHub_dect1 = 0
local distToHub_dect2 = 0
local bladeYaw_deg = 0
--阶段参数
local stage1_flag = 0
local yaw_stage1_rad = 0
local p1_stage1_ned = Vector3f() --m
local p2_stage1_ned = Vector3f() --m
local stage2_flag = 0
local loc0_neu_m = Vector3f()
--log
local file_name = "luaLogs.txt"
local file
-- create and initialise parameters
local PARAM_TABLE_KEY = 0 --0~200
assert(param:add_table(PARAM_TABLE_KEY, "LUAA_", 10), 'could not add param table') -- WPCRT: way points create
assert(param:add_param(PARAM_TABLE_KEY, 1, 'HD', 0), 'could not add param DIST_HUB')
-- bind parameters to variables
local HD = Parameter()
HD:init('LUAA_HD')
--mode
local copter_guided_mode_num = 4
local copter_land_mode_num = 9
-- function def
-- 两个FRU坐标系的z轴旋转关系，绕正z轴旋转angle
local function rotateZ(x,y,z,angle_rad)
local rot_x,rot_y,rot_z
rot_x = x * math.cos(angle_rad) - y * math.sin(angle_rad)
rot_y = x * math.sin(angle_rad) + y * math.cos(angle_rad)
rot_z = z
return rot_x,rot_y,rot_z
end
-- 两个FRU坐标系的y轴旋转关系
local function rotateY(x,y,z,angle_rad)
local rot_x,rot_y,rot_z
rot_x = x * math.cos(angle_rad) + z * math.sin(angle_rad)
rot_y = y
rot_z = -x * math.sin(angle_rad) + z * math.cos(angle_rad)
return rot_x,rot_y,rot_z
end
-- 两个FRU坐标系的x轴旋转关系
local function rotateX(x,y,z,angle_rad)
local rot_x,rot_y,rot_z
rot_x = x
rot_y = -z * math.sin(angle_rad) + y * math.cos(angle_rad)
rot_z = z * math.cos(angle_rad) + y * math.sin(angle_rad)
return rot_x,rot_y,rot_z
end
-- frame A to Frame B，A and B are all FRU Frame
-- input para
-- xa0_b,ya0_b,za0_b：A坐标系原点在坐标系B中的坐标值
-- angleX,angleY,angleZ 是绕xyz轴旋转的角度，旋转顺序z->y->x，单位deg
--frame B 是frame经过angleX,angleY,angleZ旋转后的坐标系
local function frameAToframeB(xa,ya,za,xa0_b,ya0_b,za0_b,angleX_rad,angleY_rad,angleZ_rad)
local Xb,Yb,Zb
Xb,Yb,Zb=rotateZ(xa,ya,za,angleZ_rad)
Xb,Yb,Zb=rotateY(Xb,Yb,Zb,angleY_rad)
Xb,Yb,Zb=rotateX(Xb,Yb,Zb,angleX_rad)
return Xb+xa0_b,Yb+ya0_b,Zb+za0_b
end
local function get_time_str()
local time_curr = uint32_t(millis()/1000)
local gps_week_sec = uint32_t(gps:time_week_ms(0)/1000)
local year,mont,day,hour,min,sec,wee=rtc:clock_s_to_date_fields(gps_week_sec)
local timeStr = string.format("time:%d-%d-%d-%d-%d-%d ", year,mont,day,hour,min,sec)
local timeStr2 = string.format("time: %d-%d-%d-%d-%d-%d-%d ", year,mont,day,hour,min,sec,wee)
return timeStr
end
local function computeBladeYaw()
local tan_theta = (0.01*distToHub_dect2-0.01*distToHub_dect2)/10
local theta_rad = math.atan(math.abs(0.01*distToHub_dect2-0.01*distToHub_dect2),10) --绝对值
if distToHub_dect2>distToHub_dect1 then --飞机航向与桨叶程锐角
bladeYaw_deg = math.deg(yaw_stage1_rad) + 90 - math.deg(theta_rad)
else --飞机航向与桨叶程钝角
bladeYaw_deg = math.deg(yaw_stage1_rad) + 90 + math.deg(theta_rad)
end
gcs:send_text(0, string.format("bladeYaw detect value is %f deg, predict value is %f deg", bladeYaw_deg,math.deg(yaw_stage1_rad)+90))
return true
end
local function countCurrBladeNo()
--判断blade_no_last合理性
while blade_no_last>3 do
blade_no_last=blade_no_last-3
end
blade_no=blade_no_last+1
if blade_no>3 then
blade_no=1
end
blade_no_last=blade_no
count=count+1
gcs:send_text(0, get_time_str()..string.format("blade_no %d", blade_no))
end
local function write_to_file(str)
if not file then
error("Could not open file")
end
-- write data
-- separate with comas and add a carriage return
file:write(str.."\n")
-- make sure file is upto date
file:flush()
end
local function get_has_data()
local hd = uint32_t(HD:get()):toint()
if hd==1 then
return true
else
return false
end
end
--根据桨叶角速率和实际检测时间来判断当前检测到的桨叶序号
local function fusion(now_ms)
local t0=math.abs(now_ms-lastDetectTime_ms-detectNextTime_ms) --blade_no_last=blade_no_last
local t1=math.abs(now_ms-lastDetectTime_ms-2*detectNextTime_ms) --blade_no_last=blade_no_last+1
local t2=math.abs(now_ms-lastDetectTime_ms-3*detectNextTime_ms) --blade_no_last=blade_no_last+2
local t3=math.abs(now_ms-lastDetectTime_ms-4*detectNextTime_ms) --blade_no_last=blade_no_last+3
if t0<t1 then
if t2<t3 then
if t0<t2 then
blade_no_last=blade_no_last
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/2 --更新桨叶角速度
else
blade_no_last=blade_no_last+2
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+2) --更新桨叶角速度
end
else
if t0<t3 then
blade_no_last=blade_no_last
else
blade_no_last=blade_no_last+3
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+3) --更新桨叶角速度
end
end
else
if t2<t3 then
if t1<t2 then
blade_no_last=blade_no_last+1
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+1) --更新桨叶角速度
else
blade_no_last=blade_no_last+2
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+2) --更新桨叶角速度
end
else
if t1<t3 then
blade_no_last=blade_no_last+1
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+1) --更新桨叶角速度
else
blade_no_last=blade_no_last+3
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/(2+3) --更新桨叶角速度
end
end
end
end
local function predictBladeNo(now_ms)
if (now_ms-lastDetectTime_ms)<detectNextTime_ms then --比detectNextTime_ms预测的要早检测到桨叶经过
countCurrBladeNo()
detectNextTime_ms = (detectNextTime_ms+(now_ms-lastDetectTime_ms))/2 --更新桨叶角速度
lastDetectTime_ms=now_ms
else
fusion(now_ms)
countCurrBladeNo()
lastDetectTime_ms=now_ms
end
end
local function detecting(posReady)
local now_ms = millis():toint()
--获取雷达数据
local has_data = get_has_data()
local obj_dist_cm = 3200
--没检测到任何物体直接返回false
if not has_data then
return false
end
if has_data and obj_dist_cm < dist_min*100 or obj_dist_cm > dist_max*100 then --不在检测量程内
return false
end
if math.abs(obj_dist_cm-100*distToHub_dect) >250 then --距离与预期的不符
return false
end
if lastDetectTime_ms>0 and (now_ms-lastDetectTime_ms)<detecTimeRange_ms then
return false
end
----------------------------------到达这里表示“可能”检测到了桨叶-------------------------------------
--1)起始点、结束点、过渡点，直接返回false
if index==1 or index==indexMax or index==transitPos1_idx or index==transitPos2_idx then
return false
--2)index==2，lastDetectTime_ms还是0，位置就绪才能开始检测，不然容易检测到轮毂导致误判
elseif index==2 then
if posReady then --位置就绪才开启检测
if lastDetectTime_ms==0 and detectPointInfo[index].TAKEPICFLAG == 1 then --在第一个航点第一次检测到桨叶!!
blade_no=0
countCurrBladeNo()
lastDetectTime_ms=now_ms
return true
else
predictBladeNo(now_ms)
return true
end
end
--2)需要使用detectNextTime_ms去估算当前经过的是几号桨叶！！
elseif index==transitPos2_idx+1 then
if detectPointInfo[index].TAKEPICFLAG == 1 then
if posReady then --TAKEPICFLAG == 1要位置就绪才开启检测，TAKEPICFLAG==其他的不用
predictBladeNo(now_ms)
return true
end
else
predictBladeNo(now_ms)
return true
end
else
predictBladeNo(now_ms)
return true
end
return false
end
-----------------------------------------------------------------------------------------------
function update()
--总开关
local rc_switch = rc:find_channel_for_option(300)
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,5000
end
--判断任务是否结束
if index>indexMax then
finished=1
gcs:send_text(0, 'finished!!')
--返航或降落
local copter_land_mode_num = 9
vehicle:set_mode(copter_land_mode_num)
gcs:send_text(0, 'Landing!! runing sleep after 5s')
return sleep,5000
end
--判断当前拍照位置是否就绪
local curr_loc = ahrs:get_location()
local curr_loc_neu_cm = curr_loc.get_vector_from_origin_NEU(curr_loc) -- return Vector3f dist from home(origin) in cm
local isflying = vehicle:get_likely_flying()--不太可靠
if not( math.abs(curr_loc_neu_cm:x()-100*detectPointInfo[index].NED:x())<100 --位置没就绪：先检测桨叶，再控制飞机飞行到指定航点
and math.abs(curr_loc_neu_cm:y()-100*detectPointInfo[index].NED:y())<100
and math.abs(curr_loc_neu_cm:z()+100*detectPointInfo[index].NED:z())<100 ) then
detecting(false)--检测桨叶
--控制飞行
if detectPointInfo[index].TASKFLAG==0 then
local sent_target = vehicle:set_target_pos_NED(detectPointInfo[index].NED,true,detectPointInfo[index].YAW,false,0,false,false)
if sent_target then
detectPointInfo[index].TASKFLAG=1
arrived=true
else
detectPointInfo[index].TASKFLAG=0
arrived=true
end
end
else --位置已就绪：检测桨叶，触发任务
--检测桨叶
local bladePassBy = detecting(true)
if arrived then --只会在第一次到达航点时执行
arrTime_ms = millis():toint()
arrived=false
detectPointInfo[index].TASKFLAG=2
end
--判断该节点的任务是否完成
if detectPointInfo[index].TASKFLAG==3 then
index=index+1
elseif detectPointInfo[index].TASKFLAG==2 then --未完成：继续执行任务
if detectPointInfo[index].TASK=="takepic" then
if bladePassBy then
--判断当前桨叶号并拍照
if detectPointInfo[index].TAKEPICFLAG == blade_no then
camera:take_picture(0)
--local utc_f,utc_h,utc_l = rtc:get_utc_usec32()
--local utcStr = string.format("%d-%d", utc_h:toint(),utc_l:toint())
--write_to_file(utcStr..": "..string.format("%d", blade_no))
write_to_file(get_time_str()..": "..string.format("%d", blade_no))
detectPointInfo[index].TAKEPICFLAG=detectPointInfo[index].TAKEPICFLAG+1
gcs:send_text(0, get_time_str()..string.format("take pic !!! blade_no is %d, index is %d, count is %d", blade_no,index,count))
end
--判断任务是否完成
if detectPointInfo[index].TAKEPICFLAG>3 then
detectPointInfo[index].TASKFLAG=3
end
end
elseif detectPointInfo[index].TASK=="hover5s" then
if millis():toint()-arrTime_ms>5000 then --判断悬停时间是否已经满足
detectPointInfo[index].TASKFLAG=3
end
elseif detectPointInfo[index].TASK=="none" then
--do nothing
detectPointInfo[index].TASKFLAG=3
end
end
end
return update,100
end
function dataPrepare()
--总开关
local rc_switch = rc:find_channel_for_option(300)
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,2000
end
local home = ahrs:get_home()
local base_loc = ahrs:get_location() --后续所有的航点都是基于该位置的经纬度、高度 根据offset(offset_north,offset_east)函数计算得到！！
local vehicleYaw_rad = ahrs:get_yaw() -- 飞机垂直指向桨叶平面
local normal_yaw_deg = bladeYaw_deg-90 --垂直桨叶平面的yaw值
if home and base_loc and normal_yaw_deg then
-- 定义两个关键的坐标系FRU：
-- 将飞机悬停在距离轮毂中心 distToHub_measr 处，并且航向垂直指向风叶平面
-- 坐标系s： 以此时的飞机重心位置为原点，建立NEU坐标系，此坐标系静止不动，s即static
-- 坐标系h： 以轮毂中心作为原点，x垂直指向桨叶平面向前为正，y水平向右，z轴竖直向上，h即hub
-- 坐标系h原点在坐标系s中的坐标
local angleHxfromSx_cw_rad = math.rad(normal_yaw_deg) --坐标系h的x轴顺时针距离坐标系s的x轴的角度，当bladeYaw_deg无法获取时候可以用vehicleYaw_rad近似代替
local xh0_s=distToHub_dect*math.cos(angleHxfromSx_cw_rad)
local yh0_s=distToHub_dect*math.sin(angleHxfromSx_cw_rad)
local zh0_s=0.0 -- 飞机正对轮毂中心，因此zh0_s是0
local angleZ_HtoS = math.rad(normal_yaw_deg) --坐标系h绕z轴旋转到系s的角度，当bladeYaw_deg无法获取时候可以用vehicleYaw_rad近似代替
local delta = bladeLen/(detectPointNum-1)
local base_loc_neu_cm = base_loc.get_vector_from_origin_NEU(base_loc) -- return Vector3f dist from home(origin) in cm
local loc_i
local yaw_i
local task = "none"
local xs,ys,zs; --航迹点在坐标系s中的位置值
local xh,yh,zh; --航迹点在坐标系h中的位置值
local loc_ned_i
for i = 1, indexMax do
if i==1 then --起始点，悬停
task="hover5s"
xh=-distToHub_dect; yh=0; zh=0 --航迹点在坐标系h中的位置值
yaw_i=normal_yaw_deg
elseif i==transitPos1_idx then --过渡点1
task="none"
xh= - distToHub_dect; yh=bladeLen+10; zh=0
yaw_i=normal_yaw_deg-45;
elseif i==transitPos2_idx then --过渡点2
task="none"
xh=distToHub_dect; yh=bladeLen+10; zh=0
yaw_i=normal_yaw_deg+180+45;
elseif i<transitPos1_idx then --前平面检测点
task="takepic"
xh=-distToHub_dect; yh=(i-2)*delta; zh=0
yaw_i=normal_yaw_deg;
elseif i>transitPos2_idx and i<indexMax then --后平面检测点
task="takepic"
xh=distToHub_dect; yh=bladeLen-(i-transitPos2_idx-1)*delta; zh=0
yaw_i=normal_yaw_deg+180;
elseif i==indexMax then --结束点
task="hover5s"
xh=distToHub_dect; yh=0; zh=0
yaw_i=normal_yaw_deg+180;
end
--处理端点
if i==2 or i==indexMax-1 then
yh=yh + terminal_offset_right --向右移动2m
end
if i==detectPointNum+1 or i==detectPointNum+4 then
yh=yh - terminal_offset_left --向左移动2m
end
-- 航向合理化 -360~360 deg
while yaw_i>360 or yaw_i<-360 do
if yaw_i>360 then
yaw_i=yaw_i-360
end
if yaw_i<-360 then
yaw_i=yaw_i+360
end
end
xs,ys,zs=frameAToframeB(xh,yh,zh,xh0_s,yh0_s,zh0_s,0,0,angleZ_HtoS)-- 将航点坐标转换到坐标系s
--生成导航坐标系下的航点
loc_i = base_loc:copy()
loc_i:offset(xs,ys) --N，E 偏置
loc_i:alt(loc_i:alt()+zs*100) --高度
loc_ned_i = base_loc_neu_cm:copy()
loc_ned_i:x(0.01*loc_ned_i:x()+xs)
loc_ned_i:y(0.01*loc_ned_i:y()+ys)
loc_ned_i:z(-0.01*loc_ned_i:z()-zs) --zs是neu坐标系下的值，所以要加负号
detectPointInfo[i]={}
detectPointInfo[i]={
LOC=loc_i:copy(),--不用copy会被覆盖
NED=loc_ned_i:copy(),--不用copy会被覆盖
YAW=yaw_i,
TASK=task,
TASKFLAG=0, --0初始，1正在飞往航点，2正在执行任务，3任务已完成
TAKEPICFLAG=1 --1号桨叶待拍，2号待拍，3号待拍
}
end
gcs:send_text(0, "update is running !!")
write_to_file(get_time_str().."update is running !!")
index=1
finished=0
lastDetectTime_ms=0
arrived=true
return update,2000
else
gcs:send_text(0, "waiting home and position and yaw !!")
return dataPrepare,2000
end
end
--判断是否对准轮毂,测量飞机到轮毂中心的距离
function stage3_task()
--总开关
local rc_switch = rc:find_channel_for_option(300)
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,2000
end
--等待手动调整飞机位置以让相机对准轮毂中心，航向不用动
local rc_switch2 = rc:find_channel_for_option(301)
local sw2_pos = rc_switch2:get_aux_switch_pos() -- 0,1,2 低位中位高位
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if sw2_pos < 2 then
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
gcs:send_text(0, string.format("obj dist is %f m, please press Scripting2 button to start task", 0.01*obj_dist_cm))
return stage3_task,2000
else
gcs:send_text(0, "雷达未检测到数据!请手动将飞机对准到轮毂中心,并且保证雷达能检测到轮毂中心!")
return stage3_task,2000
end
end
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
distToHub_dect = 0.01*obj_dist_cm
gcs:send_text(0, string.format("distToHub_dect is %f m", distToHub_dect))
write_to_file(get_time_str()..string.format("distToHub_dect is %f m", distToHub_dect))
gcs:send_text(0, "dataPrepare is running ")
write_to_file(get_time_str().."dataPrepare is running ")
return dataPrepare,1000
else
gcs:send_text(0, "雷达未检测到数据!")
return stage3_task,2000
end
end
--计算风机角速率
function stage2_task()
--总开关
local rc_switch = rc:find_channel_for_option(300)
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,2000
end
if stage2_flag == 0 then
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
lastDetectTime_ms = millis():toint()
gcs:send_text(0, string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
write_to_file(get_time_str()..string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
stage2_flag=1
return stage2_task,2000
end
elseif stage2_flag == 1 then
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
local now_ms = millis():toint()
if now_ms-lastDetectTime_ms>detecTimeRange_ms then
gcs:send_text(0, string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
write_to_file(get_time_str()..string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
detectNextTime_ms=now_ms-lastDetectTime_ms
lastDetectTime_ms=now_ms
stage2_flag=2
return stage2_task,2000
end
end
elseif stage2_flag == 2 then
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
local now_ms = millis():tofloat()
if now_ms-lastDetectTime_ms>detecTimeRange_ms then
gcs:send_text(0, string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
write_to_file(get_time_str()..string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
detectNextTime_ms=((now_ms-lastDetectTime_ms)+detectNextTime_ms)/2
lastDetectTime_ms=now_ms
stage2_flag=3
return stage2_task,2000
end
end
elseif stage2_flag == 3 then
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
local now_ms = millis():toint()
if now_ms-lastDetectTime_ms>detecTimeRange_ms then
gcs:send_text(0, string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
write_to_file(get_time_str()..string.format("obj_dist_m is %f m, stage2_flag is %d ", 0.01*obj_dist_cm,stage2_flag))
detectNextTime_ms=((now_ms-lastDetectTime_ms)+detectNextTime_ms)/2
detectNextTime_ms=uint32_t(detectNextTime_ms):toint()
lastDetectTime_ms=0
stage2_flag=4
gcs:send_text(0, string.format("detectNextTime_ms is %d ms", detectNextTime_ms))
write_to_file(get_time_str()..string.format("detectNextTime_ms is %d ms", detectNextTime_ms))
--命令飞机飞到初始位置,航向垂直指向桨叶平面
loc0_neu_m:z(-loc0_neu_m:z())
local sent_target = vehicle:set_target_pos_NED(loc0_neu_m,true,bladeYaw_deg-90,false,0,false,false)
return stage2_task,5000
end
end
elseif stage2_flag == 4 then
--判断是否关闭Scripting2功能
local rc_switch = rc:find_channel_for_option(301)
if not rc_switch then
gcs:send_text(0, 'No any Rc channel for Scripting2 !!')
return stage2_task,5000
end
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos > 0 then
gcs:send_text(0, '请先关闭Scripting2')
return stage2_task,2000
end
gcs:send_text(0, "请手动将飞机对准到轮毂中心,然后开启 Scripting2 button ")
--执行stage3_task
gcs:send_text(0, "stage3 task run!!")
write_to_file(get_time_str().."请手动将飞机对准到轮毂中心,然后开启 Scripting2 button \n"..get_time_str().."stage3 task run!!")
return stage3_task,3000
end
return stage2_task,100
end
--控制飞机飞行到p1p2，检测桨叶距离，计算桨叶航向角
function stage1_task()
--总开关
local rc_switch = rc:find_channel_for_option(300)
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,2000
end
if stage1_flag ==0 then
local mode = vehicle:get_mode()
if not (mode == copter_guided_mode_num) then
local setmode=vehicle:set_mode(copter_guided_mode_num)
if not setmode then
gcs:send_text(0, 'stage1_task set_mode fail!! run stage1_task after 5s')
return stage1_task,5000
end
else
gcs:send_text(0, 'stage1_task get_mode fail!! run stage1_task after 5s')
return stage1_task,5000
end
--命令飞机飞到第1个航点p1
local sent_target = vehicle:set_target_pos_NED(p1_stage1_ned,true,math.deg(yaw_stage1_rad),false,0,false,false)
if sent_target then
stage1_flag=1
else
gcs:send_text(0, 'set_target p1 fail!!')
stage1_flag=0
return stage1_task,5000
end
elseif stage1_flag==1 then
-- 判断位置是否就绪
local curr_loc = ahrs:get_location()
local curr_loc_neu = curr_loc.get_vector_from_origin_NEU(curr_loc) --返回的是cm
if math.abs(curr_loc_neu:x()-100*p1_stage1_ned:x())<100
and math.abs(curr_loc_neu:y()-100*p1_stage1_ned:y())<100
and math.abs(curr_loc_neu:z()+100*p1_stage1_ned:z())<100 then -- 一个是ned一个是neu所以是+
--位置就绪后设置飞机为位置保持????
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
distToHub_dect1 = obj_dist_cm*0.01
gcs:send_text(0, string.format("distToHub_dect1 is %f m", distToHub_dect1))
write_to_file(get_time_str()..string.format("distToHub_dect1 is %f m", distToHub_dect1))
--命令飞机飞到第2个航点p2
local sent_target = vehicle:set_target_pos_NED(p2_stage1_ned,true,math.deg(yaw_stage1_rad),false,0,false,false)
if sent_target then
stage1_flag=2
return stage1_task,5000
else
gcs:send_text(0, 'set_target p2 fail!!')
stage1_flag=0
return stage1_task,5000
end
end
else
return stage1_task,3000
end
elseif stage1_flag==2 then
-- 判断位置是否就绪
local curr_loc = ahrs:get_location()
local curr_loc_neu = curr_loc.get_vector_from_origin_NEU(curr_loc) --返回的是cm
if math.abs(curr_loc_neu:x()-100*p2_stage1_ned:x())<100
and math.abs(curr_loc_neu:y()-100*p2_stage1_ned:y())<100
and math.abs(curr_loc_neu:z()+100*p2_stage1_ned:z())<100 then -- 一个是ned一个是neu所以是+
--检测桨叶距离
local has_data = get_has_data()
local obj_dist_cm = 3200
if has_data and obj_dist_cm > dist_min*100 and obj_dist_cm < dist_max*100 then --检测到物体经过
distToHub_dect2 = obj_dist_cm*0.01
gcs:send_text(0, string.format("distToHub_dect2 is %f m", distToHub_dect2))
write_to_file(get_time_str()..string.format("distToHub_dect2 is %f m", distToHub_dect2))
stage1_flag=3
--根据得到的 distToHub_dect1，distToHub_dect2 计算桨叶航向角
if computeBladeYaw() then
write_to_file(get_time_str()..string.format("bladeYaw detect value is %f deg, predict value is %f deg", bladeYaw_deg,math.deg(yaw_stage1_rad)+90))
--将飞机航向控制到正对桨叶
vehicle:set_target_pos_NED(p2_stage1_ned,true,bladeYaw_deg-90, false,0,false,false)
--在该航点处执行stage2_task
gcs:send_text(0, "stage2 task run!!")
write_to_file(get_time_str().."stage2 task run!!")
stage2_flag=0
return stage2_task,3000
else
stage1_flag=0
gcs:send_text(0, "computeBladeYaw fail!!")
return stage1_task,5000
end
end
else
return stage1_task,3000
end
end
return stage1_task,100
end
--待机
function sleep()
--总开关
local rc_switch = rc:find_channel_for_option(300)
if not rc_switch then
return sleep,3000
end
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
return sleep,3000
end
gcs:send_text(0, 'setup is running !!')
return setup,2000
end
-- 判断高度，规划两个航点p1p2用于计算桨叶航向角
function setup()
--总开关
local rc_switch = rc:find_channel_for_option(300)
if not rc_switch then
gcs:send_text(0, ' No anny Rc channels for Scripting1 !!')
return sleep,1000
end
local sw_pos = rc_switch:get_aux_switch_pos() -- 0,1,2 低位中位高位
if sw_pos < 2 then
gcs:send_text(0, 'Scripting1 button is off!!')
return sleep,1000
end
--log
-- make a file
file = io.open(file_name, "a")
if not file then
error("Could not make file")
end
-- write the txt header
file:write(get_time_str().."\n")
file:flush()
local home = ahrs:get_home()
local curr_loc = ahrs:get_location()
local alt_rel_curr =curr_loc:alt()-home:alt() --cm
if home and curr_loc and math.abs(alt_rel_curr-hubAlt*100)<150 then
local sensor_count = 1
gcs:send_text(0, string.format("%d rangefinder sensors found.", sensor_count))
write_to_file(get_time_str()..string.format("%d rangefinder sensors found.", sensor_count))
if not sensor_count then
return
end
--记录当前航向角
yaw_stage1_rad = ahrs:get_yaw()
--规划两个航点以计算桨叶航向角:以此时刻飞机机体坐标系fru为参考系，规划两个航点
local x1=0; local y1=5.0; local z1=0;--改变会影响computeBladeYaw()的计算
local x2=0; local y2=15.0; local z2=0;--改变会影响computeBladeYaw()的计算
--转换到neu坐标系下
local x1_neu,y1_neu,z1_neu=frameAToframeB(x1,y1,z1,0,0,0,0,0,yaw_stage1_rad)
local x2_neu,y2_neu,z2_neu=frameAToframeB(x2,y2,z2,0,0,0,0,0,yaw_stage1_rad)
local p_x1_neu = Vector3f()
p_x1_neu:x(x1_neu)
p_x1_neu:y(y1_neu)
p_x1_neu:z(z1_neu)
local p_x2_neu = Vector3f()
p_x2_neu:x(x2_neu)
p_x2_neu:y(y2_neu)
p_x2_neu:z(z2_neu)
--当前机体系原点在导航坐标系下的坐标
local curr_loc_neu = curr_loc.get_vector_from_origin_NEU(curr_loc) --返回的是cm
curr_loc_neu:x(0.01*curr_loc_neu:x())
curr_loc_neu:y(0.01*curr_loc_neu:y())
curr_loc_neu:z(0.01*curr_loc_neu:z())
loc0_neu_m=curr_loc_neu:copy()
--航点
p1_stage1_ned =p_x1_neu+curr_loc_neu
p1_stage1_ned:z(-p1_stage1_ned:z())
p2_stage1_ned =p_x2_neu+curr_loc_neu
p2_stage1_ned:z(-p2_stage1_ned:z())
gcs:send_text(0, "stage1 task run!!")
write_to_file(get_time_str().."stage1 task run!!")
stage1_flag=0
return stage1_task,1000
else
gcs:send_text(0, string.format("alt not suitable!! alt_rel_curr is %f cm ", alt_rel_curr))
return sleep,10000
end
end
return sleep, 3000
以上两个脚本可在仿真中运行，验证默写功能，实际实验请使用下面的替换：
local has_data = get_has_data()
local obj_dist_cm = 3200
替换为：
local has_data = rangefinder:has_data_orient(rotation_forward)
local obj_dist_cm = rangefinder:distance_cm_orient(rotation_forward)
834
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
