void cDataLink::SendImuMsg(float ax, float ay, float az,
float rx, float ry, float rz,
float mx, float my, float mz,
float p, float T, float p_alt,
long update_mask)
{
int bytes_sent;
uint16_t len;
// ax = 1;
// ay = 2;
// az = 9.81;
//
//
//
// counter = counter + 3.14*0.01;
//
// mx=.1*cos(counter)*cos(counter)+0.1;
// my=.1*sin(counter)*cos(counter)+0.2;
// mz=0.1*sin(counter);
//
// printf("mag: %f, %f, %f\n", mx, my, mz);
/* Send high res imu */
mavlink_msg_highres_imu_pack(1, MAV_COMP_ID_IMU, &m_msg, microsSinceEpoch(),
ax, ay, az, //ax, ay, az
rx, ry, rz, //rx, ry, rz
mx, my, mz, // mx, my, mz
p, //abs
0., //diff
p_alt, //pressure_alt
T, //temperature
update_mask); //bitmask of sensor updates
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send raw_imu */
mavlink_msg_raw_imu_pack(1, MAV_COMP_ID_IMU, &m_msg, microsSinceEpoch(),
(int16_t)(ax*100.), (int16_t)(ay*100.), (int16_t)(az*100.), //ax, ay, az
(int16_t)(rx*100.), (int16_t)(ry*100.), (int16_t)(rz*100.), //rx, ry, rz
(int16_t)(mx*1000.), (int16_t)(my*1000.), (int16_t)(mz*1000.)); //bitmask of sensor updates
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send high scaled_imu */
mavlink_msg_scaled_imu_pack(1, MAV_COMP_ID_IMU, &m_msg, microsSinceEpoch(),
(int16_t)(ax*100.), (int16_t)(ay*100.), (int16_t)(az*100.), //ax, ay, az
(int16_t)(rx*100.), (int16_t)(ry*100.), (int16_t)(rz*100.), //rx, ry, rz
(int16_t)(mx*1000.), (int16_t)(my*1000.), (int16_t)(mz*1000.)); //bitmask of sensor updates
len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
(void)bytes_sent; //avoid compiler warning
}
uint16_t PackRawIMU(uint8_t system_id, uint8_t component_id, mavlink_raw_imu_t mlRawIMUData ,uint32_t time_usec){
mavlink_system_t mavlink_system;
// if (!(mlPending.wpProtState == WP_PROT_IDLE))
// return 0;
mavlink_system.sysid = system_id; ///< ID 20 for this airplane
mavlink_system.compid = component_id;//MAV_COMP_ID_IMU; ///< The component sending the message is the IMU, it could be also a Linux process
//////////////////////////////////////////////////////////////////////////
mavlink_message_t msg;
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_raw_imu_pack(mavlink_system.sysid, mavlink_system.compid, &msg , time_usec , mlRawIMUData.xacc , mlRawIMUData.yacc , mlRawIMUData.zacc , mlRawIMUData.xgyro , mlRawIMUData.ygyro , mlRawIMUData.zgyro , mlRawIMUData.xmag , mlRawIMUData.ymag , mlRawIMUData.zmag );
return( mavlink_msg_to_send_buffer(UartOutBuff, &msg));
}
int gcs_raw_imu(int sock, daisy7_imu data) {
int bytes_sent;
mavlink_message_t msg;
uint8_t buf[512];
uint16_t len;
mavlink_msg_raw_imu_pack(1, 200, &msg, microsSinceEpoch(),
data.acc.raw_x, data.acc.raw_y, data.acc.raw_z,
data.gyro.raw_x,data.gyro.raw_y,data.gyro.raw_z,
0,0,0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = gcs_udp_send(sock, buf, len);
return(bytes_sent);
}
task_return_t imu_send_raw(imu_t* imu)
{
mavlink_message_t msg;
mavlink_msg_raw_imu_pack( imu->mavlink_stream->sysid,
imu->mavlink_stream->compid,
&msg,
time_keeper_get_micros(),
imu->oriented_accelero.data[IMU_X],
imu->oriented_accelero.data[IMU_Y],
imu->oriented_accelero.data[IMU_Z],
imu->oriented_gyro.data[IMU_X],
imu->oriented_gyro.data[IMU_Y],
imu->oriented_gyro.data[IMU_Z],
imu->oriented_compass.data[IMU_X],
imu->oriented_compass.data[IMU_Y],
imu->oriented_compass.data[IMU_Z]);
mavlink_stream_send(imu->mavlink_stream,&msg);
return TASK_RUN_SUCCESS;
}
