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; }