diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0aed92bc73..c2996105c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -273,7 +273,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_GPS: handle_message_hil_gps(msg); @@ -285,7 +285,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - /* If we've received a valid message, mark the flag indicating so. + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } @@ -295,17 +295,18 @@ MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_c { /* evaluate if this system should accept this command */ bool target_ok = false; + switch (command) { - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: - /* broadcast and ignore component */ - target_ok = (target_system == 0) || (target_system == mavlink_system.sysid); - break; + case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: + /* broadcast and ignore component */ + target_ok = (target_system == 0) || (target_system == mavlink_system.sysid); + break; - default: - target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid) - || (target_component == MAV_COMP_ID_ALL)); - break; + default: + target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid) + || (target_component == MAV_COMP_ID_ALL)); + break; } return target_ok; @@ -330,6 +331,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) /* terminate other threads and this thread */ _mavlink->_task_should_exit = true; + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { /* send autopilot version message */ _mavlink->send_autopilot_capabilites(); @@ -346,22 +348,35 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + // XXX do proper translation vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; if (_cmd_pub == nullptr) { @@ -410,22 +425,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; if (_cmd_pub == nullptr) { @@ -446,7 +473,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) mavlink_msg_optical_flow_rad_decode(msg, &flow); enum Rotation flow_rot; - param_get(param_find("SENS_FLOW_ROT"),&flow_rot); + param_get(param_find("SENS_FLOW_ROT"), &flow_rot); struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -477,10 +504,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } /* Use distance value for distance sensor topic */ - struct distance_sensor_s d; - memset(&d, 0, sizeof(d)); + struct distance_sensor_s d = {}; - if(flow.distance > 0.0f) { // negative values signal invalid data + if (flow.distance > 0.0f) { // negative values signal invalid data d.timestamp = flow.integration_time_us * 1000; /* ms to us */ d.min_distance = 0.3f; d.max_distance = 5.0f; @@ -492,7 +518,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) if (_flow_distance_sensor_pub == nullptr) { _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); + } else { orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); } @@ -544,7 +571,8 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_hil_distance_sensor_pub == nullptr) { _hil_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); + } else { orb_publish(ORB_ID(distance_sensor), _hil_distance_sensor_pub, &d); } @@ -607,7 +635,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) if (_distance_sensor_pub == nullptr) { _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); @@ -669,10 +697,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0) && - values_finite) { + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0) && + values_finite) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -704,24 +732,29 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } + if (_control_mode.flag_control_offboard_enabled) { if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = set_position_target_local_ned.afx; force_sp.y = set_position_target_local_ned.afy; force_sp.z = set_position_target_local_ned.afz; + //XXX: yaw if (_force_sp_pub == nullptr) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; @@ -751,6 +784,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { pos_sp_triplet.current.position_valid = false; } @@ -761,6 +795,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -773,7 +808,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; pos_sp_triplet.current.acceleration_is_force = - is_force_sp; + is_force_sp; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -796,14 +831,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) if (_pos_sp_triplet_pub == nullptr) { _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); + &pos_sp_triplet); + } else { orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + &pos_sp_triplet); } } @@ -837,10 +874,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m PX4_ISFINITE(set_actuator_control_target.controls[7]); if ((mavlink_system.sysid == set_actuator_control_target.target_system || - set_actuator_control_target.target_system == 0) && - (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0) && - values_finite) { + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0) && + values_finite) { /* ignore all since we are setting raw actuators here */ offboard_control_mode.ignore_thrust = true; @@ -854,6 +891,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m if (_offboard_control_mode_pub == nullptr) { _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + } else { orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } @@ -862,6 +900,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m /* If we are in offboard control mode, publish the actuator controls */ bool updated; orb_check(_control_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } @@ -871,12 +910,13 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); /* Set duty cycles for the servos in actuator_controls_0 */ - for(size_t i = 0; i < 8; i++) { + for (size_t i = 0; i < 8; i++) { actuator_controls.control[i] = set_actuator_control_target.controls[i]; } if (_actuator_controls_pub == nullptr) { _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); } @@ -942,10 +982,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0) && - values_finite) { + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0) && + values_finite) { /* set correct ignore flags for thrust field: copy from mavlink message */ _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); @@ -969,6 +1009,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg && _offboard_control_mode.ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; + } else { _offboard_control_mode.ignore_bodyrate = ignore_bodyrate_msg; _offboard_control_mode.ignore_attitude = ignore_attitude_msg; @@ -992,6 +1033,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } @@ -1001,9 +1043,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(_offboard_control_mode.ignore_attitude)) { _att_sp.timestamp = hrt_absolute_time(); + if (!ignore_attitude_msg) { // only copy att sp if message contained new data mavlink_quaternion_to_euler(set_attitude_target.q, - &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body); + &_att_sp.roll_body, &_att_sp.pitch_body, &_att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body); _att_sp.R_valid = true; _att_sp.yaw_sp_move_rate = 0.0; @@ -1017,6 +1060,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_att_sp_pub == nullptr) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } else { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } @@ -1026,17 +1070,20 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) ///XXX add support for ignoring individual axes if (!(_offboard_control_mode.ignore_bodyrate)) { _rates_sp.timestamp = hrt_absolute_time(); + if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data _rates_sp.roll = set_attitude_target.body_roll_rate; _rates_sp.pitch = set_attitude_target.body_pitch_rate; _rates_sp.yaw = set_attitude_target.body_yaw_rate; } + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid _rates_sp.thrust = set_attitude_target.thrust; } if (_rates_sp_pub == nullptr) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } else { orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); } @@ -1105,6 +1152,7 @@ MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) if (!on && (on != last_on)) { _mom_switch_pos[sw]++; + if (_mom_switch_pos[sw] == state_count) { _mom_switch_pos[sw] = 0; } @@ -1135,26 +1183,42 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } struct rc_input_values rc = {}; + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; /* channels */ rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; if (_rc_pub == nullptr) { @@ -1203,6 +1267,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); unsigned max_channels = (sizeof(rc.values) / sizeof(rc.values[0])); + if (max_switch > (max_channels - 4)) { max_switch = (max_channels - 4); } @@ -1211,6 +1276,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) for (unsigned i = 0; i < max_switch; i++) { rc.values[i + 4] = decode_switch_pos_n(man.buttons, i); } + _mom_switch_state = man.buttons; if (_rc_pub == nullptr) { @@ -1272,9 +1338,10 @@ void MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) { mavlink_ping_t ping; - mavlink_msg_ping_decode( msg, &ping); + mavlink_msg_ping_decode(msg, &ping); + if ((mavlink_system.sysid == ping.target_system) && - (mavlink_system.compid == ping.target_component)) { + (mavlink_system.compid == ping.target_component)) { mavlink_message_t msg_out; mavlink_msg_ping_encode(_mavlink->get_system_id(), _mavlink->get_component_id(), &msg_out, &ping); _mavlink->send_message(MAVLINK_MSG_ID_PING, &msg_out); @@ -1287,7 +1354,8 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) mavlink_request_data_stream_t req; mavlink_msg_request_data_stream_decode(msg, &req); - if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid && req.req_message_rate != 0) { + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid + && req.req_message_rate != 0) { float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; MavlinkStream *stream; @@ -1315,11 +1383,12 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) if (!onb_unix_valid && ofb_unix_valid) { tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - if(px4_clock_settime(CLOCK_REALTIME, &tv)) { + + if (px4_clock_settime(CLOCK_REALTIME, &tv)) { warn("failed setting clock"); - } - else { - warnx("[timesync] UTC time synced."); + + } else { + warnx("[timesync] UTC time synced."); } } @@ -1349,12 +1418,13 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1 * 2) / 2 ; int64_t dt = _time_offset - offset_ns; if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = offset_ns; warnx("[timesync] Hard setting offset."); + } else { smooth_time_offset(offset_ns); } @@ -1382,17 +1452,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) float dt; if (_hil_last_frame == 0 || - (timestamp <= _hil_last_frame) || - (timestamp - _hil_last_frame) > (0.1f * 1e6f) || - (_hil_last_frame >= timestamp)) { + (timestamp <= _hil_last_frame) || + (timestamp - _hil_last_frame) > (0.1f * 1e6f) || + (_hil_last_frame >= timestamp)) { dt = 0.01f; /* default to 100 Hz */ memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro)); _hil_prev_accel[0] = 0.0f; _hil_prev_accel[1] = 0.0f; _hil_prev_accel[2] = 9.866f; + } else { dt = (timestamp - _hil_last_frame) / 1e6f; } + _hil_last_frame = timestamp; /* airspeed */ @@ -1645,12 +1717,13 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) follow_target_topic.timestamp = hrt_absolute_time(); - follow_target_topic.lat = follow_target_msg.lat*1e-7; - follow_target_topic.lon = follow_target_msg.lon*1e-7; + follow_target_topic.lat = follow_target_msg.lat * 1e-7; + follow_target_topic.lon = follow_target_msg.lon * 1e-7; follow_target_topic.alt = follow_target_msg.alt; if (_follow_target_pub == nullptr) { _follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); + } else { orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic); } @@ -1666,8 +1739,8 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) t.timestamp = hrt_absolute_time(); t.ICAO_address = adsb.ICAO_address; - t.lat = adsb.lat*1e-7; - t.lon = adsb.lon*1e-7; + t.lat = adsb.lat * 1e-7; + t.lon = adsb.lon * 1e-7; t.altitude_type = adsb.altitude_type; t.altitude = adsb.altitude / 1000.0f; t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F; @@ -1683,6 +1756,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) if (_transponder_report_pub == nullptr) { _transponder_report_pub = orb_advertise(ORB_ID(transponder_report), &t); + } else { orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); } @@ -1704,7 +1778,7 @@ void MavlinkReceiver::handle_message_gps_inject_data(mavlink_message_t *msg) if (pub == nullptr) { int idx = _gps_inject_data_next_idx; pub = orb_advertise_multi(ORB_ID(gps_inject_data), &gps_inject_data_topic, - &idx, ORB_PRIO_DEFAULT); + &idx, ORB_PRIO_DEFAULT); } else { orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic); @@ -1840,7 +1914,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* land detector */ { bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - if(hil_land_detector.landed != landed) { + + if (hil_land_detector.landed != landed) { hil_land_detector.landed = landed; hil_land_detector.timestamp = hrt_absolute_time(); @@ -1927,6 +2002,7 @@ MavlinkReceiver::receive_thread(void *arg) fds[0].fd = uart_fd; fds[0].events = POLLIN; } + #ifdef __PX4_POSIX struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); @@ -1960,31 +2036,38 @@ MavlinkReceiver::receive_thread(void *arg) usleep(sleeptime); } } + #ifdef __PX4_POSIX + if (_mavlink->get_protocol() == UDP) { if (fds[0].revents & POLLIN) { nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); } + } else { // could be TCP or other protocol } - struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); + struct sockaddr_in *srcaddr_last = _mavlink->get_client_source_address(); + int localhost = (127 << 24) + 1; + if (!_mavlink->get_client_source_initialized()) { // set the address either if localhost or if 3 seconds have passed // this ensures that a GCS running on localhost can get a hold of // the system within the first N seconds hrt_abstime stime = _mavlink->get_start_time(); + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000)) - || (srcaddr_last->sin_addr.s_addr == htonl(localhost))) { + || (srcaddr_last->sin_addr.s_addr == htonl(localhost))) { srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last->sin_port = srcaddr.sin_port; _mavlink->set_client_source_initialized(); warnx("changing partner IP to: %s", inet_ntoa(srcaddr.sin_addr)); } } + #endif // only start accepting messages once we're sure who we talk to @@ -2018,21 +2101,23 @@ void MavlinkReceiver::print_status() uint64_t MavlinkReceiver::sync_stamp(uint64_t usec) { - if(_time_offset > 0) + if (_time_offset > 0) { return usec - (_time_offset / 1000) ; - else + + } else { return hrt_absolute_time(); + } } void MavlinkReceiver::smooth_time_offset(int64_t offset_ns) { /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, - * the faster the moving average updates in response to + * the faster the moving average updates in response to * new offset samples. */ - _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; } @@ -2041,7 +2126,7 @@ void *MavlinkReceiver::start_helper(void *context) MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - void* ret = rcv->receive_thread(NULL); + void *ret = rcv->receive_thread(nullptr); delete rcv;