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 }
task_return_t imu_send_scaled(imu_t* imu) { mavlink_message_t msg; mavlink_msg_scaled_imu_pack(imu->mavlink_stream->sysid, imu->mavlink_stream->compid, &msg, time_keeper_get_millis(), 1000 * imu->scaled_accelero.data[IMU_X], 1000 * imu->scaled_accelero.data[IMU_Y], 1000 * imu->scaled_accelero.data[IMU_Z], 1000 * imu->scaled_gyro.data[IMU_X], 1000 * imu->scaled_gyro.data[IMU_Y], 1000 * imu->scaled_gyro.data[IMU_Z], 1000 * imu->scaled_compass.data[IMU_X], 1000 * imu->scaled_compass.data[IMU_Y], 1000 * imu->scaled_compass.data[IMU_Z]); mavlink_stream_send(imu->mavlink_stream,&msg); return TASK_RUN_SUCCESS; }