130 lines
4.0 KiB
C++
130 lines
4.0 KiB
C++
#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];
|
||
}
|
||
|
||
|