void MAVLinkProxy::heartbeat()
{
static mavlink_message_t msg;
static uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// System definition
static uint8_t system_type = MAV_TYPE_QUADROTOR;
static uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY;
uint8_t system_mode = MAV_MODE_PREFLIGHT;
uint32_t custom_mode = 0;
uint8_t system_state = MAV_STATE_STANDBY;
if(_connected)
{
system_mode = MAV_MODE_STABILIZE_DISARMED;
}
if(_flying)
{
system_mode = MAV_MODE_STABILIZE_ARMED;
system_state = MAV_STATE_ACTIVE;
}
mavlink_msg_heartbeat_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
mavlink_msg_sys_status_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg, &_sys_status);
mavlink_msg_to_send_buffer(buf, &msg);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
_heartbeat_timer.expires_from_now(boost::posix_time::milliseconds(MAVLINK_HEARTBEAT_INTERVAL));
_heartbeat_timer.async_wait(boost::bind(&MAVLinkProxy::heartbeat, this));
}
void Test::sendSystemStatus(uint8_t batteryPercentage, float batteryVoltage)
{
mavlink_sys_status_t msg;
msg.battery_remaining = batteryPercentage;
msg.voltage_battery = (uint16_t)(batteryVoltage * 1000);
msg.onboard_control_sensors_present = MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_3D_GYRO;
msg.onboard_control_sensors_enabled = MAV_SYS_STATUS_SENSOR_3D_MAG | MAV_SYS_STATUS_SENSOR_3D_GYRO;
msg.onboard_control_sensors_health = MAV_SYS_STATUS_SENSOR_3D_MAG;
mavlink_message_t buf;
mavlink_msg_sys_status_encode(SYSTEM_ID, COMPONENT_ID, &buf, &msg);
testMessage(buf);
}
void mavlink_connector::slow_send()
{
mavlink_gps_raw_int_t gps_raw_int_t;
mavlink_battery_status_t battery_status_t;
mavlink_rc_channels_t rc_channels_t;
mavlink_message_t msg;
gps_raw_int_t.lat = (int32_t)(dji_variable::global_position_degree.lati * 1e7) ;
gps_raw_int_t.lon = (int32_t)(dji_variable::global_position_degree.longti * 1e7);
gps_raw_int_t.alt = (int32_t)(dji_variable::global_position_degree.alti * 1000);
mavlink_msg_gps_raw_int_encode(0,200,&msg,&gps_raw_int_t);
send_msg(&msg);
mavlink_msg_heartbeat_encode(0,200,&msg,make_heartbeat());
send_msg(&msg);
// printf("sended heartbeating......\n");
battery_status_t.battery_remaining = dji_variable::battery;
for (int i = 0 ;i< 6;i++)
battery_status_t.voltages[i] = (uint16_t)((float)dji_variable::battery / 100.0f * 22.2 * 1000);
mavlink_msg_battery_status_encode(0,200,&msg,&battery_status_t);
send_msg(&msg);
mavlink_sys_status_t sys_status_t;
memset(&sys_status_t,0, sizeof(sys_status_t));
sys_status_t.battery_remaining = dji_variable::battery;
sys_status_t.voltage_battery = 222 * dji_variable::battery ;
mavlink_msg_sys_status_encode(0,200,&msg,&sys_status_t);
send_msg(&msg);
mavlink_rc_channels_scaled_t rc_channels_scaled_t;
rc_channels_scaled_t.chan1_scaled = dji_variable::rc_channels.roll;
rc_channels_scaled_t.chan2_scaled = dji_variable::rc_channels.pitch;
rc_channels_scaled_t.chan3_scaled = dji_variable::rc_channels.throttle;
rc_channels_scaled_t.chan4_scaled = dji_variable::rc_channels.yaw;
rc_channels_scaled_t.chan5_scaled = dji_variable::rc_channels.mode;
rc_channels_scaled_t.chan6_scaled = dji_variable::rc_channels.gear;
mavlink_msg_rc_channels_scaled_encode(0,200,&msg,&rc_channels_scaled_t);
send_msg(&msg);
}
