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