aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLINK: support CAN_FRAME and MAV_CMD_CAN_FORWARD

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-07 08:22:26 +11:00
parent b6f78ede2e
commit ba502b462f
2 changed files with 46 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/GCS_MAVLink/GCS.h
View File

@ -565,6 +565,13 @@ protected:
MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_long_t &packet);
/*
handle MAV_CMD_CAN_FORWARD and CAN_FRAME messages for CAN over MAVLink
*/
void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &);
MAV_RESULT handle_can_forward(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
void handle_can_frame(const mavlink_message_t &msg) const;
#if AP_OPTICALFLOW_ENABLED
void handle_optical_flow(const mavlink_message_t &msg);
#endif

41
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3399,6 +3399,28 @@ MAV_RESULT GCS_MAVLINK::handle_fixed_mag_cal_yaw(const mavlink_command_long_t &p
#endif
}
/*
handle MAV_CMD_CAN_FORWARD
*/
MAV_RESULT GCS_MAVLINK::handle_can_forward(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
#if HAL_CANMANAGER_ENABLED
return AP::can().handle_can_forward(chan, packet, msg) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
#else
return MAV_RESULT_UNSUPPORTED;
#endif
}
/*
handle CAN_FRAME messages
*/
void GCS_MAVLINK::handle_can_frame(const mavlink_message_t &msg) const
{
#if HAL_CANMANAGER_ENABLED
AP::can().handle_can_frame(msg);
#endif
}
void GCS_MAVLINK::handle_distance_sensor(const mavlink_message_t &msg)
{
RangeFinder *rangefinder = AP::rangefinder();
@ -3690,6 +3712,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_named_value(msg);
break;
case MAVLINK_MSG_ID_CAN_FRAME:
handle_can_frame(msg);
break;
}
}
@ -4451,7 +4477,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
case MAV_CMD_FIXED_MAG_CAL_YAW:
result = handle_fixed_mag_cal_yaw(packet);
break;
default:
result = MAV_RESULT_UNSUPPORTED;
break;
@ -4538,7 +4564,18 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
hal.util->persistent_data.last_mavlink_cmd = packet.command;
const MAV_RESULT result = handle_command_long_packet(packet);
MAV_RESULT result;
// special handling of messages that need the mavlink_message_t
switch (packet.command) {
case MAV_CMD_CAN_FORWARD:
result = handle_can_forward(packet, msg);
break;
default:
result = handle_command_long_packet(packet);
break;
}
// send ACK or NAK
mavlink_msg_command_ack_send(chan, packet.command, result,