無名創(chuàng)新——樹莓派4B+ROS+RealSense T265室內(nèi)定位替代光流開源方案
硬件機(jī)載端：樹莓派4B 4/8G 、T265、RPLIDAR-A2
配套開發(fā)環(huán)境：
樹莓派系統(tǒng)Ubuntu 20.04.3 LTS
RealSense SDK 2.0
ROS系統(tǒng)版本：noetic+realsense-ros
多平臺遠(yuǎn)程登陸軟件：nomachine（win10、mac、安卓、ubuntu等）
有兩種方案實(shí)現(xiàn)t265位姿數(shù)據(jù)，本方案通過修改宏定義T265_STATE_CAPTURE_ENABLE實(shí)現(xiàn)
1、可直接利用SDK函數(shù)中的rs-pose獲取位姿數(shù)據(jù)，參考鏈接如下：
rs-pose (intelrealsense.com)
2、利用官方提供的realsense-ros包中自帶的demo啟動roslaunch realsense2_camera rs_t265.launch，通過訂閱節(jié)點(diǎn)發(fā)布的"/camera/odom/sample"話題獲取位姿數(shù)據(jù)
傳輸浮點(diǎn)數(shù)據(jù)需有用到的數(shù)據(jù)轉(zhuǎn)換函數(shù)（大小端通用）
#include <iostream>
#include "datatf.h"
using namespace std;
union
{
unsigned char floatByte[4];
float floatValue;
}FloatUnion;
/***************************************************************************************
@函數(shù)名：void Float2Byte(float *FloatValue, unsigned char *Byte, unsigned char Subscript)
@入口參數(shù)：FloatValue:float值
? ? ?Byte:數(shù)組
? ? ? ?Subscript:指定從數(shù)組第幾個(gè)元素開始寫入
@出口參數(shù)：無
功能描述：將float數(shù)據(jù)轉(zhuǎn)成4字節(jié)數(shù)據(jù)并存入指定地址
@作者：無名小哥
@日期：2020年01月17日
****************************************************************************************/
void Float2Byte(float *FloatValue, unsigned char *Byte, unsigned char Subscript)
{
FloatUnion.floatValue = (float)2;
if(FloatUnion.floatByte[0] == 0)//小端模式
{
FloatUnion.floatValue = *FloatValue;
Byte[Subscript]? ? ?= FloatUnion.floatByte[0];
Byte[Subscript + 1] = FloatUnion.floatByte[1];
Byte[Subscript + 2] = FloatUnion.floatByte[2];
Byte[Subscript + 3] = FloatUnion.floatByte[3];
}
else//大端模式
{
FloatUnion.floatValue = *FloatValue;
Byte[Subscript]? ? ?= FloatUnion.floatByte[3];
Byte[Subscript + 1] = FloatUnion.floatByte[2];
Byte[Subscript + 2] = FloatUnion.floatByte[1];
Byte[Subscript + 3] = FloatUnion.floatByte[0];
}
}
/***************************************************************************************
@函數(shù)名：void Byte2Float(unsigned char *Byte, unsigned char Subscript, float *FloatValue)
@入口參數(shù)：Byte:數(shù)組
? ? ?Subscript:指定從數(shù)組第幾個(gè)元素開始寫入
? ? ? ?FloatValue:float值
@出口參數(shù)：無
功能描述：從指定地址將4字節(jié)數(shù)據(jù)轉(zhuǎn)成float數(shù)據(jù)
@作者：無名小哥
@日期：2020年01月17日
****************************************************************************************/
void Byte2Float(unsigned char *Byte, unsigned char Subscript, float *FloatValue)
{
FloatUnion.floatByte[0]=Byte[Subscript];
FloatUnion.floatByte[1]=Byte[Subscript + 1];
FloatUnion.floatByte[2]=Byte[Subscript + 2];
FloatUnion.floatByte[3]=Byte[Subscript + 3];
*FloatValue=FloatUnion.floatValue;
}
機(jī)載端代碼如下:
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Odometry.h>
#include <tf2/LinearMath/Quaternion.h>
#include <librealsense2/rs.hpp>
#include "datatf.h"
#define T265_STATE_CAPTURE_ENABLE? ?1
static uint8_t NCLink_Head[2]={0xFC,0xFF};//數(shù)據(jù)幀頭
static uint8_t NCLink_End[2] ={0xA1,0xA2};//數(shù)據(jù)幀尾
uint8_t nclink_databuf[100];//待發(fā)送數(shù)據(jù)緩沖區(qū)
serial::Serial com;
geometry_msgs::Pose current_position;
geometry_msgs::Pose target_position;
geometry_msgs::Vector3 currrent_velocity;
std_msgs::Bool current_position_update_flag;
std_msgs::Bool target_position_update_flag;
void NCLink_Send_Currrent_State(float userdata1? ,float userdata2,
? ? ? ? float userdata3? ,float userdata4,
? ? ? ? float userdata5? ,float userdata6,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? float userdata7? ,float userdata8,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? float userdata9? ,float userdata10,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? float quality? ? ,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte0,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte1,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte2,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte3,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte4,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte5,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte6,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t byte7,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? uint8_t update_flag,uint8_t msg_id)
{
? ? uint8_t sum=0,_cnt=0,i=0;
? ? nclink_databuf[_cnt++]=NCLink_Head[0];
? ? nclink_databuf[_cnt++]=NCLink_Head[1];
? ? nclink_databuf[_cnt++]=msg_id;
? ? nclink_databuf[_cnt++]=0;
? ? ? ??
? ? Float2Byte(&userdata1,nclink_databuf,_cnt);//position.x
? ? _cnt+=4;
? ? Float2Byte(&userdata2,nclink_databuf,_cnt);//position.y
? ? _cnt+=4;
? ? Float2Byte(&userdata3,nclink_databuf,_cnt);//position.z
? ? _cnt+=4;
? ? Float2Byte(&userdata4,nclink_databuf,_cnt);//velocity.x
? ? _cnt+=4;
? ? Float2Byte(&userdata5,nclink_databuf,_cnt);//velocity.y
? ? _cnt+=4;
? ? Float2Byte(&userdata6,nclink_databuf,_cnt);//velocity.z
? ? _cnt+=4;
? ? Float2Byte(&userdata7,nclink_databuf,_cnt);//quaternion.x
? ? _cnt+=4;
? ? Float2Byte(&userdata8,nclink_databuf,_cnt);//quaternion.y
? ? _cnt+=4;
? ? Float2Byte(&userdata9,nclink_databuf,_cnt);//quaternion.z
? ? _cnt+=4;//28
? ? Float2Byte(&userdata10,nclink_databuf,_cnt);//quaternion.w
? ? _cnt+=4;//32
? ? Float2Byte(&quality,nclink_databuf,_cnt);//quality
? ? _cnt+=4;//32
? ? nclink_databuf[_cnt++]=byte0;
? ? nclink_databuf[_cnt++]=byte1;
? ? nclink_databuf[_cnt++]=byte2;
? ? nclink_databuf[_cnt++]=byte3;
? ? nclink_databuf[_cnt++]=byte4;
? ? nclink_databuf[_cnt++]=byte5;
? ? nclink_databuf[_cnt++]=byte6;
? ? nclink_databuf[_cnt++]=byte7;
? ? nclink_databuf[_cnt++]=update_flag;
? ? nclink_databuf[3] = _cnt-4;
? ? for(i=0;i<_cnt;i++) sum ^= nclink_databuf[i];?
? ? nclink_databuf[_cnt++]=sum;
? ? nclink_databuf[_cnt++]=NCLink_End[0];
? ? nclink_databuf[_cnt++]=NCLink_End[1];
? ? com.write(nclink_databuf,_cnt);
? ? //ROS_INFO("%d",_cnt);
}
void serial_write_callback(const std_msgs::String::ConstPtr& msg)
{
? ? ROS_INFO_STREAM("writing to serial port"<<msg->data);
? ? com.write(msg->data);
}
#ifdef T265_STATE_CAPTURE_ENABLE?
void t265_callback_internal(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
? ? ROS_INFO_STREAM("t265_callback_internal");
? ? NCLink_Send_Currrent_State(msg->pose.position.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.position.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.position.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?currrent_velocity.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?currrent_velocity.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?currrent_velocity.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.orientation.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.orientation.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.orientation.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.orientation.w,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?100.0f,0x01,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?0,1,2,3,4,5,6,7,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?0x0D);
}
#endif
void t265_callback_external(const nav_msgs::Odometry::ConstPtr& msg)
{
? ? ROS_INFO_STREAM("t265_callback_external");?
? ? NCLink_Send_Currrent_State(msg->pose.pose.position.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.pose.position.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.pose.position.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->twist.twist.linear.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->twist.twist.linear.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->twist.twist.linear.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?-msg->pose.pose.orientation.y,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.pose.orientation.z,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?-msg->pose.pose.orientation.x,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?msg->pose.pose.orientation.w,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?100.0f,0x01,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?0,1,2,3,4,5,6,7,
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?0x0D);
}
int main(int argc,char** argv)
{
? ? ros::init(argc,argv,"serialport");
? ? ros::NodeHandle n;
? ? ros::Subscriber serial_write_sub=n.subscribe("serial_write",1000,serial_write_callback);
? ? ros::Publisher serial_read_pub=n.advertise<std_msgs::String>("serial_write",1000);
? ? ros::Subscriber t265_sub_external=n.subscribe("/camera/odom/sample",1000,t265_callback_external);
#ifdef T265_STATE_CAPTURE_ENABLE? ?
? ? ros::Subscriber t265_sub_innernal=n.subscribe("/t265/odom/sample",1000,t265_callback_internal);
? ? ros::Publisher t265_pub=n.advertise<geometry_msgs::PoseStamped>("/t265/odom/sample",1000);?
? ? rs2::pipeline pipe;
? ? rs2::config cfg;
? ? cfg.enable_stream(RS2_STREAM_POSE,RS2_FORMAT_6DOF);
? ? pipe.start(cfg);
#endif
? ? try
? ? {
? ? ? ? com.setPort("/dev/serial0");//("/dev/ttyUSB0");("/dev/ttyAMA0");
? ? ? ? com.setBaudrate(921600);
? ? ? ? serial::Timeout to=serial::Timeout::simpleTimeout(1000);
? ? ? ? com.setTimeout(to);
? ? ? ? com.open();
? ? }
? ? catch (serial::IOException& e)
? ? {
? ? ? ? ROS_ERROR_STREAM("unable to open port");
? ? ? ? return -1;
? ? }
? ? if(com.isOpen())
? ? {
? ? ? ? ROS_INFO_STREAM("serial port is initialized");
? ? }
? ? else return -1;
? ? ros::Rate loop_rate(100);
? ? while(ros::ok())
? ? {
? ? ? ? if(com.available())
? ? ? ? {
? ? ? ? ? ? ROS_INFO_STREAM("read from serial port\n");
? ? ? ? ? ? std_msgs::String result;
? ? ? ? ? ? result.data=com.read(com.available());
? ? ? ? ? ? ROS_INFO_STREAM("read: "<<result.data);
? ? ? ? ? ? serial_read_pub.publish(result);
? ? ? ? }
? ? ? ? ros::spinOnce();
? ? ? ? loop_rate.sleep();
#ifdef T265_STATE_CAPTURE_ENABLE??
? ? ? ? auto frames=pipe.wait_for_frames();
? ? ? ? auto f=frames.first_or_default(RS2_STREAM_POSE);
? ? ? ? auto pose_data=f.as<rs2::pose_frame>().get_pose_data();
? ? ? ? //rs2_pose pose_data;
? ? ? ? geometry_msgs::PoseStamped pmsg;
? ? ? ? pmsg.pose.position.x=current_position.position.x=-pose_data.translation.x;
? ? ? ? pmsg.pose.position.y=current_position.position.y=-pose_data.translation.z;
? ? ? ? pmsg.pose.position.z=current_position.position.z=pose_data.translation.y;
? ? ? ? currrent_velocity.x=-pose_data.velocity.x;
? ? ? ? currrent_velocity.y=-pose_data.velocity.z;
? ? ? ? currrent_velocity.z=pose_data.velocity.y;
? ? ? ? pmsg.pose.orientation.x=pose_data.rotation.x;
? ? ? ? pmsg.pose.orientation.y=pose_data.rotation.y;
? ? ? ? pmsg.pose.orientation.z=pose_data.rotation.z;
? ? ? ? pmsg.pose.orientation.w=pose_data.rotation.w;
? ? ? ? t265_pub.publish(pmsg);
? ? ? ? ROS_INFO("translation:%f %f %f",
? ? ? ? pose_data.translation.x,
? ? ? ? pose_data.translation.y,
? ? ? ? pose_data.translation.z);
? ? ? ? ROS_INFO("velocity:%f %f %f",
? ? ? ? pose_data.velocity.x,
? ? ? ? pose_data.velocity.y,
? ? ? ? pose_data.velocity.z);
? ? ? ? ROS_INFO("quad:%f %f %f %f",
? ? ? ? pose_data.rotation.x,
? ? ? ? pose_data.rotation.y,
? ? ? ? pose_data.rotation.z,
? ? ? ? pose_data.rotation.w);
#endif? ? ? ??
? ? }
}
飛控端解析代碼如下：
void NCLink_Data_Prase_Process_Lite(uint8_t *data_buf,uint8_t num)//飛控?cái)?shù)據(jù)解析進(jìn)程
{
? uint8_t sum = 0;
? for(uint8_t i=0;i<(num-3);i++)? sum ^= *(data_buf+i);
? if(!(sum==*(data_buf+num-3)))? ? return;//判斷sum
if(!(*(data_buf)==NCLink_Head[1]&&*(data_buf+1)==NCLink_Head[0]))? ? ? ? ?return;//判斷幀頭
if(!(*(data_buf+num-2)==NCLink_End[0]&&*(data_buf+num-1)==NCLink_End[1])) return;//幀尾校驗(yàn)??
if(*(data_buf+2)==0X0A)? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//地面站控制移動數(shù)據(jù)
? {
? ? ngs_sdk.move_mode=*(data_buf+4),
ngs_sdk.mode_order=*(data_buf+5);
? ? ngs_sdk.move_distance=(uint16_t)(*(data_buf+6)<<8)|*(data_buf+7);;
? ? ngs_sdk.update_flag=true;
ngs_sdk.move_flag.sdk_front_flag=false;
ngs_sdk.move_flag.sdk_behind_flag=false;
ngs_sdk.move_flag.sdk_left_flag=false;
ngs_sdk.move_flag.sdk_right_flag=false;
ngs_sdk.move_flag.sdk_up_flag=false;
ngs_sdk.move_flag.sdk_down_flag=false;
if(*(data_buf+4)==SDK_FRONT)
{
ngs_sdk.move_flag.sdk_front_flag=true;
ngs_sdk.f_distance=ngs_sdk.move_distance;
}
else if(*(data_buf+4)==SDK_BEHIND)?
{
ngs_sdk.move_flag.sdk_behind_flag=true;
ngs_sdk.f_distance=-ngs_sdk.move_distance;
}
else if(*(data_buf+4)==SDK_LEFT)??
{
ngs_sdk.move_flag.sdk_left_flag=true;
ngs_sdk.f_distance=-ngs_sdk.move_distance;
}
else if(*(data_buf+4)==SDK_RIGHT)
{
ngs_sdk.move_flag.sdk_right_flag=true;
ngs_sdk.f_distance=ngs_sdk.move_distance;
}
else if(*(data_buf+4)==SDK_UP)
{?
ngs_sdk.move_flag.sdk_up_flag=true;
ngs_sdk.f_distance=ngs_sdk.move_distance;
}
else if(*(data_buf+4)==SDK_DOWN)?
{
ngs_sdk.move_flag.sdk_down_flag=true;
ngs_sdk.f_distance=-ngs_sdk.move_distance;
}
NCLink_Send_Check_Flag[8]=1;
Pilot_Status_Tick();
? }
else if(*(data_buf+2)==0X0C)
? {
Guide_Flight_Lng =((int32_t)(*(data_buf+4)<<24)|(*(data_buf+5)<<16)|(*(data_buf+6)<<8)|*(data_buf+7));
Guide_Flight_Lat =((int32_t)(*(data_buf+8)<<24)|(*(data_buf+9)<<16)|(*(data_buf+10)<<8)|*(data_buf+11));
Guide_Flight_Cnt++;;
Guide_Flight_Flag=1;
Pilot_Status_Tick();
}
else if(*(data_buf+2)==0X0D)
{
Byte2Float(data_buf,4,¤t_state.position_x);
Byte2Float(data_buf,8,¤t_state.position_y);
Byte2Float(data_buf,12,¤t_state.position_z);
Byte2Float(data_buf,16,¤t_state.velocity_x);
Byte2Float(data_buf,20,¤t_state.velocity_y);
Byte2Float(data_buf,24,¤t_state.velocity_z);
current_state.position_x*=100.0f;
current_state.position_y*=100.0f;
current_state.position_z*=100.0f;
current_state.velocity_x*=100.0f;
current_state.velocity_y*=100.0f;
current_state.velocity_z*=100.0f;
Byte2Float(data_buf,28,¤t_state.q[0]);
Byte2Float(data_buf,32,¤t_state.q[1]);
Byte2Float(data_buf,36,¤t_state.q[2]);
Byte2Float(data_buf,40,¤t_state.q[3]);
float _q[4];
_q[0]=current_state.q[3]*(1.0f);
_q[1]=current_state.q[0]*(1.0f);
_q[2]=current_state.q[2]*(-1.0f);
_q[3]=current_state.q[1]*(1.0f);
current_state.q[0]=_q[0];
current_state.q[1]=_q[1];
current_state.q[2]=_q[2];
current_state.q[3]=_q[3];
quad_getangle(current_state.q,current_state.rpy);
if(current_state.rpy[2]<0.0f)? ?current_state.rpy[2]+=360.0f;
if(current_state.rpy[2]>360.0f) current_state.rpy[2]-=360.0f;
Byte2Float(data_buf,44,¤t_state.quality);
current_state.update_flag=*(data_buf+48);
current_state.byte[0]=*(data_buf+49);
current_state.byte[1]=*(data_buf+50);
current_state.byte[2]=*(data_buf+51);
current_state.byte[3]=*(data_buf+52);
current_state.byte[4]=*(data_buf+53);
current_state.byte[5]=*(data_buf+54);
current_state.byte[6]=*(data_buf+55);
current_state.byte[7]=*(data_buf+56);
Pilot_Status_Tick();
? Get_Systime(&nclink_t);
}
}
飛控IMU與VIO融合代碼如下：
uint16_t VIO_Sync_Cnt=5;
float K_ACC_VIO=1.0f,K_VEL_VIO=5.0f,K_POS_VIO=1.0f;
void? VIO_SLAM_INS_CF(void)
{
? Vector2f speed_delta={0};
float obs_err[2];
? if(current_state.update_flag==1)//存在數(shù)據(jù)光流更新時(shí)，20ms一次
? {?
opt_data.valid=1;
current_state.update_flag=0;?
OpticalFlow_Position.x=current_state.position_x;
OpticalFlow_Position.y=current_state.position_y;
OpticalFlow_Speed.x? ?=current_state.velocity_x;
OpticalFlow_Speed.y? ?=current_state.velocity_y;
OpticalFlow_Speed_Err.x=OpticalFlow_Speed.x-VIO_SINS.Speed[_EAST];
OpticalFlow_Speed_Err.y=OpticalFlow_Speed.y-VIO_SINS.Speed[_NORTH];
? ?
? ? OpticalFlow_Position_Err.x=OpticalFlow_Position.x-VIO_SINS.Pos_Backups[_EAST][VIO_Sync_Cnt];
? ? OpticalFlow_Position_Err.y=OpticalFlow_Position.y-VIO_SINS.Pos_Backups[_NORTH][VIO_Sync_Cnt];
OpticalFlow_Speed_Err.x=OpticalFlow_Speed.x-VIO_SINS.Vel_Backups[_EAST][VIO_Sync_Cnt];
? ? OpticalFlow_Speed_Err.y=OpticalFlow_Speed.y-VIO_SINS.Vel_Backups[_NORTH][VIO_Sync_Cnt];
OpticalFlow_Speed_Err.x=constrain_float(OpticalFlow_Speed_Err.x,-250,250);
OpticalFlow_Speed_Err.y=constrain_float(OpticalFlow_Speed_Err.y,-250,250);
? }
obs_err[0]=OpticalFlow_Speed_Err.x;
obs_err[1]=OpticalFlow_Speed_Err.y;
//三路反饋量修正慣導(dǎo)
correct[0].acc +=obs_err[0]* K_ACC_VIO*0.005f;//加速度矯正量
correct[1].acc +=obs_err[1]* K_ACC_VIO*0.005f;//加速度矯正
correct[0].acc=constrain_float(correct[0].acc,-50,50);
correct[1].acc=constrain_float(correct[1].acc,-50,50);
correct[0].vel? =obs_err[0]* K_VEL_VIO*0.005f;//速度矯正量
correct[1].vel? =obs_err[1]* K_VEL_VIO*0.005f;//速度矯正量
correct[0].pos? =obs_err[0]* K_POS_VIO*0.005f;//位置矯正量
correct[1].pos? =obs_err[1]* K_POS_VIO*0.005f;//位置矯正量
acc.x=-Origion_NamelessQuad._Acceleration[_EAST];//慣導(dǎo)加速度沿載體橫滾,機(jī)頭左側(cè)為正
acc.y= Origion_NamelessQuad._Acceleration[_NORTH];//慣導(dǎo)加速度沿載體機(jī)頭,機(jī)頭前向?yàn)檎?/p>
? VIO_SINS.Acceleration[_EAST] = acc.x+correct[0].acc;//慣導(dǎo)加速度沿載體橫滾,機(jī)頭左側(cè)為正
? VIO_SINS.Acceleration[_NORTH]= acc.y+correct[1].acc;//慣導(dǎo)加速度沿載體機(jī)頭,機(jī)頭前向?yàn)檎?/p>
??
speed_delta.x=(VIO_SINS.Acceleration[_EAST])*0.005f;
? speed_delta.y=(VIO_SINS.Acceleration[_NORTH])*0.005f;?
?
VIO_SINS.Position[_EAST]+=VIO_SINS.Speed[_EAST]*0.005f+0.5f*speed_delta.x*0.005f+pos.x+correct[0].pos;
VIO_SINS.Position[_NORTH] +=VIO_SINS.Speed[_NORTH] *0.005f+0.5f*speed_delta.y*0.005f+pos.y+correct[1].pos;
VIO_SINS.Speed[_EAST]+=speed_delta.x+correct[0].vel;
VIO_SINS.Speed[_NORTH] +=speed_delta.y+correct[1].vel;
for(uint16_t i=Num-1;i>0;i--)
{
VIO_SINS.Pos_Backups[_NORTH][i]=VIO_SINS.Pos_Backups[_NORTH][i-1];
VIO_SINS.Pos_Backups[_EAST][i]=VIO_SINS.Pos_Backups[_EAST][i-1];
VIO_SINS.Vel_Backups[_NORTH][i]=VIO_SINS.Vel_Backups[_NORTH][i-1];
VIO_SINS.Vel_Backups[_EAST][i]=VIO_SINS.Vel_Backups[_EAST][i-1];
}? ?
VIO_SINS.Pos_Backups[_NORTH][0]=VIO_SINS.Position[_NORTH];
? VIO_SINS.Pos_Backups[_EAST][0]=VIO_SINS.Position[_EAST];?
? VIO_SINS.Vel_Backups[_NORTH][0]=VIO_SINS.Speed[_NORTH];
? VIO_SINS.Vel_Backups[_EAST][0]=VIO_SINS.Speed[_EAST];
from_vio_to_body_frame(VIO_SINS.Position[_EAST],VIO_SINS.Position[_NORTH],
? &OpticalFlow_SINS.Position[_EAST],&OpticalFlow_SINS.Position[_NORTH],
current_state.rpy[2]);
from_vio_to_body_frame(VIO_SINS.Speed[_EAST],VIO_SINS.Speed[_NORTH],
? &OpticalFlow_SINS.Speed[_EAST],&OpticalFlow_SINS.Speed[_NORTH],
current_state.rpy[2]);
}
