AP_Radar: Add iNav Radar functionality

Add Formation Flight (based on Inav Radar) code

ArduPlane/ArduPlane.cpp

@@ -132,6 +132,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if HAL_BUTTON_ENABLED
     SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
 #endif
+#if AP_RADAR_ENABLED
+    SCHED_TASK_CLASS(AP_Radar, &plane.radar, update,    100,    25,  152),
+#endif
 #if STATS_ENABLED == ENABLED
     SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153),
 #endif

ArduPlane/Parameters.cpp

@@ -946,6 +946,12 @@ const AP_Param::Info Plane::var_info[] = {
     GOBJECT(optflow,   "FLOW", AP_OpticalFlow),
 #endif
 
+#if AP_RADAR_ENABLED
+    // @Group: RADAR
+    // @Path: ../libraries/AP_Radar/AP_Radar.cpp
+    GOBJECT(radar,   "RADAR", AP_Radar),
+#endif
+
     // @Group: MIS_
     // @Path: ../libraries/AP_Mission/AP_Mission.cpp
     GOBJECT(mission, "MIS_",       AP_Mission),

ArduPlane/Parameters.h

@@ -357,6 +357,8 @@ class Parameters {
         k_param_acro_yaw_rate,
         k_param_takeoff_throttle_max_t,
         k_param_autotune_options,
+
+        k_param_radar,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -77,6 +77,7 @@
 #include <AP_Rally/AP_Rally.h>
 
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
+#include <AP_Radar/AP_Radar.h>     // iNav Radar
 #include <AP_Parachute/AP_Parachute.h>
 #include <AP_ADSB/AP_ADSB.h>
 #include <AP_ICEngine/AP_ICEngine.h>
@@ -265,6 +266,11 @@ class Plane : public AP_Vehicle {
     void precland_update(void);
 #endif
 
+#if AP_RADAR_ENABLED
+    // iNav Radar
+    AP_Radar radar;
+#endif
+
     // returns a Location for a rally point or home; if
     // HAL_RALLY_ENABLED is false, just home.
     Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt_amsl_cm) const;

ArduPlane/system.cpp

@@ -148,6 +148,11 @@ void Plane::init_ardupilot()
         optflow.init(-1);
     }
 #endif
+#if AP_RADAR_ENABLED
+    if (radar.enabled()) {
+        radar.init(-1);
+    }
+#endif
 
 // init cargo gripper
 #if AP_GRIPPER_ENABLED

Tools/ardupilotwaf/ardupilotwaf.py

@@ -44,6 +44,7 @@
     'AP_Notify',
     'AP_OpticalFlow',
     'AP_Param',
+    'AP_Radar',
     'AP_Rally',
     'AP_RangeFinder',
     'AP_Scheduler',

Tools/scripts/build_options.py

@@ -84,11 +84,12 @@ def __init__(self,
     Feature('Notify', 'NOTIFY_NEOPIXEL', 'AP_NOTIFY_NEOPIXEL_ENABLED', 'Enable NeoPixel', 0, None),  # noqa
 
     Feature('MSP', 'MSP', 'HAL_MSP_ENABLED', 'Enable MSP Telemetry and MSP OSD', 0, 'OSD'),
-    Feature('MSP', 'MSP_SENSORS', 'HAL_MSP_SENSORS_ENABLED', 'Enable MSP Sensors', 0, 'MSP_GPS,MSP_BARO,MSP_COMPASS,MSP_AIRSPEED,MSP,MSP_OPTICALFLOW,MSP_RANGEFINDER,OSD'),   # NOQA: E501
+    Feature('MSP', 'MSP_SENSORS', 'HAL_MSP_SENSORS_ENABLED', 'Enable MSP Sensors', 0, 'MSP_GPS,MSP_BARO,MSP_COMPASS,MSP_AIRSPEED,MSP,MSP_OPTICALFLOW,MSP_RANGEFINDER,MSP_RADAR,OSD'),   # NOQA: E501
     Feature('MSP', 'MSP_GPS', 'HAL_MSP_GPS_ENABLED', 'Enable MSP GPS', 0, 'MSP,OSD'),
     Feature('MSP', 'MSP_COMPASS', 'AP_COMPASS_MSP_ENABLED', 'Enable MSP Compass', 0, 'MSP,OSD'),
     Feature('MSP', 'MSP_OPTICALFLOW', 'HAL_MSP_OPTICALFLOW_ENABLED', 'Enable MSP OpticalFlow', 0, 'MSP,OSD,OPTICALFLOW'), # also OPTFLOW dep   # NOQA: E501
     Feature('MSP', 'MSP_RANGEFINDER', 'HAL_MSP_RANGEFINDER_ENABLED', 'Enable MSP Rangefinder', 0, 'MSP,OSD,RANGEFINDER'),
+    Feature('MSP', 'MSP_RADAR', 'HAL_MSP_RADAR_ENABLED', 'Enable MSP iNav Radar', 0, 'MSP,OSD,RADAR'),   # NOQA: E501
     Feature('MSP', 'MSP_DISPLAYPORT', 'HAL_WITH_MSP_DISPLAYPORT', 'Enable MSP DisplayPort OSD (aka CANVAS MODE)', 0, 'MSP,OSD'),   # NOQA: E501
 
     Feature('ICE', 'ICE Engine', 'AP_ICENGINE_ENABLED', 'Enable Internal Combustion Engine support', 0, 'RPM'),
@@ -284,6 +285,7 @@ def __init__(self,
     Feature('Baro', 'BARO_WIND_COMP', 'HAL_BARO_WIND_COMP_ENABLED', 'Enable Baro Wind Compensation', 0, None),
     Feature('Baro', 'BARO_TEMPCAL', 'AP_TEMPCALIBRATION_ENABLED', 'Enable Baro Temperature Calibration', 0, None),
 
+    Feature('Sensors', 'RADAR', 'AP_RADAR_ENABLED', 'Enable iNav Radar', 0, None),
     Feature('Sensors', 'RPM', 'AP_RPM_ENABLED', 'Enable RPM sensors', 0, None),
     Feature('Sensors', 'RPM_EFI', 'AP_RPM_EFI_ENABLED', 'Enable RPM EFI sensors', 0, 'RPM,EFI'),
     Feature('Sensors', 'RPM_ESC_TELEM', 'AP_RPM_ESC_TELEM_ENABLED', 'Enable RPM ESC Telemetry sensors', 0, 'RPM'),

libraries/AP_MSP/AP_MSP_Telem_Backend.cpp

@@ -24,6 +24,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_OpticalFlow/AP_OpticalFlow.h>
+#include <AP_Radar/AP_Radar.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_RCMapper/AP_RCMapper.h>
 #include <AP_RSSI/AP_RSSI.h>
@@ -421,7 +422,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_command(msp_packet_t *cmd, ms
     // initialize reply by default
     reply->cmd = cmd->cmd;
 
-    if (MSP2_IS_SENSOR_MESSAGE(cmd_msp)) {
+    if (MSP2_IS_SENSOR_MESSAGE(cmd_msp) || cmd_msp == MSP2_SET_RADAR_POS) {
         ret = msp_process_sensor_command(cmd_msp, src);
     } else {
         ret = msp_process_out_command(cmd_msp, dst);
@@ -519,11 +520,28 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_m
         msp_handle_airspeed(*pkt);
     }
     break;
+    case MSP2_SET_RADAR_POS: {
+        const MSP::msp_radar_pos_message_t *pkt = (const MSP::msp_radar_pos_message_t *)src->ptr;
+        msp_handle_radar(*pkt);
+    }
+    break;
     }
 
     return MSP_RESULT_NO_REPLY;
 }
 
+void AP_MSP_Telem_Backend::msp_handle_radar(const MSP::msp_radar_pos_message_t &pkt)
+{
+#if HAL_MSP_RADAR_ENABLED
+    AP_Radar *radar = AP::radar();
+    if (radar == nullptr) {
+        return;
+    }
+    radar->handle_msp(pkt);
+#endif
+}
+
+
 void AP_MSP_Telem_Backend::msp_handle_opflow(const MSP::msp_opflow_data_message_t &pkt)
 {
 #if HAL_MSP_OPTICALFLOW_ENABLED

libraries/AP_MSP/AP_MSP_Telem_Backend.h

@@ -185,6 +185,7 @@ friend AP_MSP;
     void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt);
     void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt);
     void msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt);
+    void msp_handle_radar(const MSP::msp_radar_pos_message_t &pkt);
 
     // implementation specific helpers
     // we only set arming status

libraries/AP_MSP/msp.h

@@ -129,6 +129,18 @@ typedef enum : uint8_t {
     MSP_BATTERY_INIT
 } battery_state_e;
 
+// inav radar
+typedef struct PACKED {
+    uint8_t radar_no;
+    uint8_t state;
+    uint32_t lat;
+    uint32_t lon;
+    uint32_t alt;
+    uint16_t heading;
+    uint16_t speed;
+    uint8_t lq;
+} msp_radar_pos_message_t;
+
 uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len);
 uint32_t msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len);
 uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request=false);

libraries/AP_MSP/msp_protocol.h

@@ -350,3 +350,7 @@
 #define MSP2_SENSOR_COMPASS         0x1F04
 #define MSP2_SENSOR_BAROMETER       0x1F05
 #define MSP2_SENSOR_AIRSPEED        0x1F06
+
+// ESP32 radar commands
+#define MSP2_SET_RADAR_POS       0x100B //SET radar position information
+#define MSP2_SET_RADAR_ITD       0x100C //SET radar information to display

libraries/AP_OSD/AP_OSD.h

@@ -221,6 +221,7 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen
     AP_OSD_Setting hgt_abvterr{false, 23, 7};
     AP_OSD_Setting fence{false, 14, 9};
     AP_OSD_Setting rngf;
+    AP_OSD_Setting radar;
 #if HAL_PLUSCODE_ENABLE
     AP_OSD_Setting pluscode;
 #endif
@@ -259,6 +260,7 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen
     void draw_gspeed(uint8_t x, uint8_t y);
     void draw_horizon(uint8_t x, uint8_t y);
     void draw_home(uint8_t x, uint8_t y);
+    void draw_radar(uint8_t x, uint8_t y);
     void draw_throttle(uint8_t x, uint8_t y);
     void draw_heading(uint8_t x, uint8_t y);
 #if AP_RPM_ENABLED
@@ -280,6 +282,7 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen
     void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude);
     void draw_distance(uint8_t x, uint8_t y, float distance);
     char get_arrow_font_index (int32_t angle_cd);
+    void draw_vdistance(uint8_t x, uint8_t y, float distance);
 #if HAL_WITH_ESC_TELEM
     void draw_esc_temp(uint8_t x, uint8_t y);
     void draw_esc_rpm(uint8_t x, uint8_t y);

libraries/AP_OSD/AP_OSD_Screen.cpp

@@ -36,6 +36,7 @@
 #include <AP_Common/Location.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_GPS/AP_GPS.h>
+#include <AP_Radar/AP_Radar.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_MSP/msp.h>
 #include <AP_OLC/AP_OLC.h>
@@ -1038,6 +1039,21 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
 	// @Range: 0 15
 	AP_SUBGROUPINFO(rrpm, "RPM", 62, AP_OSD_Screen, AP_OSD_Setting),
 #endif
+    // @Param: RADAR_EN
+    // @DisplayName: RADAR_EN
+    // @Description: Displays iNav Radar info for peer aircraft
+    // @Values: 0:Disabled,1:Enabled
+
+    // @Param: RADAR_X
+    // @DisplayName: RADAR_X
+    // @Description: Horizontal position on screen
+    // @Range: 0 29
+
+    // @Param: RADAR_Y
+    // @DisplayName: RADAR_Y
+    // @Description: Vertical position on screen
+    // @Range: 0 15
+    AP_SUBGROUPINFO(radar, "RADAR", 63, AP_OSD_Screen, AP_OSD_Setting),
 
     AP_GROUPEND
 };
@@ -1679,6 +1695,40 @@ void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
     }
 }
 
+void AP_OSD_Screen::draw_radar(uint8_t x, uint8_t y)
+{
+    static uint8_t id = 0;
+    static uint32_t last_peer_change = 0;
+    AP_AHRS &ahrs = AP::ahrs();
+    AP_Radar *ap_radar = AP_Radar::get_singleton();
+    WITH_SEMAPHORE(ahrs.get_semaphore());
+    Location loc;
+    if (ahrs.get_location(loc) && ap_radar->get_peer_healthy(id)) {
+        const Location &peer_loc = ap_radar->get_peer(id).location;
+        float distance = loc.get_distance(peer_loc);
+        ftype vertical_distance;
+        if (!peer_loc.get_alt_distance(loc, vertical_distance)) {
+            vertical_distance = 0.0f;
+        }
+        int32_t angle = wrap_360_cd(loc.get_bearing_to(peer_loc) - ahrs.yaw_sensor);
+        int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
+        if (distance < 2.0f) {
+            //avoid fast rotating arrow at small distances
+            angle = 0;
+        }
+        char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
+        backend->write(x, y, false, "%c%c", id + 65, arrow);
+        draw_distance(x+2, y, distance);
+        draw_vdistance(x+1, y+1, vertical_distance);
+    } else {
+        backend->write(x, y, true, "%c", id + 65);
+    }
+    if (AP_HAL::millis() - last_peer_change > 2000) {
+        id = ap_radar->get_next_healthy_peer(id);
+	last_peer_change = AP_HAL::millis();
+    }
+}
+
 void AP_OSD_Screen::draw_heading(uint8_t x, uint8_t y)
 {
     AP_AHRS &ahrs = AP::ahrs();
@@ -1882,6 +1932,27 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
     }
 }
 
+void AP_OSD_Screen::draw_vdistance(uint8_t x, uint8_t y, float distance)
+{
+    char sym;
+    if (distance > 25.0f) {
+        sym = SYMBOL(SYM_UP_UP);
+    } else if (distance >=0.0f) {
+        sym = SYMBOL(SYM_UP);
+    } else if (distance >= -25.0f) {
+        sym = SYMBOL(SYM_DOWN);
+    } else {
+        sym = SYMBOL(SYM_DOWN_DOWN);
+    }
+    float distance_scaled = u_scale(ALTITUDE, fabsf(distance));
+    if ((osd->units != AP_OSD::UNITS_AVIATION) && (distance_scaled < 9.95f)) {
+        backend->write(x, y, false, "%c%.1f%c", sym, (float)distance_scaled, u_icon(DISTANCE));
+    } else {
+        const char *fmt = osd->units == AP_OSD::UNITS_AVIATION ? "%c%4d%c" : "%c%2d%c";
+        backend->write(x, y, false, fmt, sym, (int)roundf(distance_scaled), u_icon(DISTANCE));
+    }
+}
+
 #if HAL_WITH_ESC_TELEM
 void AP_OSD_Screen::draw_esc_temp(uint8_t x, uint8_t y)
 {
@@ -2357,6 +2428,10 @@ void AP_OSD_Screen::draw(void)
     DRAW_SETTING(eff);
     DRAW_SETTING(callsign);
     DRAW_SETTING(current2);
+
+#if AP_RADAR_ENABLED
+    DRAW_SETTING(radar);
+#endif
 }
 #endif
 #endif // OSD_ENABLED