ardupilot开发 --- 旋转关系 篇

两个FRU坐标系的旋转关系、矩阵

-- 两个FRU坐标系的z轴旋转关系，绕正z轴旋转angle
function rotateZ(x,y,z,angle_deg)
    local rot_x,rot_y,rot_z
    local angle_rad = math.rad(angle_deg)
    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轴旋转关系 
function rotateY(x,y,z,angle_deg)
    local rot_x,rot_y,rot_z
    local angle_rad = math.rad(angle_deg)
    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轴旋转关系 
function rotateX(x,y,z,angle_deg)
    local rot_x,rot_y,rot_z
    local angle_rad = math.rad(angle_deg)
    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
function frameAToframeB(xa,ya,za,xa0_b,ya0_b,za0_b,angleX_deg,angleY_deg,angleZ_deg)
    local Xb,Yb,Zb
    Xb,Yb,Zb=rotateZ(xa,ya,za,angleZ_deg)
    Xb,Yb,Zb=rotateY(Xb,Yb,Zb,angleY_deg)
    Xb,Yb,Zb=rotateX(Xb,Yb,Zb,angleX_deg)
    return Xb+xa0_b,Yb+ya0_b,Zb+za0_b
end