diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index cfaa54b56d..835f678cd2 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -522,6 +522,8 @@ void Morse::update(const struct sitl_input &input) } report_FPS(); + + send_report(); } @@ -543,3 +545,85 @@ void Morse::report_FPS(void) last_frame_count_s = state.timestamp; } } + + + +/* + send a report to the vehicle control code over MAVLink +*/ +void Morse::send_report(void) +{ + const uint32_t now = AP_HAL::millis(); +#if defined(__CYGWIN__) || defined(__CYGWIN64__) + if (now < 10000) { + // don't send lidar reports until 10s after startup. This + // avoids a windows threading issue with non-blocking sockets + // and the initial wait on uartA + return; + } +#endif + + // this is usually loopback + if (!mavlink.connected && mav_socket.connect(mavlink_loopback_address, mavlink_loopback_port)) { + ::printf("Morse MAVLink loopback connected to %s:%u\n", mavlink_loopback_address, (unsigned)mavlink_loopback_port); + mavlink.connected = true; + } + if (!mavlink.connected) { + return; + } + + // send a OBSTACLE_DISTANCE messages at 15 Hz + if (now - send_report_last_ms >= (1000/15) && scanner.points.length == scanner.ranges.length && scanner.points.length > 0) { + send_report_last_ms = now; + + mavlink_obstacle_distance_t packet {}; + packet.time_usec = AP_HAL::micros64(); + packet.min_distance = 1; + packet.max_distance = 0; + packet.sensor_type = MAV_DISTANCE_SENSOR_LASER; + packet.increment = 0; // use increment_f + + packet.angle_offset = 180; + packet.increment_f = (-5); // NOTE! This is negative because the distances[] arc is counter-clockwise + + for (uint8_t i=0; i= scanner.points.length) { + continue; + } + + // convert m to cm and sanity check + const Vector2f v = Vector2f(scanner.points.data[i].x, scanner.points.data[i].y); + const float distance_cm = v.length()*100; + if (distance_cm < packet.min_distance || distance_cm >= 65535) { + continue; + } + + packet.distances[i] = distance_cm; + const float max_cm = scanner.ranges.data[i] * 100.0; + if (packet.max_distance < max_cm && max_cm > 0 && max_cm < 65535) { + packet.max_distance = max_cm; + } + } + + mavlink_message_t msg; + mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); + uint8_t saved_seq = chan0_status->current_tx_seq; + chan0_status->current_tx_seq = mavlink.seq; + uint16_t len = mavlink_msg_obstacle_distance_encode( + 0, + 0, + &msg, &packet); + chan0_status->current_tx_seq = saved_seq; + + uint8_t msgbuf[len]; + len = mavlink_msg_to_send_buffer(msgbuf, &msg); + if (len > 0) { + mav_socket.send(msgbuf, len); + } + } + +} diff --git a/libraries/SITL/SIM_Morse.h b/libraries/SITL/SIM_Morse.h index 0a453b75c2..e65a36832d 100644 --- a/libraries/SITL/SIM_Morse.h +++ b/libraries/SITL/SIM_Morse.h @@ -39,6 +39,22 @@ public: } private: + + // loopback to convert inbound Morse lidar data into inbound mavlink msgs + const char *mavlink_loopback_address = "127.0.0.1"; + const uint16_t mavlink_loopback_port = 5762; + SocketAPM mav_socket { false }; + struct { + // socket to telem2 on aircraft + bool connected; + mavlink_message_t rxmsg; + mavlink_status_t status; + uint8_t seq; + } mavlink {}; + + void send_report(void); + uint32_t send_report_last_ms; + const char *morse_ip = "127.0.0.1"; // assume sensors are streamed on port 60000 diff --git a/libraries/SITL/examples/Morse/rover_scanner.parm b/libraries/SITL/examples/Morse/rover_scanner.parm index 00e3ae0743..dec2bcbd9b 100644 --- a/libraries/SITL/examples/Morse/rover_scanner.parm +++ b/libraries/SITL/examples/Morse/rover_scanner.parm @@ -4,5 +4,8 @@ SERVO1_FUNCTION 73 SERVO3_FUNCTION 74 # setup laser scanner -PRX_TYPE 11 +PRX_TYPE 2 AVOID_ENABLE 7 +AVOID_MARGIN 0.5 +OA_MARGIN_MAX 0.6 +OA_TYPE 1