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
}
/*-----------------------------------------------------------------------------
* Service MAVlink protocol message routine
*/
void Timer1_IRQHandler() {
mavlink_message_t msg;
MadgwickAHRSupdateIMU(
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val);
mavlink_msg_heartbeat_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_TYPE_GROUND_ROVER,
MAV_AUTOPILOT_GENERIC,
mavlink_sys_mode,
0,
mavlink_sys_state);
mavlink_send_msg(&msg);
mavlink_msg_sys_status_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG,
MAV_SYS_STATUS_SENSOR_3D_GYRO
| MAV_SYS_STATUS_SENSOR_3D_ACCEL
| MAV_SYS_STATUS_SENSOR_3D_MAG
| MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE
| MAV_SYS_STATUS_SENSOR_GPS,
50, // TODO remove hardoded values
0,
-1,
-1,
0,
0,
0,
0,
0,
0);
mavlink_send_msg(&msg);
mavlink_msg_autopilot_version_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
0, // No capabilities (MAV_PROTOCOL_CAPABILITY). TBD
42,
42,
42,
42,
"DEADBEEF",
"DEADBEEF",
"DEADBEEF",
42,
42,
42);
mavlink_send_msg(&msg);
mavlink_msg_highres_imu_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
params[PARAM_AX].val,
params[PARAM_AY].val,
params[PARAM_AZ].val,
params[PARAM_GX].val,
params[PARAM_GY].val,
params[PARAM_GZ].val,
params[PARAM_MX].val,
params[PARAM_MY].val,
params[PARAM_MZ].val,
0,
0,
0,
params[PARAM_T].val,
(1 << 12) | ((1 << 9) - 1));
mavlink_send_msg(&msg);
mavlink_msg_attitude_quaternion_pack(
mavlink_system.sysid,
mavlink_system.compid,
&msg,
usec(),
q0,
q1,
q2,
q3,
DEG_TO_RAD(params[PARAM_GX].val),
DEG_TO_RAD(params[PARAM_GY].val),
DEG_TO_RAD(params[PARAM_GZ].val));
mavlink_send_msg(&msg);
usec_service_routine();
MSS_TIM1_clear_irq();
}
