void Test::sendParameter(const char* parameterId, float value)
{
mavlink_param_value_t msg;
msg.param_count = 1;
strcpy(msg.param_id, parameterId);
msg.param_index = -1;
msg.param_value = value;
mavlink_message_t buf;
mavlink_msg_param_value_encode(SYSTEM_ID, COMPONENT_ID, &buf, &msg);
testMessage(buf);
}
/**
* The following functions are helper functions for reading the various parameters aboard the boat.
*/
void _transmitParameter(uint16_t id)
{
mavlink_message_t msg;
mavlink_param_union_t x;
mavlink_param_value_t valueMsg = {
0.0,
parameterCount,
0,
"",
MAV_PARAM_TYPE_UINT32
};
switch (id) {
case 0:
x.param_uint32 = (systemStatus.status & (1 << 0))?1:0;
valueMsg.param_value = x.param_float;
valueMsg.param_index = 0;
strncpy(valueMsg.param_id, "MODE_AUTO", 16);
break;
case 1:
x.param_uint32 = (systemStatus.status & (1 << 1))?1:0;
valueMsg.param_value = x.param_float;
valueMsg.param_index = 1;
strncpy(valueMsg.param_id, "MODE_HIL", 16);
break;
case 2:
x.param_uint32 = (systemStatus.status & (1 << 2))?1:0;
valueMsg.param_value = x.param_float;
valueMsg.param_index = 2;
strncpy(valueMsg.param_id, "MODE_HILSENSE", 16);
break;
case 3:
x.param_uint32 = (systemStatus.status & (1 << 3))?1:0;
valueMsg.param_value = x.param_float;
valueMsg.param_index = 3;
strncpy(valueMsg.param_id, "MODE_RCDISCON", 16);
break;
default:
return; // Do nothing if there's no matching parameter.
break;
}
mavlink_msg_param_value_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &valueMsg);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
//---------------------------------------------------------------------------------
//-- Send Parameter (Raw)
void
MavESP8266Component::_sendParameter(MavESP8266Bridge* sender, const char* id, uint32_t value, uint16_t index)
{
//-- Build message
mavlink_param_value_t msg;
msg.param_count = MavESP8266Parameters::ID_COUNT;
msg.param_index = index;
strncpy(msg.param_id, id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
memcpy(&msg.param_value, &value, sizeof(uint32_t));
msg.param_type = MAV_PARAM_TYPE_UINT32;
mavlink_message_t mmsg;
mavlink_msg_param_value_encode(
getWorld()->getVehicle()->systemID(),
MAV_COMP_ID_UDP_BRIDGE,
&mmsg,
&msg
);
sender->sendMessage(&mmsg);
}
void MAVLinkProxy::dataReceived(const boost::system::error_code &error, size_t received_bytes)
{
static mavlink_message_t msg;
static mavlink_status_t status;
// Response message and buffer
static mavlink_message_t msg_response;
static uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Waypoint reception variables
static int waypoints_to_receive = 0;
static uint16_t waypoints_received = 0;
static int waypoint_timeout = 200;
static unsigned long last_waypoint_time = 0;
for(int i = 0; i < (int) received_bytes; i++)
{
if(mavlink_parse_char(MAVLINK_COMM_0, _received_msg_buf[i], &msg, &status))
{
// Handle received messages
msg_response.magic = 0;
// Waypoint reception
if(waypoints_to_receive > 0 && (last_waypoint_time + waypoint_timeout <= (std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1))))
{
// Timeout
printf("Timeout receiving waypoints!\n");
waypoints_to_receive = 0;
last_waypoint_time = 0;
}
if(waypoints_to_receive > 0 && msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM)
{
// Got waypoint
waypoints_received++;
waypoints_to_receive--;
mavlink_mission_item_t waypoint;
mavlink_msg_mission_item_decode(&msg, &waypoint);
handleWaypoint(&waypoint);
last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);
if(waypoints_to_receive > 0)
{
mavlink_mission_request_t request;
request.seq = waypoints_received;
request.target_system = msg.sysid;
request.target_component = msg.compid;
mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request);
}
else
{
mavlink_mission_ack_t ack;
ack.target_system = msg.sysid;
ack.target_component = msg.compid;
ack.type = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_ack_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &ack);
processPartialMission();
}
}
else if(msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST)
{
printf("Waypoint count request\n");
uint16_t mission_length = 0;
mavlink_msg_mission_count_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, msg.sysid, msg.compid, mission_length);
}
else if(msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST)
{
printf("Parameter request\n");
mavlink_param_value_t param;
param.param_count = 0;
param.param_id[0] = '\0';
param.param_index = 0;
param.param_type = MAV_PARAM_TYPE_UINT8;
param.param_value = 0;
mavlink_msg_param_value_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, ¶m);
}
else if(msg.msgid == MAVLINK_MSG_ID_MISSION_COUNT)
{
mavlink_mission_count_t mission_count;
mavlink_msg_mission_count_decode(&msg, &mission_count);
printf("%d waypoints available\n", mission_count.count);
_mission_ready = false;
_partial_mission.clear();
waypoints_to_receive = mission_count.count;
waypoints_received = 0;
last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1);
mavlink_mission_request_t request;
request.seq = 0;
request.target_system = msg.sysid;
request.target_component = msg.compid;
mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request);
}
else if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// Heartbeat from MAVLink ground station
}
else
{
// Unhandled message type
printf("Unknown MAVLink packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
}
// Send a response if the message has been populated
if(msg_response.magic != 0)
{
mavlink_msg_to_send_buffer(buf, &msg_response);
_socket.send_to(boost::asio::buffer(buf), _endpoint);
}
}
}
// Listen for next packet
_socket.async_receive_from(boost::asio::buffer(_received_msg_buf), _endpoint, boost::bind(&MAVLinkProxy::dataReceived, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
}
