diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 2e29a59d26..172e1501c7 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -1,7 +1,18 @@ /* - * AP_UAVCAN.cpp + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. * - * Author: Eugene Shamaev + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Author: Eugene Shamaev, Siddharth Bharat Purohit */ #include @@ -15,18 +26,7 @@ #include #include -#if !HAL_MINIMIZE_FEATURES - #include -#endif - -// Zubax GPS and other GPS, baro, magnetic sensors -#include -#include - -#include -#include -#include -#include +#include #include #include @@ -37,7 +37,10 @@ #include #include -#include +#include +#include +#include +#include #define LED_DELAY_US 50000 @@ -50,10 +53,6 @@ extern const AP_HAL::HAL& hal; // The overhead of including definitions of DSDL is very high and it is best to // concentrate in one place. -// TODO: temperature can come not only from baro. There should be separation on node ID -// to check where it belongs to. If it is not baro that is the source, separate layer -// of listeners/nodes should be added. - // table of user settable CAN bus parameters const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: NODE @@ -92,278 +91,6 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // set this to 1 to minimise resend of stale msgs #define CAN_PERIODIC_TX_TIMEOUT_MS 2 -static void gnss_fix_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get()); - if (state == nullptr) { - return; - } - - bool process = false; - - if (msg.status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) { - state->status = AP_GPS::GPS_Status::NO_FIX; - } else { - if (msg.status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) { - state->status = AP_GPS::GPS_Status::NO_FIX; - } else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) { - state->status = AP_GPS::GPS_Status::GPS_OK_FIX_2D; - process = true; - } else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_3D_FIX) { - state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D; - process = true; - } - - if (msg.gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) { - uint64_t epoch_ms = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); - epoch_ms /= 1000; - uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC; - state->time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK); - state->time_week_ms = (uint32_t)(gps_ms - (state->time_week) * AP_MSEC_PER_WEEK); - } - } - - if (process) { - Location loc = { }; - loc.lat = msg.latitude_deg_1e8 / 10; - loc.lng = msg.longitude_deg_1e8 / 10; - loc.alt = msg.height_msl_mm / 10; - state->location = loc; - state->location.options = 0; - - if (!uavcan::isNaN(msg.ned_velocity[0])) { - Vector3f vel(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]); - state->velocity = vel; - state->ground_speed = norm(vel.x, vel.y); - state->ground_course = wrap_360(degrees(atan2f(vel.y, vel.x))); - state->have_vertical_velocity = true; - } else { - state->have_vertical_velocity = false; - } - - float pos_cov[9]; - msg.position_covariance.unpackSquareMatrix(pos_cov); - if (!uavcan::isNaN(pos_cov[8])) { - if (pos_cov[8] > 0) { - state->vertical_accuracy = sqrtf(pos_cov[8]); - state->have_vertical_accuracy = true; - } else { - state->have_vertical_accuracy = false; - } - } else { - state->have_vertical_accuracy = false; - } - - const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]); - if (!uavcan::isNaN(horizontal_pos_variance)) { - if (horizontal_pos_variance > 0) { - state->horizontal_accuracy = sqrtf(horizontal_pos_variance); - state->have_horizontal_accuracy = true; - } else { - state->have_horizontal_accuracy = false; - } - } else { - state->have_horizontal_accuracy = false; - } - - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); - if (!uavcan::isNaN(vel_cov[0])) { - state->speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0); - state->have_speed_accuracy = true; - } else { - state->have_speed_accuracy = false; - } - - state->num_sats = msg.sats_used; - } else { - state->have_vertical_velocity = false; - state->have_vertical_accuracy = false; - state->have_horizontal_accuracy = false; - state->have_speed_accuracy = false; - state->num_sats = 0; - } - - state->last_gps_time_ms = AP_HAL::millis(); - - // after all is filled, update all listeners with new data - ap_uavcan->update_gps_state(msg.getSrcNodeID().get()); -} - -static void gnss_fix_cb0(const uavcan::ReceivedDataStructure& msg) -{ gnss_fix_cb(msg, 0); } -static void gnss_fix_cb1(const uavcan::ReceivedDataStructure& msg) -{ gnss_fix_cb(msg, 1); } -static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { gnss_fix_cb0, gnss_fix_cb1 }; - -static void gnss_aux_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get()); - if (state == nullptr) { - return; - } - - if (!uavcan::isNaN(msg.hdop)) { - state->hdop = msg.hdop * 100.0; - } - - if (!uavcan::isNaN(msg.vdop)) { - state->vdop = msg.vdop * 100.0; - } -} - -static void gnss_aux_cb0(const uavcan::ReceivedDataStructure& msg) -{ gnss_aux_cb(msg, 0); } -static void gnss_aux_cb1(const uavcan::ReceivedDataStructure& msg) -{ gnss_aux_cb(msg, 1); } -static void (*gnss_aux_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { gnss_aux_cb0, gnss_aux_cb1 }; - -static void magnetic_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get(), 0); - if (state == nullptr) { - return; - } - - state->mag_vector[0] = msg.magnetic_field_ga[0]; - state->mag_vector[1] = msg.magnetic_field_ga[1]; - state->mag_vector[2] = msg.magnetic_field_ga[2]; - - // after all is filled, update all listeners with new data - ap_uavcan->update_mag_state(msg.getSrcNodeID().get(), 0); -} - -static void magnetic_cb0(const uavcan::ReceivedDataStructure& msg) -{ magnetic_cb(msg, 0); } -static void magnetic_cb1(const uavcan::ReceivedDataStructure& msg) -{ magnetic_cb(msg, 1); } -static void (*magnetic_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { magnetic_cb0, magnetic_cb1 }; - -static void magnetic_cb_2(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_UAVCAN::Mag_Info *state = ap_uavcan->find_mag_node(msg.getSrcNodeID().get(), msg.sensor_id); - if (state == nullptr) { - return; - } - - state->mag_vector[0] = msg.magnetic_field_ga[0]; - state->mag_vector[1] = msg.magnetic_field_ga[1]; - state->mag_vector[2] = msg.magnetic_field_ga[2]; - - // after all is filled, update all listeners with new data - ap_uavcan->update_mag_state(msg.getSrcNodeID().get(), msg.sensor_id); -} - -static void magnetic_cb_2_0(const uavcan::ReceivedDataStructure& msg) -{ magnetic_cb_2(msg, 0); } -static void magnetic_cb_2_1(const uavcan::ReceivedDataStructure& msg) -{ magnetic_cb_2(msg, 1); } -static void (*magnetic_cb_2_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { magnetic_cb_2_0, magnetic_cb_2_1 }; - -static void air_data_sp_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get()); - if (state == nullptr) { - return; - } - - state->pressure = msg.static_pressure; - state->pressure_variance = msg.static_pressure_variance; - - // after all is filled, update all listeners with new data - ap_uavcan->update_baro_state(msg.getSrcNodeID().get()); -} - -static void air_data_sp_cb0(const uavcan::ReceivedDataStructure& msg) -{ air_data_sp_cb(msg, 0); } -static void air_data_sp_cb1(const uavcan::ReceivedDataStructure& msg) -{ air_data_sp_cb(msg, 1); } -static void (*air_data_sp_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { air_data_sp_cb0, air_data_sp_cb1 }; - -// Temperature is not main parameter so do not update listeners when it is received -static void air_data_st_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_UAVCAN::Baro_Info *state = ap_uavcan->find_baro_node(msg.getSrcNodeID().get()); - if (state == nullptr) { - return; - } - - state->temperature = msg.static_temperature; - state->temperature_variance = msg.static_temperature_variance; -} - -static void air_data_st_cb0(const uavcan::ReceivedDataStructure& msg) -{ air_data_st_cb(msg, 0); } -static void air_data_st_cb1(const uavcan::ReceivedDataStructure& msg) -{ air_data_st_cb(msg, 1); } -static void (*air_data_st_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { air_data_st_cb0, air_data_st_cb1 }; - -static void battery_info_st_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) -{ - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); - if (ap_uavcan == nullptr) { - return; - } - - AP_UAVCAN::BatteryInfo_Info *state = ap_uavcan->find_bi_id((uint16_t) msg.battery_id); - if (state == nullptr) { - return; - } - - state->temperature = msg.temperature; - state->voltage = msg.voltage; - state->current = msg.current; - state->full_charge_capacity_wh = msg.full_charge_capacity_wh; - state->remaining_capacity_wh = msg.remaining_capacity_wh; - state->status_flags = msg.status_flags; - - // after all is filled, update all listeners with new data - ap_uavcan->update_bi_state((uint16_t) msg.battery_id); -} - -static void battery_info_st_cb0(const uavcan::ReceivedDataStructure& msg) -{ battery_info_st_cb(msg, 0); } -static void battery_info_st_cb1(const uavcan::ReceivedDataStructure& msg) -{ battery_info_st_cb(msg, 1); } -static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) - = { battery_info_st_cb0, battery_info_st_cb1 }; - // publisher interfaces static uavcan::Publisher* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS]; @@ -380,41 +107,6 @@ AP_UAVCAN::AP_UAVCAN() : _SRV_conf[i].servo_pending = false; } - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - _gps_nodes[i] = UINT8_MAX; - _gps_node_taken[i] = 0; - } - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - _baro_nodes[i] = UINT8_MAX; - _baro_node_taken[i] = 0; - } - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - _mag_nodes[i] = UINT8_MAX; - _mag_node_taken[i] = 0; - _mag_node_max_sensorid_count[i] = 1; - } - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - _gps_listener_to_node[i] = UINT8_MAX; - _gps_listeners[i] = nullptr; - - _baro_listener_to_node[i] = UINT8_MAX; - _baro_listeners[i] = nullptr; - - _mag_listener_to_node[i] = UINT8_MAX; - _mag_listeners[i] = nullptr; - _mag_listener_sensor_ids[i] = 0; - } - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - _bi_id[i] = UINT8_MAX; - _bi_id_taken[i] = 0; - _bi_BM_listener_to_id[i] = UINT8_MAX; - _bi_BM_listeners[i] = nullptr; - } - SRV_sem = hal.util->new_semaphore(); _led_out_sem = hal.util->new_semaphore(); @@ -498,68 +190,11 @@ void AP_UAVCAN::init(uint8_t driver_index) return; } - uavcan::Subscriber *gnss_fix; - gnss_fix = new uavcan::Subscriber(*_node); - start_res = gnss_fix->start(gnss_fix_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN GNSS subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *gnss_aux; - gnss_aux = new uavcan::Subscriber(*_node); - start_res = gnss_aux->start(gnss_aux_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *magnetic; - magnetic = new uavcan::Subscriber(*_node); - start_res = magnetic->start(magnetic_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN Compass subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *magnetic2; - magnetic2 = new uavcan::Subscriber(*_node); - start_res = magnetic2->start(magnetic_cb_2_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN Compass for multiple mags subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *air_data_sp; - air_data_sp = new uavcan::Subscriber(*_node); - start_res = air_data_sp->start(air_data_sp_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN Baro subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *air_data_st; - air_data_st = new uavcan::Subscriber(*_node); - start_res = air_data_st->start(air_data_st_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN Temperature subscriber start problem, error %d\n\r", start_res); - return; - } - - uavcan::Subscriber *battery_info_st; - battery_info_st = new uavcan::Subscriber(*_node); - start_res = battery_info_st->start(battery_info_st_cb_arr[driver_index]); - - if (start_res < 0) { - debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem, error %d\n\r", start_res); - return; - } + // Roundup all subscribers from supported drivers + AP_GPS_UAVCAN::subscribe_msgs(this); + AP_Compass_UAVCAN::subscribe_msgs(this); + AP_Baro_UAVCAN::subscribe_msgs(this); + AP_BattMonitor_UAVCAN::subscribe_msgs(this); act_out_array[driver_index] = new uavcan::Publisher(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); @@ -575,9 +210,7 @@ void AP_UAVCAN::init(uint8_t driver_index) _led_conf.devices_count = 0; -#if !HAL_MINIMIZE_FEATURES configureCanAcceptanceFilters(*_node); -#endif /* * Informing other nodes that we're ready to work. @@ -840,553 +473,4 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t return true; } - -///// GPS ///// - -uint8_t AP_UAVCAN::find_gps_without_listener(void) -{ - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_gps_listeners[i] == nullptr && _gps_nodes[i] != UINT8_MAX) { - return _gps_nodes[i]; - } - } - - return UINT8_MAX; -} - -uint8_t AP_UAVCAN::register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_gps_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - - if (preferred_channel != 0 && preferred_channel <= AP_UAVCAN_MAX_GPS_NODES) { - _gps_listeners[sel_place] = new_listener; - _gps_listener_to_node[sel_place] = preferred_channel - 1; - _gps_node_taken[_gps_listener_to_node[sel_place]]++; - ret = preferred_channel; - - debug_uavcan(2, "reg_GPS place:%d, chan: %d\n\r", sel_place, preferred_channel); - } else { - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - if (_gps_node_taken[i] == 0) { - _gps_listeners[sel_place] = new_listener; - _gps_listener_to_node[sel_place] = i; - _gps_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_GPS place:%d, chan: %d\n\r", sel_place, i); - break; - } - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_gps_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - if (_gps_nodes[i] == node) { - _gps_listeners[sel_place] = new_listener; - _gps_listener_to_node[sel_place] = i; - _gps_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_GPS place:%d, chan: %d\n\r", sel_place, i); - break; - } - } - - return ret; -} - -void AP_UAVCAN::remove_gps_listener(AP_GPS_Backend* rem_listener) -{ - // Check for all listeners and compare pointers - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_gps_listeners[i] == rem_listener) { - _gps_listeners[i] = nullptr; - - // Also decrement usage counter and reset listening node - if (_gps_node_taken[_gps_listener_to_node[i]] > 0) { - _gps_node_taken[_gps_listener_to_node[i]]--; - } - _gps_listener_to_node[i] = UINT8_MAX; - } - } -} - -AP_GPS::GPS_State *AP_UAVCAN::find_gps_node(uint8_t node) -{ - // Check if such node is already defined - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - if (_gps_nodes[i] == node) { - return &_gps_node_state[i]; - } - } - - // If not - try to find free space for it - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - if (_gps_nodes[i] == UINT8_MAX) { - _gps_nodes[i] = node; - return &_gps_node_state[i]; - } - } - - // If no space is left - return nullptr - return nullptr; -} - -void AP_UAVCAN::update_gps_state(uint8_t node) -{ - // Go through all listeners of specified node and call their's update methods - for (uint8_t i = 0; i < AP_UAVCAN_MAX_GPS_NODES; i++) { - if (_gps_nodes[i] != node) { - continue; - } - for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) { - if (_gps_listener_to_node[j] == i) { - _gps_listeners[j]->handle_gnss_msg(_gps_node_state[i]); - } - } - } -} - - -///// BARO ///// - -/* - * Find discovered not taken baro node with smallest node ID - */ -uint8_t AP_UAVCAN::find_smallest_free_baro_node() -{ - uint8_t ret = UINT8_MAX; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_node_taken[i] == 0) { - ret = MIN(ret, _baro_nodes[i]); - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_baro_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - if (preferred_channel != 0 && preferred_channel < AP_UAVCAN_MAX_BARO_NODES) { - _baro_listeners[sel_place] = new_listener; - _baro_listener_to_node[sel_place] = preferred_channel - 1; - _baro_node_taken[_baro_listener_to_node[sel_place]]++; - ret = preferred_channel; - - debug_uavcan(2, "reg_Baro place:%d, chan: %d\n\r", sel_place, preferred_channel); - } else { - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_node_taken[i] == 0) { - _baro_listeners[sel_place] = new_listener; - _baro_listener_to_node[sel_place] = i; - _baro_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_BARO place:%d, chan: %d\n\r", sel_place, i); - break; - } - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_baro_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place != UINT8_MAX) { - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_nodes[i] != node) { - continue; - } - _baro_listeners[sel_place] = new_listener; - _baro_listener_to_node[sel_place] = i; - _baro_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_BARO place:%d, chan: %d\n\r", sel_place, i); - break; - } - } - - return ret; -} - -void AP_UAVCAN::remove_baro_listener(AP_Baro_Backend* rem_listener) -{ - // Check for all listeners and compare pointers - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_baro_listeners[i] != rem_listener) { - continue; - } - _baro_listeners[i] = nullptr; - - // Also decrement usage counter and reset listening node - if (_baro_node_taken[_baro_listener_to_node[i]] > 0) { - _baro_node_taken[_baro_listener_to_node[i]]--; - } - _baro_listener_to_node[i] = UINT8_MAX; - } -} - -AP_UAVCAN::Baro_Info *AP_UAVCAN::find_baro_node(uint8_t node) -{ - // Check if such node is already defined - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_nodes[i] == node) { - return &_baro_node_state[i]; - } - } - - // If not - try to find free space for it - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_nodes[i] == UINT8_MAX) { - - _baro_nodes[i] = node; - return &_baro_node_state[i]; - } - } - - // If no space is left - return nullptr - return nullptr; -} - -void AP_UAVCAN::update_baro_state(uint8_t node) -{ - // Go through all listeners of specified node and call their's update methods - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BARO_NODES; i++) { - if (_baro_nodes[i] != node) { - continue; - } - for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) { - if (_baro_listener_to_node[j] == i) { - _baro_listeners[j]->handle_baro_msg(_baro_node_state[i].pressure, _baro_node_state[i].temperature); - } - } - } -} - - -///// COMPASS ///// - -/* - * Find discovered mag node with smallest node ID and which is taken N times, - * where N is less than its maximum sensor id. - * This allows multiple AP_Compass_UAVCAN instanses listening multiple compasses - * that are on one node. - */ -uint8_t AP_UAVCAN::find_smallest_free_mag_node() -{ - uint8_t ret = UINT8_MAX; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_node_taken[i] < _mag_node_max_sensorid_count[i]) { - ret = MIN(ret, _mag_nodes[i]); - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_mag_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - if (preferred_channel != 0 && preferred_channel < AP_UAVCAN_MAX_MAG_NODES) { - _mag_listeners[sel_place] = new_listener; - _mag_listener_to_node[sel_place] = preferred_channel - 1; - _mag_node_taken[_mag_listener_to_node[sel_place]]++; - ret = preferred_channel; - - debug_uavcan(2, "reg_Compass place:%d, chan: %d\n\r", sel_place, preferred_channel); - } else { - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_node_taken[i] == 0) { - _mag_listeners[sel_place] = new_listener; - _mag_listener_to_node[sel_place] = i; - _mag_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_MAG place:%d, chan: %d\n\r", sel_place, i); - break; - } - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_mag_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_nodes[i] != node) { - continue; - } - _mag_listeners[sel_place] = new_listener; - _mag_listener_to_node[sel_place] = i; - _mag_listener_sensor_ids[sel_place] = 0; - _mag_node_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_MAG place:%d, chan: %d\n\r", sel_place, i); - break; - } - - return ret; -} - -void AP_UAVCAN::remove_mag_listener(AP_Compass_Backend* rem_listener) -{ - // Check for all listeners and compare pointers - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_mag_listeners[i] != rem_listener) { - continue; - } - _mag_listeners[i] = nullptr; - - // Also decrement usage counter and reset listening node - if (_mag_node_taken[_mag_listener_to_node[i]] > 0) { - _mag_node_taken[_mag_listener_to_node[i]]--; - } - _mag_listener_to_node[i] = UINT8_MAX; - } -} - -AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node, uint8_t sensor_id) -{ - // Check if such node is already defined - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_nodes[i] != node) { - continue; - } - if (_mag_node_max_sensorid_count[i] < sensor_id) { - _mag_node_max_sensorid_count[i] = sensor_id; - debug_uavcan(2, "AP_UAVCAN: Compass: found sensor id %d on node %d\n\r", (int)(sensor_id), (int)(node)); - } - return &_mag_node_state[i]; - } - - // If not - try to find free space for it - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_nodes[i] != UINT8_MAX) { - continue; - } - _mag_nodes[i] = node; - _mag_node_max_sensorid_count[i] = (sensor_id ? sensor_id : 1); - debug_uavcan(2, "AP_UAVCAN: Compass: register sensor id %d on node %d\n\r", (int)(sensor_id), (int)(node)); - return &_mag_node_state[i]; - } - - // If no space is left - return nullptr - return nullptr; -} - -void AP_UAVCAN::update_mag_state(uint8_t node, uint8_t sensor_id) -{ - // Go through all listeners of specified node and call their's update methods - for (uint8_t i = 0; i < AP_UAVCAN_MAX_MAG_NODES; i++) { - if (_mag_nodes[i] != node) { - continue; - } - for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) { - if (_mag_listener_to_node[j] != i) { - continue; - } - - /*If the current listener has default sensor_id, - while our sensor_id is not default, we have - to assign our sensor_id to this listener*/ - if ((_mag_listener_sensor_ids[j] == 0) && (sensor_id != 0)) { - bool already_taken = false; - for (uint8_t k = 0; k < AP_UAVCAN_MAX_LISTENERS; k++) { - if (_mag_listener_sensor_ids[k] == sensor_id) { - already_taken = true; - } - } - if (!already_taken) { - debug_uavcan(2, "AP_UAVCAN: Compass: sensor_id updated to %d for listener %d\n", sensor_id, j); - _mag_listener_sensor_ids[j] = sensor_id; - } - } - - /*If the current listener has the sensor_id that we have, - or our sensor_id is default, ask the listener to handle the measurements - (the default one is used for the nodes that have only one compass*/ - if ((sensor_id == 0) || (_mag_listener_sensor_ids[j] == sensor_id)) { - _mag_listeners[j]->handle_mag_msg(_mag_node_state[i].mag_vector); - } - } - } -} - - -///// BATTERY ///// - -uint8_t AP_UAVCAN::find_smallest_free_bi_id() -{ - uint8_t ret = UINT8_MAX; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id_taken[i] == 0) { - ret = MIN(ret, _bi_id[i]); - } - } - - return ret; -} - -uint8_t AP_UAVCAN::register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id) -{ - uint8_t sel_place = UINT8_MAX, ret = 0; - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_bi_BM_listeners[i] == nullptr) { - sel_place = i; - break; - } - } - - if (sel_place == UINT8_MAX) { - return 0; - } - - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id[i] != id) { - continue; - } - _bi_BM_listeners[sel_place] = new_listener; - _bi_BM_listener_to_id[sel_place] = i; - _bi_id_taken[i]++; - ret = i + 1; - - debug_uavcan(2, "reg_BI place:%d, chan: %d\n\r", sel_place, i); - break; - } - - return ret; -} - -void AP_UAVCAN::remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener) -{ - // Check for all listeners and compare pointers - for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) { - if (_bi_BM_listeners[i] != rem_listener) { - continue; - } - - _bi_BM_listeners[i] = nullptr; - - // Also decrement usage counter and reset listening node - if (_bi_id_taken[_bi_BM_listener_to_id[i]] > 0) { - _bi_id_taken[_bi_BM_listener_to_id[i]]--; - } - _bi_BM_listener_to_id[i] = UINT8_MAX; - } -} - -AP_UAVCAN::BatteryInfo_Info *AP_UAVCAN::find_bi_id(uint8_t id) -{ - // Check if such node is already defined - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id[i] == id) { - return &_bi_id_state[i]; - } - } - - // If not - try to find free space for it - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id[i] == UINT8_MAX) { - _bi_id[i] = id; - return &_bi_id_state[i]; - } - } - - // If no space is left - return nullptr - return nullptr; -} - -void AP_UAVCAN::update_bi_state(uint8_t id) -{ - // Go through all listeners of specified node and call their's update methods - for (uint8_t i = 0; i < AP_UAVCAN_MAX_BI_NUMBER; i++) { - if (_bi_id[i] != id) { - continue; - } - for (uint8_t j = 0; j < AP_UAVCAN_MAX_LISTENERS; j++) { - if (_bi_BM_listener_to_id[j] != i) { - continue; - } - _bi_BM_listeners[j]->handle_bi_msg(_bi_id_state[i].voltage, _bi_id_state[i].current, _bi_id_state[i].temperature); - } - } -} - #endif // HAL_WITH_UAVCAN diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 030b612c4c..900117be26 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -1,6 +1,18 @@ /* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. * - * Author: Eugene Shamaev + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Author: Eugene Shamaev, Siddharth Bharat Purohit */ #ifndef AP_UAVCAN_H_ #define AP_UAVCAN_H_ @@ -9,14 +21,8 @@ #include #include -#include #include -#include -#include -#include -#include - #include #ifndef UAVCAN_NODE_POOL_SIZE @@ -31,12 +37,6 @@ #define UAVCAN_SRV_NUMBER 18 #endif -#define AP_UAVCAN_MAX_LISTENERS 4 -#define AP_UAVCAN_MAX_GPS_NODES 4 -#define AP_UAVCAN_MAX_MAG_NODES 4 -#define AP_UAVCAN_MAX_BARO_NODES 4 -#define AP_UAVCAN_MAX_BI_NUMBER 4 - #define AP_UAVCAN_SW_VERS_MAJOR 1 #define AP_UAVCAN_SW_VERS_MINOR 0 @@ -79,75 +79,6 @@ public: ///// LED ///// bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); - ///// GPS ///// - - uint8_t find_gps_without_listener(void); - - // this function will register the listening class on a first free channel or on the specified channel - // if preferred_channel = 0 then free channel will be searched for - // if preferred_channel > 0 then listener will be added to specific channel - // return value is the number of assigned channel or 0 if fault - // channel numbering starts from 1 - uint8_t register_gps_listener(AP_GPS_Backend* new_listener, uint8_t preferred_channel); - - uint8_t register_gps_listener_to_node(AP_GPS_Backend* new_listener, uint8_t node); - - // Removes specified listener from all nodes - void remove_gps_listener(AP_GPS_Backend* rem_listener); - - // Returns pointer to GPS state connected with specified node. - // If node is not found and there are free space, locate a new one - AP_GPS::GPS_State *find_gps_node(uint8_t node); - - // Updates all listeners of specified node - void update_gps_state(uint8_t node); - - ///// BARO ///// - - struct Baro_Info { - float pressure; - float pressure_variance; - float temperature; - float temperature_variance; - }; - - uint8_t find_smallest_free_baro_node(); - uint8_t register_baro_listener(AP_Baro_Backend* new_listener, uint8_t preferred_channel); - uint8_t register_baro_listener_to_node(AP_Baro_Backend* new_listener, uint8_t node); - void remove_baro_listener(AP_Baro_Backend* rem_listener); - Baro_Info *find_baro_node(uint8_t node); - void update_baro_state(uint8_t node); - - ///// COMPASS ///// - - struct Mag_Info { - Vector3f mag_vector; - }; - - uint8_t find_smallest_free_mag_node(); - uint8_t register_mag_listener(AP_Compass_Backend* new_listener, uint8_t preferred_channel); - uint8_t register_mag_listener_to_node(AP_Compass_Backend* new_listener, uint8_t node); - void remove_mag_listener(AP_Compass_Backend* rem_listener); - Mag_Info *find_mag_node(uint8_t node, uint8_t sensor_id); - void update_mag_state(uint8_t node, uint8_t sensor_id); - - ///// BATTERY ///// - - struct BatteryInfo_Info { - float temperature; - float voltage; - float current; - float remaining_capacity_wh; - float full_charge_capacity_wh; - uint8_t status_flags; - }; - - uint8_t find_smallest_free_bi_id(); - uint8_t register_BM_bi_listener_to_id(AP_BattMonitor_Backend* new_listener, uint8_t id); - void remove_BM_bi_listener(AP_BattMonitor_Backend* rem_listener); - BatteryInfo_Info *find_bi_id(uint8_t id); - void update_bi_state(uint8_t id); - template class RegistryBinder { @@ -255,45 +186,6 @@ private: } _led_conf; AP_HAL::Semaphore *_led_out_sem; - - ///// GPS ///// - // 255 - means free node - uint8_t _gps_nodes[AP_UAVCAN_MAX_GPS_NODES]; - - // Counter of how many listeners are connected to this source - uint8_t _gps_node_taken[AP_UAVCAN_MAX_GPS_NODES]; - - // GPS data of the sources - AP_GPS::GPS_State _gps_node_state[AP_UAVCAN_MAX_GPS_NODES]; - - // 255 - means no connection - uint8_t _gps_listener_to_node[AP_UAVCAN_MAX_LISTENERS]; - - // Listeners with callbacks to be updated - AP_GPS_Backend* _gps_listeners[AP_UAVCAN_MAX_LISTENERS]; - - ///// BARO ///// - uint8_t _baro_nodes[AP_UAVCAN_MAX_BARO_NODES]; - uint8_t _baro_node_taken[AP_UAVCAN_MAX_BARO_NODES]; - Baro_Info _baro_node_state[AP_UAVCAN_MAX_BARO_NODES]; - uint8_t _baro_listener_to_node[AP_UAVCAN_MAX_LISTENERS]; - AP_Baro_Backend* _baro_listeners[AP_UAVCAN_MAX_LISTENERS]; - - ///// COMPASS ///// - uint8_t _mag_nodes[AP_UAVCAN_MAX_MAG_NODES]; - uint8_t _mag_node_taken[AP_UAVCAN_MAX_MAG_NODES]; - Mag_Info _mag_node_state[AP_UAVCAN_MAX_MAG_NODES]; - uint8_t _mag_node_max_sensorid_count[AP_UAVCAN_MAX_MAG_NODES]; - uint8_t _mag_listener_to_node[AP_UAVCAN_MAX_LISTENERS]; - AP_Compass_Backend* _mag_listeners[AP_UAVCAN_MAX_LISTENERS]; - uint8_t _mag_listener_sensor_ids[AP_UAVCAN_MAX_LISTENERS]; - - ///// BATTERY ///// - uint16_t _bi_id[AP_UAVCAN_MAX_BI_NUMBER]; - uint16_t _bi_id_taken[AP_UAVCAN_MAX_BI_NUMBER]; - BatteryInfo_Info _bi_id_state[AP_UAVCAN_MAX_BI_NUMBER]; - uint16_t _bi_BM_listener_to_id[AP_UAVCAN_MAX_LISTENERS]; - AP_BattMonitor_Backend* _bi_BM_listeners[AP_UAVCAN_MAX_LISTENERS]; }; #endif /* AP_UAVCAN_H_ */