#include "Sub.h" #ifdef USERHOOK_INIT void Sub::userhook_init() { // put your initialisation code here // this will be called once at start-up } #endif #ifdef USERHOOK_FASTLOOP void Sub::userhook_FastLoop() { // put your 100Hz code here } #endif #ifdef USERHOOK_50HZLOOP extern mavlink_rov_control_t rov_control; void Sub::userhook_50Hz() { USBL_PowerSwitch(); getgain(); getyaw(); } #endif #ifdef USERHOOK_MEDIUMLOOP void Sub::userhook_MediumLoop() { // put your 10Hz code here } #endif #ifdef USERHOOK_SLOWLOOP void Sub::userhook_SlowLoop() { // put your 3.3Hz code here } #endif #ifdef USERHOOK_SUPERSLOWLOOP void Sub::userhook_SuperSlowLoop() { // put your 1Hz code here } #endif void Sub::USBL_PowerSwitch(void) { if (rov_control.floodlight == 1 && motors.armed()) { lights = 1; }else { lights = 0; } if(motors.armed() && rov_control.USBL_flag==1 ){ usblpoweroff = 1;//usbl }else{ usblpoweroff = 0;//usbl } } extern mavlink_rov_state_monitoring_t rov_message; void Sub::getgain(void){ //上位机设置油门 //uint16_t _gain = SRV_Channels::srv_channel(9)->get_output_min();//上位机设置油门 gain = 1.0; static int j = 0; j++; if(j>800) { //gcs().send_text(MAV_SEVERITY_WARNING, " gain %f\n",(float)gain); j=0; } } void Sub::getyaw(void){ if (!motors.armed()){ yaw_press = (int16_t)(ahrs.yaw_sensor/100);//记住方位 return; }else{ } } void Sub::updat_Atom(void){ uart2_read_Atom(hal.uartD); GetAngle(); } void Sub::uart2_read_Atom(AP_HAL::UARTDriver *uart) { if (uart == nullptr) { return; } uint32_t readtime=AP_HAL::micros(); unsigned char received_char; unsigned char Xordata; unsigned char last_byte; while(uart->available())//recive num { received_char = uart->read(); ucRxBufferATOM[ucRxCnt_atom]=received_char; //数据头是41 78 ----------------------- if(ucRxBufferATOM[0]!=0x41) { ucRxCnt_atom=0; // 不是头 continue; } else if(ucRxCnt_atom>0 && ucRxBufferATOM[1]!=0x78){ ucRxCnt_atom=0; //不是头 continue; } //--------------------- ucRxCnt_atom++; last_byte =ucRxBufferATOM[5]+7;//数据个数ucRxBufferATOM[5] + 帧头6个：41 78 FF 06 81 35+校验1+尾6d if(ucRxCnt_atom>last_byte && ucRxBufferATOM[last_byte]==0x6D) { break; } if(AP_HAL::micros() - readtime >800)//时间溢出0.8ms {//接收数据超时 ucRxCnt_atom=0; usart_state_atom =FALSE;//通信失败 memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256); gcs().send_text(MAV_SEVERITY_INFO, "atom read timeout"); return; } } if (ucRxCnt_atom==0 ){//没有接收到数据 return; }else{ //异或校验 Xordata =Atom_BBC(ucRxBufferATOM,ucRxCnt_atom-2); if (Xordata != ucRxBufferATOM[ucRxCnt_atom-2]) {//校验失败 static int j = 0; j++; if(j>400) { gcs().send_text(MAV_SEVERITY_INFO, " ATOM 400 times error \n"); j=0; } ucRxCnt_atom =0; usart_state_atom =FALSE; //通信失败 memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256); return; } usart_state_atom =TRUE;//通信成功 ucRxCnt_atom =0; } } //原子九轴校验 异或校验 unsigned char Sub::Atom_BBC(unsigned char *addr,uint16_t len) { unsigned char *DataPoint; DataPoint = addr; unsigned char XorData = 0; unsigned short DataLength=len; while(DataLength--) { XorData = XorData ^ *DataPoint; DataPoint++; } return XorData; } void Sub::GetAngle(void) { //添加数据 uint16_t data_cnt_sum ; data_cnt_sum = ucRxBufferATOM[5]+6;//6个字节的帧头 uint32_t data_adress[127]; uint16_t data_cnt; uint16_t adress_index = 0; uint16_t data_index=6; uint16_t SaberID = 0x0000; uint16_t i =0; if (usart_state_atom == TRUE ) { //收到的是命令 SaberID = ucRxBufferATOM[3]+(ucRxBufferATOM[4]<<8); } if(usart_state_atom ==TRUE && (SaberID== RespAllStatusID ||SaberID == SystemResetAckID)) { //接收到的是命令反馈 for(i=0;i<12;i++){ SaberCommandRes[i] = ucRxBufferATOM[i]; } usart_state_atom = FALSE; memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256); }else if(usart_state_atom ==TRUE &&SaberID == SaberDatapacketID){//接收到的是数据 while(data_index< data_cnt_sum) { //通信成功 数据包 data_adress[adress_index]= (ucRxBufferATOM[data_index+1]<<8)+ ucRxBufferATOM[data_index]; if (data_adress[adress_index] == 0x8C01) {//读取陀螺仪角速度 GyroX = char_to_float(ucRxBufferATOM[data_index+3],ucRxBufferATOM[data_index+4],ucRxBufferATOM[data_index+5],ucRxBufferATOM[data_index+6]); GyroY = char_to_float(ucRxBufferATOM[data_index+7],ucRxBufferATOM[data_index+8],ucRxBufferATOM[data_index+9],ucRxBufferATOM[data_index+10]); GyroZ = char_to_float(ucRxBufferATOM[data_index+11],ucRxBufferATOM[data_index+12],ucRxBufferATOM[data_index+13],ucRxBufferATOM[data_index+14]); //转化成弧度 GyroX=GyroX*3.14/180.0; GyroY=GyroY*3.14/180.0; GyroZ=GyroZ*3.14/180.0; } else if(data_adress[adress_index] ==0xB001) {//读取角度 float yawbuf=0.0; Roll = char_to_float(ucRxBufferATOM[data_index+3],ucRxBufferATOM[data_index+4],ucRxBufferATOM[data_index+5],ucRxBufferATOM[data_index+6]); Pitch = char_to_float(ucRxBufferATOM[data_index+7],ucRxBufferATOM[data_index+8],ucRxBufferATOM[data_index+9],ucRxBufferATOM[data_index+10]); yawbuf = char_to_float(ucRxBufferATOM[data_index+11],ucRxBufferATOM[data_index+12],ucRxBufferATOM[data_index+13],ucRxBufferATOM[data_index+14]); //转化成弧度 Roll_Raian =Roll*3.14/180.0; Pitch_Raian =Pitch*3.14/180.0; //yaw转化成0-360 if(yawbuf < 0.0) { Yaw = 360.0 + yawbuf ; } else { Yaw = yawbuf; } //------PITCH 90度安装----------- /*Yaw_Angle = (360.0-Yaw); Yaw_Raian = Yaw_Angle*3.14/180.0; attitude_control.update_ATOM_sensor(Roll_Raian,Pitch_Raian,Yaw_Raian,GyroY,GyroX,GyroZ);//// 注意原子九轴X是俯仰 Y是翻滚应当将XY反过来试一试 */ //-------ROLL 90安装---------------------- float yawbufraian=0.0; //原子的角速度方向和飞控相反 yawbufraian =(360.0-Yaw)*3.14/180.0+1.57; Yaw_Angle = (360.0-Yaw)+90.0; //限制角度在0-360之间 if (Yaw_Angle>360) { Yaw_Angle = Yaw_Angle-360; } if (yawbufraian>6.28) { Yaw_Raian = yawbufraian-6.28; } else{ Yaw_Raian =yawbufraian; } attitude_control.update_ATOM_sensor(Roll_Raian,Pitch_Raian,Yaw_Raian,GyroY,GyroX,-GyroZ);//// 注意原子九轴X是俯仰 Y是翻滚应当将XY反过来试一试 } data_cnt = ucRxBufferATOM[data_index+2]; data_index =data_index+data_cnt +3; adress_index++; } usart_state_atom = FALSE; memset(ucRxBufferATOM, 0, sizeof(unsigned char)*256); } } //将char类型转换为float类型，输入为4个字节 小端在前，输出为float类型 float Sub::char_to_float(unsigned char u1,unsigned char u2,unsigned char u3,unsigned char u4) { data_floatfromchar.u[0] = u1; data_floatfromchar.u[1] = u2; data_floatfromchar.u[2] = u3; data_floatfromchar.u[3] = u4; return data_floatfromchar.f; } #define maxangle 55.0 //机器人物理空间运动状态的水平竖直切换 void Sub::direction0_90(void) { static int16_t cnt=0; static int16_t cnt2=0; static int16_t cnt_backzero1=0; static int16_t cnt_backzero2=0; /* 控制区间的变化 抬头或者低头大于60度进入正负竖直状态； 进入正负竖直状态之后 外置传感器的低头抬头要保证在机器人（不是任何传感器）在空间中60到150以及-60到-150之间，保持竖直状态标志为1，超出这个范围恢复水平状态 侧歪若超过45度，说明控制不了，回到水平状态 即三个姿态均为0度 水平状态下 roll不能过大 最大控制为70度，如果超过80度 说明控制不稳定回到水平状态 */ /*ahrs.pitch_sensor： *（90） *（>60） * *------ * *--------* * *（<-60） *（-90） */ if ((ahrs.pitch_sensor >= attitude_control.change_angle*100) && (fabsf(Pitch)=maxangle ||(Roll>-120.0 && Roll<-60)){ //侧歪>45 || 外置九轴大于-120 小于-60（机器人空间姿态150 - 210度） cnt_backzero1++; if(cnt_backzero1 >50){ cnt_backzero1 = 0; attitude_control.roll_pitch_back_orign = 1;//复位回到水平 } }else{ cnt_backzero1 = 0; } if ((Roll<150.0 && Roll>30)) {//外置九轴30到150, 机器人空间姿态-60 - 60度 cnt++; if (cnt>=50) { cnt = 0; agl_sec = section0;//水平 } } } else if(agl_sec == section_90) { if (fabsf(Pitch)>=maxangle ||(Roll>-120.0 && Roll<-60)){ //侧歪>45 || 外置九轴大于-120 小于-60（机器人空间姿态150 - 210度） cnt_backzero2++; if(cnt_backzero2 >50){ cnt_backzero2 = 0; attitude_control.roll_pitch_back_orign = 1;//复位水平 } }else{ cnt_backzero2 = 0; } if (Roll>30 && Roll< 150)//Offset_ver -30 {//外置九轴30到150, 机器人空间姿态-60 - 60度 cnt2++; if (cnt2>=50) { cnt2 = 0; agl_sec = section0;//水平 } } } else if(fabsf(Pitch)>=maxangle){ //20210624--------------- if (control_mode == STABILIZE) { attitude_control.roll_pitch_back_orign = 1;//复位回到水平 } agl_sec = section0;//水平 } //-------------------------------------------- /* 机器人 飞控 外置九轴的空间姿态对照表 机器人pitch 飞控 pitch 九轴 roll 计算 0 0 90 90-飞控 60 60 30 90-飞控 90 90 0 90-飞控 120 60 -30 飞控-90 150 30 -60 飞控-90 180 0 -90 飞控-90 270 -90 -180 飞控-90 360 0 90 90-飞控 这里目前机器人的角度限制在-90到+90 也就是90-飞控的模式下 */ if (attitude_control.roll_pitch_back_orign ==1){ if (fabsf(ahrs.pitch_sensor)<3000 &&fabsf(ahrs.roll_sensor)<3000) {// 确保已经回到水平状态 attitude_control.roll_pitch_back_orign =0; } } if (agl_sec == section0) {//水平状态 /*if (attitude_control.roll_pitch_back_orign ==1){ if (fabsf(ahrs.pitch_sensor)<3000 &&fabsf(ahrs.roll_sensor)<3000) {// 确保已经回到水平状态 attitude_control.roll_pitch_back_orign =0; } }*/ agl_act =action_Hor; } else if(agl_sec == section90) {//竖直 agl_act =action_Ver_postive; //外置九轴PID的pitch给定 attitude_control.deta_angle_901 =90.0-attitude_control.angle_geiding/100.0;//注意这里angle_geiding的范围是-90到90 也就是机器人本体物理角度的-90到正90度，参照上述说明 } else if(agl_sec == section_90) {//负向竖直 agl_act =action_Ver_negtive; //外置九轴PID的pitch给定 attitude_control.deta_angle_901 =90.0-attitude_control.angle_geiding/100.0; //外置九轴的roll范围是-180到+180 if(attitude_control.deta_angle_901<-180.0) { attitude_control.deta_angle_901 = attitude_control.deta_angle_901 +360.0; } } if(agl_act ==action_Hor) {//相当于外置九轴的pitch 不能超过70度，超过非常不稳定 if((ahrs.roll_sensor>7000&&ahrs.roll_sensor<8000) ||(ahrs.roll_sensor<-7000 && ahrs.roll_sensor>-8000)) { //回到水平状态 attitude_control.roll_pitch_back_orign =1; } else if(ahrs.roll_sensor>=8000 || ahrs.roll_sensor<=-8000) {//超过80 disarm //arming.disarm();//海检测试抗流性的时候侧着安装不能启动 attitude_control.roll_pitch_back_orign =1; } } if (!motors.armed()) { attitude_control.roll_pitch_back_orign = 0; } attitude_control.update_Hor_Ver_Choose(agl_act);//将状态传递到姿态控制类 static int j = 0; j++; if(j>500) { // gcs().send_text(MAV_SEVERITY_WARNING, " roll_pitch_back_orign %d %d\n",(int)agl_act,(int)attitude_control.roll_pitch_back_orign); j=0; } }