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