xb/build
0
0
Fork 0
Code Issues Activity

Initial commit from DCSP - 2026/1/15 15:11:58

Browse Source
This commit is contained in:
xb
2026-01-15 15:11:58 +08:00
commit c93536b187
100 changed files with 59765 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
sensor/source/sensor_wheel.cpp Normal file
View File

@@ -0,0 +1,129 @@
#include "sensor_wheel.h"
#include "orbit_info.h"
#include "GNCFunction.h"
#include "libconvert.h"
#include "liblog.h"
double WheelOutputPeriod = 1.0; //动量轮输出周期
//其中k为被转换发送的数据，exp取1000，limit4 取4.294967296e9，数组a[]为转换后发送的4字节数据。
void cal_cmder_para(double k, double exp, uint8_t *a);
/// @brief 生成遥测动量轮帧
/// @param frame:生成的遥测帧数据
/// @return:0
uint8_t generate_wheel_frame(uint8_t *frame,double CmdWheel)
{
FlyWheel_Frame wheel;
memset(frame,0,WHEEL_FRAME_LENGTH);
memset( (uint8_t *)&wheel.header,0,WHEEL_FRAME_LENGTH);
wheel.header = 0xFF;
//wheel.rotate_speed[3] = 10;
cal_cmder_para(CmdWheel,WHEEL_EXP,wheel.rotate_speed);
wheel.id = 0x11;
uint8_t checksum = 0;
uint8_t *buffer = (uint8_t *)&wheel.header;
for(int kc = 1;kc<WHEEL_FRAME_LENGTH-2;kc++)
checksum +=buffer[kc];
wheel.checksum = checksum;
memcpy(frame,(uint8_t *)&wheel.header,WHEEL_FRAME_LENGTH);
spdlog::info("函数{0}动量轮数据帧长度为:{1:2d} 校验和为:{2:2x}",__FUNCTION__,
sizeof(FlyWheel_Frame),frame[WHEEL_FRAME_LENGTH-1]);
return 0;
}
void cal_cmder_para(double k, double exp, uint8_t *a)
{
//float limit4=4.294967296e9;
unsigned long t;
if(k<0) t=(unsigned long)(WHEEL_LIMIT4+k*exp);
else t=(unsigned long)(k*exp);
a[0]=(uint8_t)(t>>24)&0xff;
a[1]=(uint8_t)(t>>16)&0xff;
a[2]=(uint8_t)(t>>8)&0xff;
a[3]=(uint8_t)t&0xff;
}
double cal_message_para(int s1, int s2, int s3, int s4, int exp)
{
//float limit4=4.294967296e9;
double result=0;
unsigned long k = 0;
k=(((unsigned long)s1<<24) & 0xff000000) | (((unsigned long)s2<<16) & 0xff0000) | (((unsigned long)s3<<8) & 0xff00) | ((unsigned long)s4 & 0xff);
if(k>0x7fffffff)
{
result = (double)k;
result = (result-WHEEL_LIMIT4);
result = result/exp;
}
else
{
result = (double)k;
result = result/exp;
}
return result;
}
/* 动量轮输出模拟 */
void sensor_WheelMeas(double WheelTorqueBody[3], double WheelAM[3], double CmdWheel[3]) {
/* 动量轮测量模拟模块 */
/* 输入量 */
/* WheelMtxI 动量轮转动惯量(主对角) */
/* WheelMtxInstall 动量轮总安装矩阵 */
/* CmdWheel 动量轮转速指令rpm*/
/* WheelOutputPeriod 动量轮采样周期rpm*/
/* 输出量 */
/* MeasWheelRPM 动量轮输出值rpm转每分 */
/* WheelTorqueBody 动量轮总力矩 */
/* WheelAM 动量轮角动量 */
double wgi_true[3], DeltaWheelVel[3], temp[3], WheelTorque[3], WheelAMInstall[3], MeasWheelRPM[3];
static double WheelRPM[3] = { 0, 0, 0 };
double WheelMtxI = 1.273 * 1e-4;
double WheelMtxInstall[3][3] = { {0, 1, 0},
{-1, 0, 0},
{0, 0, 1} };
MeasWheelRPM[0] = CmdWheel[0] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
MeasWheelRPM[1] = CmdWheel[1] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
MeasWheelRPM[2] = CmdWheel[2] + ((rand() / ((double)RAND_MAX)) * 2.0 - 1.0) / 0.5;
DeltaWheelVel[0] = (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[0] * 2.0 * 3.1415926535 / 60.0);
DeltaWheelVel[1] = (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[1] * 2.0 * 3.1415926535 / 60.0);
DeltaWheelVel[2] = (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0) - (WheelRPM[2] * 2.0 * 3.1415926535 / 60.0);
temp[0] = DeltaWheelVel[0] / WheelOutputPeriod;
temp[1] = DeltaWheelVel[1] / WheelOutputPeriod;
temp[2] = DeltaWheelVel[2] / WheelOutputPeriod;
WheelTorque[0] = -1.0 * WheelMtxI * temp[0];
WheelTorque[1] = -1.0 * WheelMtxI * temp[1];
WheelTorque[2] = -1.0 * WheelMtxI * temp[2];
mtxMtp((double*)WheelTorqueBody, (double*)WheelMtxInstall, 3, 3, (double*)WheelTorque, 3, 1);
WheelAMInstall[0] = WheelMtxI * (CmdWheel[0] * 2.0 * 3.1415926535 / 60.0);
WheelAMInstall[1] = WheelMtxI * (CmdWheel[1] * 2.0 * 3.1415926535 / 60.0);
WheelAMInstall[2] = WheelMtxI * (CmdWheel[2] * 2.0 * 3.1415926535 / 60.0);
mtxMtp((double*)WheelAM, (double*)WheelMtxInstall, 3, 3, (double*)WheelAMInstall, 3, 1);
WheelRPM[0] = CmdWheel[0];
WheelRPM[1] = CmdWheel[1];
WheelRPM[2] = CmdWheel[2];
}