diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c1f07fea17c828..7908d64ea92b00 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -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 @@ -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 { @@ -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) { /* @@ -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 @@ -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); } /* @@ -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; } @@ -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 @@ -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; @@ -4120,7 +4108,7 @@ 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); } @@ -4128,7 +4116,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_DATA96: handle_data_packet(msg); - break; + break; #if HAL_VISUALODOM_ENABLED case MAVLINK_MSG_ID_VISION_POSITION_DELTA: @@ -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; } @@ -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(); @@ -5654,7 +5642,7 @@ void GCS_MAVLINK::send_water_depth() const for (uint8_t i=0; inum_sensors(); i++) { const AP_RangeFinder_Backend *s = rangefinder->get_backend(i); - + if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { continue; } @@ -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; } @@ -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. @@ -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 */