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