From 454560ca4cecbe567fdbe13fc89dfa87c36a0054 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Sun, 10 Jul 2016 16:20:25 -0700 Subject: [PATCH] AP_ADSB: organize variables into in_state and avoid_state structs --- libraries/AP_ADSB/AP_ADSB.cpp | 165 ++++++++++++++++++---------------- libraries/AP_ADSB/AP_ADSB.h | 51 +++++++---- 2 files changed, 119 insertions(+), 97 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index ea1e043621..e53ac623f4 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -41,14 +41,15 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Description: ADSB based Collision Avoidance Behavior selector // @Values: 0:None,1:Loiter,2:LoiterAndDescend // @User: Advanced - AP_GROUPINFO("BEHAVIOR", 1, AP_ADSB, _behavior, ADSB_BEHAVIOR_NONE), + AP_GROUPINFO("BEHAVIOR", 1, AP_ADSB, avoid_state.behavior, ADSB_BEHAVIOR_NONE), // @Param: LIST_MAX // @DisplayName: ADSB vehicle list size // @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values. // @Range: 1 100 // @User: Advanced - AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, _list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT), + AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, in_state.list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT), + AP_GROUPEND }; @@ -58,25 +59,28 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { */ void AP_ADSB::init(void) { - _vehicle_count = 0; - if (_vehicle_list == nullptr) { - if (_list_size_param != constrain_int16(_list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) { - _list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT); - _list_size_param.save(); + // in_state + in_state.vehicle_count = 0; + if (in_state.vehicle_list == nullptr) { + if (in_state.list_size_param != constrain_int16(in_state.list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) { + in_state.list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT); + in_state.list_size_param.save(); } - _list_size = _list_size_param; - _vehicle_list = new adsb_vehicle_t[_list_size]; + in_state.list_size = in_state.list_size_param; + in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size]; - if (_vehicle_list == nullptr) { + if (in_state.vehicle_list == nullptr) { // dynamic RAM allocation of _vehicle_list[] failed, disable gracefully hal.console->printf("Unable to initialize ADS-B vehicle list\n"); _enabled.set_and_notify(0); } } - _lowest_threat_distance = 0; - _highest_threat_distance = 0; - _another_vehicle_within_radius = false; - _is_evading_threat = false; + + // avoid_state + avoid_state.lowest_threat_distance = 0; + avoid_state.highest_threat_distance = 0; + avoid_state.another_vehicle_within_radius = false; + avoid_state.is_evading_threat = false; } /* @@ -84,10 +88,10 @@ void AP_ADSB::init(void) */ void AP_ADSB::deinit(void) { - _vehicle_count = 0; - if (_vehicle_list != nullptr) { - delete [] _vehicle_list; - _vehicle_list = nullptr; + in_state.vehicle_count = 0; + if (in_state.vehicle_list != nullptr) { + delete [] in_state.vehicle_list; + in_state.vehicle_list = nullptr; } } @@ -96,24 +100,32 @@ void AP_ADSB::deinit(void) */ void AP_ADSB::update(void) { + // update _my_loc + if (!_ahrs.get_position(_my_loc)) { + _my_loc.zero(); + } + if (!_enabled) { - if (_vehicle_list != nullptr) { + if (in_state.vehicle_list != nullptr) { deinit(); } // nothing to do return; - } else if (_vehicle_list == nullptr) { + } else if (in_state.vehicle_list == nullptr) { init(); return; - } else if (_list_size != _list_size_param) { + } else if (in_state.list_size != in_state.list_size_param) { deinit(); return; } + uint32_t now = AP_HAL::millis(); + + // check current list for vehicles that time out uint16_t index = 0; - while (index < _vehicle_count) { + while (index < in_state.vehicle_count) { // check list and drop stale vehicles. When disabled, the list will get flushed - if (AP_HAL::millis() - _vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) { + if (now - in_state.vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) { // don't increment index, we want to check this same index again because the contents changed // also, if we're disabled then clear the list delete_vehicle(index); @@ -123,7 +135,6 @@ void AP_ADSB::update(void) } perform_threat_detection(); - //hal.console->printf("ADSB: cnt %u, lowT %.0f, highT %.0f\r", _vehicle_count, _lowest_threat_distance, _highest_threat_distance); } /* @@ -131,13 +142,11 @@ void AP_ADSB::update(void) */ void AP_ADSB::perform_threat_detection(void) { - Location my_loc; - if (_vehicle_count == 0 || - _ahrs.get_position(my_loc) == false) { + if (in_state.vehicle_count == 0 || _my_loc.is_zero()) { // nothing to do or current location is unknown so we can't calculate any collisions - _another_vehicle_within_radius = false; - _lowest_threat_distance = 0; // 0 means invalid - _highest_threat_distance = 0; // 0 means invalid + avoid_state.another_vehicle_within_radius = false; + avoid_state.lowest_threat_distance = 0; // 0 means invalid + avoid_state.highest_threat_distance = 0; // 0 means invalid return; } @@ -152,8 +161,8 @@ void AP_ADSB::perform_threat_detection(void) uint16_t min_distance_index = 0; uint16_t max_distance_index = 0; - for (uint16_t index = 0; index < _vehicle_count; index++) { - float distance = get_distance(my_loc, get_location(_vehicle_list[index])); + for (uint16_t index = 0; index < in_state.vehicle_count; index++) { + float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index])); if (min_distance > distance || index == 0) { min_distance = distance; min_distance_index = index; @@ -164,24 +173,24 @@ void AP_ADSB::perform_threat_detection(void) } if (distance <= VEHICLE_THREAT_RADIUS_M) { - _vehicle_list[index].threat_level = ADSB_THREAT_HIGH; + in_state.vehicle_list[index].threat_level = ADSB_THREAT_HIGH; } else { - _vehicle_list[index].threat_level = ADSB_THREAT_LOW; + in_state.vehicle_list[index].threat_level = ADSB_THREAT_LOW; } } // for index - _highest_threat_index = min_distance_index; - _highest_threat_distance = min_distance; + avoid_state.highest_threat_index = min_distance_index; + avoid_state.highest_threat_distance = min_distance; - _lowest_threat_index = max_distance_index; - _lowest_threat_distance = max_distance; + avoid_state.lowest_threat_index = max_distance_index; + avoid_state.lowest_threat_distance = max_distance; // if within radius, set flag and enforce a double radius to clear flag - if (is_zero(_highest_threat_distance) || // 0 means invalid - _highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) { - _another_vehicle_within_radius = false; - } else if (_highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) { - _another_vehicle_within_radius = true; + if (is_zero(avoid_state.highest_threat_distance) || // 0 means invalid + avoid_state.highest_threat_distance > 2*VEHICLE_THREAT_RADIUS_M) { + avoid_state.another_vehicle_within_radius = false; + } else if (avoid_state.highest_threat_distance <= VEHICLE_THREAT_RADIUS_M) { + avoid_state.another_vehicle_within_radius = true; } } @@ -204,21 +213,21 @@ Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const */ void AP_ADSB::delete_vehicle(uint16_t index) { - if (index < _vehicle_count) { + if (index < in_state.vehicle_count) { // if the vehicle is the lowest/highest threat, invalidate it - if (index == _lowest_threat_index) { - _lowest_threat_distance = 0; + if (index == avoid_state.lowest_threat_index) { + avoid_state.lowest_threat_distance = 0; } - if (index == _highest_threat_index) { - _highest_threat_distance = 0; + if (index == avoid_state.highest_threat_index) { + avoid_state.highest_threat_distance = 0; } - if (index != (_vehicle_count-1)) { - _vehicle_list[index] = _vehicle_list[_vehicle_count-1]; + if (index != (in_state.vehicle_count-1)) { + in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; } // TODO: is memset needed? When we decrement the index we essentially forget about it - memset(&_vehicle_list[_vehicle_count-1], 0, sizeof(adsb_vehicle_t)); - _vehicle_count--; + memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t)); + in_state.vehicle_count--; } } @@ -229,8 +238,8 @@ void AP_ADSB::delete_vehicle(uint16_t index) */ bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const { - for (uint16_t i = 0; i < _vehicle_count; i++) { - if (_vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) { + for (uint16_t i = 0; i < in_state.vehicle_count; i++) { + if (in_state.vehicle_list[i].info.ICAO_address == vehicle.info.ICAO_address) { *index = i; return true; } @@ -244,7 +253,7 @@ bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const */ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) { - if (_vehicle_list == nullptr) { + if (in_state.vehicle_list == nullptr) { // We are only null when disabled. Updating is inhibited. return; } @@ -258,36 +267,34 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) // found, update it set_vehicle(index, vehicle); - } else if (_vehicle_count < _list_size) { + } else if (in_state.vehicle_count < in_state.list_size) { // not found and there's room, add it to the end of the list - set_vehicle(_vehicle_count, vehicle); - _vehicle_count++; + set_vehicle(in_state.vehicle_count, vehicle); + in_state.vehicle_count++; } else { // buffer is full, replace the vehicle with lowest threat as long as it's not further away - Location my_loc; - if (!is_zero(_lowest_threat_distance) && // nonzero means it is valid - _ahrs.get_position(my_loc)) { // true means my_loc is valid + if (!is_zero(avoid_state.lowest_threat_distance) && !_my_loc.is_zero()) { // nonzero means it is valid - float distance = get_distance(my_loc, get_location(vehicle)); - if (distance < _lowest_threat_distance) { // is closer than the furthest + float distance = _my_loc.get_distance(get_location(vehicle)); + if (distance < avoid_state.lowest_threat_distance) { // is closer than the furthest // overwrite the lowest_threat/furthest - index = _lowest_threat_index; + index = avoid_state.lowest_threat_index; set_vehicle(index, vehicle); // this is now invalid because the vehicle was overwritten, need // to run perform_threat_detection() to determine new one because // we aren't keeping track of the second-furthest vehicle. - _lowest_threat_distance = 0; + avoid_state.lowest_threat_distance = 0; // is it the nearest? Then it's the highest threat. That's an easy check // that we don't need to run perform_threat_detection() to determine - if (_highest_threat_distance > distance) { - _highest_threat_distance = distance; - _highest_threat_index = index; + if (avoid_state.highest_threat_distance > distance) { + avoid_state.highest_threat_distance = distance; + avoid_state.highest_threat_index = index; } } // if distance @@ -300,35 +307,35 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) */ void AP_ADSB::set_vehicle(uint16_t index, const adsb_vehicle_t &vehicle) { - if (index < _list_size) { - _vehicle_list[index] = vehicle; - _vehicle_list[index].last_update_ms = AP_HAL::millis(); + if (index < in_state.list_size) { + in_state.vehicle_list[index] = vehicle; + in_state.vehicle_list[index].last_update_ms = AP_HAL::millis(); } } void AP_ADSB::send_adsb_vehicle(mavlink_channel_t chan) { - if (_vehicle_list == nullptr || _vehicle_count == 0) { + if (in_state.vehicle_list == nullptr || in_state.vehicle_count == 0) { return; } uint32_t now = AP_HAL::millis(); - if (send_index[chan] >= _vehicle_count) { + if (in_state.send_index[chan] >= in_state.vehicle_count) { // we've finished a list - if (now - send_start_ms[chan] < 1000) { + if (now - in_state.send_start_ms[chan] < 1000) { // too soon to start a new one return; } else { // start new list - send_start_ms[chan] = now; - send_index[chan] = 0; + in_state.send_start_ms[chan] = now; + in_state.send_index[chan] = 0; } } - if (send_index[chan] < _vehicle_count) { - mavlink_adsb_vehicle_t vehicle = _vehicle_list[send_index[chan]].info; - send_index[chan]++; + if (in_state.send_index[chan] < in_state.vehicle_count) { + mavlink_adsb_vehicle_t vehicle = in_state.vehicle_list[in_state.send_index[chan]].info; + in_state.send_index[chan]++; mavlink_msg_adsb_vehicle_send(chan, vehicle.ICAO_address, diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 6ea291288e..988a67c8a7 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -69,12 +69,13 @@ public: // add or update vehicle_list from inbound mavlink msg void update_vehicle(const mavlink_message_t* msg); - bool get_possible_threat() { return _enabled && _another_vehicle_within_radius; } - ADSB_BEHAVIOR get_behavior() { return (ADSB_BEHAVIOR)(_behavior.get()); } - bool get_is_evading_threat() { return _enabled && _is_evading_threat; } - void set_is_evading_threat(bool is_evading) { if (_enabled) { _is_evading_threat = is_evading; } } - uint16_t get_vehicle_count() { return _vehicle_count; } + bool get_possible_threat() { return _enabled && avoid_state.another_vehicle_within_radius; } + + ADSB_BEHAVIOR get_behavior() { return (ADSB_BEHAVIOR)(avoid_state.behavior.get()); } + bool get_is_evading_threat() { return _enabled && avoid_state.is_evading_threat; } + void set_is_evading_threat(bool is_evading) { if (_enabled) { avoid_state.is_evading_threat = is_evading; } } + uint16_t get_vehicle_count() { return in_state.vehicle_count; } // send ADSB_VEHICLE mavlink message, usually as a StreamRate void send_adsb_vehicle(mavlink_channel_t chan); @@ -106,24 +107,38 @@ private: const AP_AHRS &_ahrs; AP_Int8 _enabled; - AP_Int8 _behavior; - AP_Int16 _list_size_param; - uint16_t _list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start - adsb_vehicle_t *_vehicle_list; - uint16_t _vehicle_count = 0; - bool _another_vehicle_within_radius = false; - bool _is_evading_threat = false; - // index of and distance to vehicle with lowest threat - uint16_t _lowest_threat_index = 0; - float _lowest_threat_distance = 0; + Location_Class _my_loc; - // index of and distance to vehicle with highest threat - uint16_t _highest_threat_index = 0; - float _highest_threat_distance = 0; + // ADSB-IN state. Maintains list of external vehicles + struct { + // list management + AP_Int16 list_size_param; + uint16_t list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start + adsb_vehicle_t *vehicle_list = nullptr; + uint16_t vehicle_count = 0; // streamrate stuff uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS]; uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS]; + } in_state; + + + + // Avoidance state + struct { + AP_Int8 behavior; + + bool another_vehicle_within_radius = false; + bool is_evading_threat = false; + + // index of and distance to vehicle with lowest threat + uint16_t lowest_threat_index = 0; + float lowest_threat_distance = 0; + + // index of and distance to vehicle with highest threat + uint16_t highest_threat_index = 0; + float highest_threat_distance = 0; + } avoid_state; };