throttle and remove some osd updates

Author: PeterJBurke

libraries/AP_Avoidance/AP_Avoidance.cpp

@@ -343,7 +343,7 @@ void AP_Avoidance::get_adsb_samples()
 void AP_Avoidance::get_odid_samples()
 {
     //TODO: Process samples here
-    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Avoidance: Get %d drones", AP::vehicle()->odidscanner.get_count());
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Avoidance: Get %d drones", AP::vehicle()->odidscanner.get_count());
     for(int i = 0;i<AP::vehicle()->odidscanner.get_count();i++) {
         auto v = AP::vehicle()->odidscanner.get_vehicle(i);
         uint8_t mac[6];
@@ -621,12 +621,19 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
 {
     MAV_COLLISION_THREAT_LEVEL new_threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
     MAV_COLLISION_ACTION action = MAV_COLLISION_ACTION_NONE;
-    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Threat level: %d", new_threat_level);
+    // GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Threat level: %d", new_threat_level);
     AP::logger().Write("ODID", "TimeUS,Threat_level", "Qb", AP_HAL::micros64(), new_threat_level);
 
 
     if (threat != nullptr) {
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z);
+        // xxx need a timer here, update to OSD is too often at 10 Hz...
+        // pseudo code:
+        // now=millis()
+        // if now-_time_of_last_GCS_nearest_drone_update > 1000 ms then post new message...
+        uint32_t now = AP_HAL::millis();
+        if ( (now - time_of_last_GCS_nearest_drone_update ) > 1000){
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z);
+        }
         // double closest_approach_xy = threat->closest_approach_xy;
         // double closest_approach_z = threat->closest_approach_z;
         // AP::logger().Write("ODID", "TimeUS,xy,z", "Qff", AP_HAL::micros64(), closest_approach_xy, closest_approach_z);

libraries/AP_Avoidance/AP_Avoidance.h

@@ -154,6 +154,9 @@ class AP_Avoidance {
     static Vector3f perpendicular_xyz(const Location &p1, const Vector3f &v1, const Location &p2);
     static Vector2f perpendicular_xy(const Location &p1, const Vector3f &v1, const Location &p2);
 
+    // ODID avoidance variables
+    uint32_t time_of_last_GCS_nearest_drone_update;
+
 private:
 
     void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour) const;

libraries/AP_ODIDScanner/AP_ODIDScanner.cpp

@@ -87,22 +87,24 @@ case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION:
 }
 void AP_ODIDScanner::update() {
     const uint32_t now_ms = AP_HAL::millis();
-    if (now_ms - last_dev_hb_ms > 5000 && now_ms - last_dev_hb_msg_ms > 5000) {
+
+    if (now_ms - last_dev_hb_ms > 15000 && now_ms - last_dev_hb_msg_ms > 15000) {
         last_dev_hb_msg_ms = now_ms;
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Device Not Found");
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Device Not Found at %f",float(now_ms));
         _port->printf("Scanner: Where is this printing?");
     }
-    if (now_ms - last_hb_send_ms > 1000) {
 
+    if (now_ms - last_hb_send_ms > 1000) { // why this?
         last_hb_send_ms = now_ms;
-    mavlink_msg_heartbeat_send(
+        mavlink_msg_heartbeat_send(
         _chan,
         gcs().frame_type(),
         MAV_AUTOPILOT_ARDUPILOTMEGA,
         0,
         gcs().custom_mode(),
         0);
-        } 
+    } 
+
 }
 
 bool AP_ODIDScanner::message_from_rx(mavlink_channel_t& chan) {

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -398,7 +398,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_OpenDroneID, &vehicle.opendroneid,  update,                   10,  50, 236),
 #endif
 // test comment...
-    SCHED_TASK_CLASS(AP_ODIDScanner,&vehicle.odidscanner, update, 1, 50, 237),
+    SCHED_TASK_CLASS(AP_ODIDScanner,&vehicle.odidscanner, update, 10, 50, 237),
 #if OSD_ENABLED
     SCHED_TASK(publish_osd_info, 1, 10, 240),
 #endif