Skip to content

Commit

Permalink
GCS_MAVLink: Remove wrong usage of COMMAND_ACK message
Browse files Browse the repository at this point in the history 
SET_MODE message does not exist inside the MAV_CMD enum
as described in the mavlink specification.
The system that is using SET_MODE to communicate with the
vehicle should rely on HEARTBEAT message to detect if
the mode was set correctly.

Signed-off-by: Patrick José Pereira <[email protected]>
  • Loading branch information
@patrickelectric
patrickelectric committed Feb 1, 2024
1 parent cd1e127 commit 7d84a2f
Showing 1 changed file with 16 additions and 28 deletions.
44 changes: 16 additions & 28 deletions libraries/GCS_MAVLink/GCS_Common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ bool GCS_MAVLINK::init(uint8_t instance)
}
// since tcdrain() and TCSADRAIN may not be implemented...
hal.scheduler->delay(1);

_port->set_flow_control(old_flow_control);

// now change back to desired baudrate
Expand Down Expand Up @@ -328,7 +328,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const

float current, consumed_mah, consumed_wh;
const int8_t percentage = battery_remaining_pct(instance);

if (battery.current_amps(current, instance)) {
current = constrain_float(current * 100,-INT16_MAX,INT16_MAX);
} else {
Expand Down Expand Up @@ -1787,7 +1787,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
{
const uint8_t c = (uint8_t)_port->read();
const uint32_t protocol_timeout = 4000;

if (alternative.handler &&
now_ms - alternative.last_mavlink_ms > protocol_timeout) {
/*
Expand All @@ -1799,7 +1799,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us)
alternative.last_alternate_ms = now_ms;
gcs_alternative_active[chan] = true;
}

/*
we may also try parsing as MAVLink if we haven't had a
successful parse on the alternative protocol for 4s
Expand Down Expand Up @@ -2600,19 +2600,7 @@ void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg)
const MAV_MODE _base_mode = (MAV_MODE)packet.base_mode;
const uint32_t _custom_mode = packet.custom_mode;

const MAV_RESULT result = _set_mode_common(_base_mode, _custom_mode);

// send ACK or NAK. Note that this is extraodinarily improper -
// we are sending a command-ack for a message which is not a
// command. The command we are acking (ID=11) doesn't actually
// exist, but if it did we'd probably be acking something
// completely unrelated to setting modes.
if (HAVE_PAYLOAD_SPACE(chan, COMMAND_ACK)) {
mavlink_msg_command_ack_send(chan, MAVLINK_MSG_ID_SET_MODE, result,
0, 0,
msg.sysid,
msg.compid);
}
_set_mode_common(_base_mode, _custom_mode);
}

/*
Expand Down Expand Up @@ -3150,7 +3138,7 @@ float GCS_MAVLINK::vfr_hud_climbrate() const
{
#if AP_AHRS_ENABLED
Vector3f velned;
if (AP::ahrs().get_velocity_NED(velned) ||
if (AP::ahrs().get_velocity_NED(velned) ||
AP::ahrs().get_vert_pos_rate_D(velned.z)) {
return -velned.z;
}
Expand Down Expand Up @@ -3183,7 +3171,7 @@ void GCS_MAVLINK::send_vfr_hud()
}

/*
handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command
handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command
Optionally disable PX4IO overrides. This is done for quadplanes to
prevent the mixer running while rebooting which can start the VTOL
Expand Down Expand Up @@ -3731,7 +3719,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)

// correct offboard timestamp to be in local ms since boot
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP));

AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
Expand Down Expand Up @@ -4120,15 +4108,15 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
#endif

case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
// only pass if override is not selected
// only pass if override is not selected
if (!(_port->get_options() & _port->OPTION_NOSTREAMOVERRIDE)) {
handle_request_data_stream(msg);
}
break;

case MAVLINK_MSG_ID_DATA96:
handle_data_packet(msg);
break;
break;

#if HAL_VISUALODOM_ENABLED
case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
Expand Down Expand Up @@ -4514,7 +4502,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm
AP::compass().force_save_calibration();
ret = MAV_RESULT_ACCEPTED;
}

return ret;
}

Expand Down Expand Up @@ -5645,7 +5633,7 @@ void GCS_MAVLINK::send_water_depth() const

if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){
return;
}
}

// get position
const AP_AHRS &ahrs = AP::ahrs();
Expand All @@ -5654,7 +5642,7 @@ void GCS_MAVLINK::send_water_depth() const

for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);

if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
Expand Down Expand Up @@ -6709,7 +6697,7 @@ uint64_t GCS_MAVLINK::capabilities() const
if (!AP_BoardConfig::ftp_disabled()){ //if ftp disable board option is not set
ret |= MAV_PROTOCOL_CAPABILITY_FTP;
}

return ret;
}

Expand Down Expand Up @@ -6829,7 +6817,7 @@ void GCS_MAVLINK::send_high_latency2() const

//send_text(MAV_SEVERITY_INFO, "Yaw: %u", (((uint16_t)ahrs.yaw_sensor / 100) % 360));

mavlink_msg_high_latency2_send(chan,
mavlink_msg_high_latency2_send(chan,
AP_HAL::millis(), //[ms] Timestamp (milliseconds since boot or Unix epoch)
gcs().frame_type(), // Type of the MAV (quadrotor, helicopter, etc.)
MAV_AUTOPILOT_ARDUPILOTMEGA, // Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers.
Expand Down Expand Up @@ -6879,7 +6867,7 @@ int8_t GCS_MAVLINK::high_latency_air_temperature() const
}

/*
handle a MAV_CMD_CONTROL_HIGH_LATENCY command
handle a MAV_CMD_CONTROL_HIGH_LATENCY command
Enable or disable any marked (via SERIALn_PROTOCOL) high latency connections
*/
Expand Down

0 comments on commit 7d84a2f

Please sign in to comment.