diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 43d9bfda5f48ca..e4372b51654155 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -8,20 +8,22 @@ extern const AP_HAL::HAL& hal; #define AP_MOUNT_UPDATE_DT 0.02 // update rate in seconds. update() should be called at this rate -#define AP_MOUNT_POI_REQ_TIMOUT_MS 3000 // POI calculations valid for 3 seconds -#define AP_MOUNT_POI_DIST_M_MAX 10000 // POI calculations limit of 10,000m (10km) +#define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000 // POI calculations continue to be updated for this many seconds after last request +#define AP_MOUNT_POI_RESULT_TIMEOUT_MS 3000 // POI calculations valid for 3 seconds +#define AP_MOUNT_POI_DIST_M_MAX 10000 // POI calculations limit of 10,000m (10km) // Default init function for every mount void AP_Mount_Backend::init() { // setting default target sysid from parameters _target_sysid = _params.sysid_default.get(); + #if AP_MOUNT_POI_TO_LATLONALT_ENABLED // create a calculation thread for poi. if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void), "mount_calc_poi", 8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "POI: failed to start thread"); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread"); } #endif } @@ -429,29 +431,44 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) // get poi information. Returns true on success and fills in gimbal attitude, location and poi location bool AP_Mount_Backend::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) { - { - WITH_SEMAPHORE(poi_calculation.sem); - if ((AP_HAL::millis() - poi_calculation.poi_update_ms) > AP_MOUNT_POI_REQ_TIMOUT_MS) { - // last lat,lon,alt values are stale - return false; - } - if (poi_calculation.att_quat.is_nan()) { - return false; - } - quat = poi_calculation.att_quat; - loc = poi_calculation.loc; - poi_loc = poi_calculation.poi_loc; - return true; + WITH_SEMAPHORE(poi_calculation.sem); + + // record time of request + const uint32_t now_ms = AP_HAL::millis(); + poi_calculation.poi_request_ms = now_ms; + + // check if poi calculated recently + if (now_ms - poi_calculation.poi_update_ms > AP_MOUNT_POI_RESULT_TIMEOUT_MS) { + return false; + } + + // check attitude is valid + if (poi_calculation.att_quat.is_nan()) { + return false; } + + quat = poi_calculation.att_quat; + loc = poi_calculation.loc; + poi_loc = poi_calculation.poi_loc; + return true; } -// calculate poi +// calculate the Location that the gimbal is pointing at void AP_Mount_Backend::calculate_poi() { while (true) { // run this loop at 10hz hal.scheduler->delay(100); + // calculate poi if requested within last 30 seconds + { + WITH_SEMAPHORE(poi_calculation.sem); + if ((poi_calculation.poi_request_ms == 0) || + (AP_HAL::millis() - poi_calculation.poi_request_ms > AP_MOUNT_POI_REQUEST_TIMEOUT_MS)) { + continue; + } + } + // get the current location of vehicle const AP_AHRS &ahrs = AP::ahrs(); Location curr_loc; @@ -459,32 +476,31 @@ void AP_Mount_Backend::calculate_poi() continue; } - // change vehicle location to AMSL + // change vehicle alt to AMSL curr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE); - // retrieve gimbal attitude - Quaternion quat; - if (!get_attitude_quaternion(quat)) { - // gimbal attitude unavailable - continue; - } - // project forward from vehicle looking for terrain // start testing at vehicle's location Location test_loc = curr_loc; Location prev_test_loc = curr_loc; + // get terrain altitude (AMSL) at test_loc auto terrain = AP_Terrain::get_singleton(); float terrain_amsl_m; - // get terrain altitude (AMSL) at test_loc - if (!terrain->height_amsl(test_loc, terrain_amsl_m, true)) { - // failed to get terrain altitude + if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) { continue; } + // get the terrain spacing float dist_increment_m; if (!terrain->get_terrain_spacing(dist_increment_m)) { - // failed to get the terrain spacing + continue; + } + + // retrieve gimbal attitude + Quaternion quat; + if (!get_attitude_quaternion(quat)) { + // gimbal attitude unavailable continue; } @@ -497,33 +513,31 @@ void AP_Mount_Backend::calculate_poi() while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) { total_dist_m += dist_increment_m; - // Take backup of previous test location and terrain amsl + // backup previous test location and terrain amsl prev_test_loc = test_loc; prev_terrain_amsl_m = terrain_amsl_m; - // Move test location forward + // move test location forward test_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m); - // Get terrain's alt-above-sea-level (at test_loc) - // Fail if terrain alt cannot be retrieved + // get terrain's alt-above-sea-level (at test_loc) + // fail if terrain alt cannot be retrieved if (!terrain->height_amsl(test_loc, terrain_amsl_m, true) || std::isnan(terrain_amsl_m)) { - // failed to get terrain alt get_terrain_alt_success = false; continue; } } - // if a fail occurs in the loop while getting terrain alt then continue to next interation + + // if a fail occurred above when getting terrain alt then restart calculations from the beginning if (!get_terrain_alt_success) { continue; } + if (total_dist_m >= AP_MOUNT_POI_DIST_M_MAX) { // unable to find terrain within dist_max continue; } - if (is_negative(terrain_amsl_m)) { - // failed to retrieve terrain alt - continue; - } + // test location has dropped below terrain // interpolate along line between prev_test_loc and test_loc float dist_interp_m = linear_interpolate(0, dist_increment_m, 0, prev_test_loc.alt * 0.01 - prev_terrain_amsl_m, test_loc.alt * 0.01 - terrain_amsl_m); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 33a110f3997cc8..fbd2ca0a801044 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -227,7 +227,7 @@ class AP_Mount_Backend virtual bool suppress_heartbeat() const { return false; } #if AP_MOUNT_POI_TO_LATLONALT_ENABLED - // calculate poi information + // calculate the Location that the gimbal is pointing at void calculate_poi(); #endif @@ -282,6 +282,7 @@ class AP_Mount_Backend #if AP_MOUNT_POI_TO_LATLONALT_ENABLED struct { HAL_Semaphore sem; // semaphore protecting this structure + uint32_t poi_request_ms; // system time POI was last requested uint32_t poi_update_ms; // system time POI was calculated Location loc; // gimbal location used for poi calculation Location poi_loc; // location of the POI