can_acceptance_filter_configurator.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
00003  *                    Ilia  Sheremet <illia.sheremet@gmail.com>
00004  */
00005 
00006 #include <gtest/gtest.h>
00007 #include <cassert>
00008 
00009 #include <uavcan/transport/can_acceptance_filter_configurator.hpp>
00010 #include "../node/test_node.hpp"
00011 #include "uavcan/node/subscriber.hpp"
00012 #include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
00013 #include <uavcan/equipment/air_data/Sideslip.hpp>
00014 #include <uavcan/equipment/air_data/TrueAirspeed.hpp>
00015 #include <uavcan/equipment/air_data/AngleOfAttack.hpp>
00016 #include <uavcan/equipment/ahrs/Solution.hpp>
00017 #include <uavcan/equipment/air_data/StaticPressure.hpp>
00018 #include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
00019 #include <uavcan/node/service_client.hpp>
00020 #include <uavcan/node/service_server.hpp>
00021 #include <iostream>
00022 
00023 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
00024 
00025 template <typename DataType>
00026 struct SubscriptionListener
00027 {
00028     typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
00029 
00030     struct ReceivedDataStructureCopy
00031     {
00032         uavcan::MonotonicTime ts_monotonic;
00033         uavcan::UtcTime ts_utc;
00034         uavcan::TransferType transfer_type;
00035         uavcan::TransferID transfer_id;
00036         uavcan::NodeID src_node_id;
00037         uavcan::uint8_t iface_index;
00038         DataType msg;
00039 
00040         ReceivedDataStructureCopy(const ReceivedDataStructure& s) :
00041             ts_monotonic(s.getMonotonicTimestamp()),
00042             ts_utc(s.getUtcTimestamp()),
00043             transfer_type(s.getTransferType()),
00044             transfer_id(s.getTransferID()),
00045             src_node_id(s.getSrcNodeID()),
00046             iface_index(s.getIfaceIndex()),
00047             msg(s)
00048         { }
00049     };
00050 
00051     std::vector<DataType> simple;
00052     std::vector<ReceivedDataStructureCopy> extended;
00053 
00054     void receiveExtended(ReceivedDataStructure& msg)
00055     {
00056         extended.push_back(msg);
00057     }
00058 
00059     void receiveSimple(DataType& msg)
00060     {
00061         simple.push_back(msg);
00062     }
00063 
00064     typedef SubscriptionListener<DataType> SelfType;
00065     typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (ReceivedDataStructure&)> ExtendedBinder;
00066     typedef uavcan::MethodBinder<SelfType*, void(SelfType::*) (DataType&)> SimpleBinder;
00067 
00068     ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
00069     SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
00070 };
00071 
00072 static void writeServiceServerCallback(
00073     const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,
00074     uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp)
00075 {
00076     std::cout << req << std::endl;
00077     rsp.error = rsp.ERROR_UNKNOWN;
00078 }
00079 
00080 TEST(CanAcceptanceFilter, Basic_test)
00081 {
00082     uavcan::GlobalDataTypeRegistry::instance().reset();
00083     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1;
00084     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2;
00085     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3;
00086     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4;
00087     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::Solution> _reg5;
00088     uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6;
00089     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7;
00090 
00091     SystemClockDriver clock_driver;
00092     CanDriverMock can_driver(1, clock_driver);
00093     TestNode node(can_driver, clock_driver, 24);
00094 
00095     uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand,
00096                     SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node);
00097     uavcan::Subscriber<uavcan::equipment::air_data::Sideslip,
00098                        SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node);
00099     uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed,
00100                        SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node);
00101     uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack,
00102                        SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node);
00103     uavcan::Subscriber<uavcan::equipment::ahrs::Solution,
00104                        SubscriptionListener<uavcan::equipment::ahrs::Solution>::ExtendedBinder> sub_5(node);
00105     uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
00106                        SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node);
00107     uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
00108                        SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node);
00109     uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node);
00110 
00111     SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1;
00112     SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2;
00113     SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3;
00114     SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4;
00115     SubscriptionListener<uavcan::equipment::ahrs::Solution> listener_5;
00116     SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6;
00117 
00118     sub_1.start(listener_1.bindExtended());
00119     sub_2.start(listener_2.bindExtended());
00120     sub_3.start(listener_3.bindExtended());
00121     sub_4.start(listener_4.bindExtended());
00122     sub_5.start(listener_5.bindExtended());
00123     sub_6.start(listener_6.bindExtended());
00124     sub_6_1.start(listener_6.bindExtended());
00125     server.start(writeServiceServerCallback);
00126     std::cout << "Subscribers are initialized." << std::endl;
00127 
00128     uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10);
00129 
00130     int configure_filters_assert = anon_test_configuration.computeConfiguration();
00131     ASSERT_EQ(configure_filters_assert, 0);
00132     std::cout << "Filters are calculated with anonymous configuration." << std::endl;
00133 
00134     const auto& configure_array = anon_test_configuration.getConfiguration();
00135     uint32_t configure_array_size = configure_array.getSize();
00136     std::cout << "Number of configurations after first time computeConfiguration() invoked: "
00137             << configure_array_size << std::endl;
00138     ASSERT_EQ(configure_array_size, 9);
00139 
00140     std::cout << "Adding two additional configurations ...  " << std::endl;
00141     uavcan::CanFilterConfig aux_config_1, aux_config_2;
00142     aux_config_1.id = 911;
00143     aux_config_1.mask = 1488;
00144     aux_config_2.id = 999999;
00145     aux_config_2.mask = 849128412;
00146     configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1);
00147     ASSERT_EQ(configure_filters_assert, 0);
00148     configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2);
00149     ASSERT_EQ(configure_filters_assert, 0);
00150     configure_array_size = configure_array.getSize();
00151     std::cout << "New configuration anon_container size: " << configure_array_size << std::endl;
00152     ASSERT_EQ(configure_array_size, 11);
00153 
00154     std::cout << "Applying configuration ...  " << std::endl;
00155     configure_filters_assert = anon_test_configuration.applyConfiguration();
00156     ASSERT_EQ(configure_filters_assert, 0);
00157     std::cout << "Filters are configured." << std::endl;
00158     configure_array_size = configure_array.getSize();
00159     std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl;
00160     ASSERT_EQ(configure_array_size, 10);
00161 
00162     for (uint16_t i = 0; i<configure_array_size; i++)
00163     {
00164         std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
00165         std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
00166     }
00167 
00168     ASSERT_EQ(configure_array.getByIndex(0)->id, 911);
00169     ASSERT_EQ(configure_array.getByIndex(0)->mask, 1488);
00170     ASSERT_EQ(configure_array.getByIndex(1)->id, 999999);
00171     ASSERT_EQ(configure_array.getByIndex(1)->mask, 849128412);
00172     ASSERT_EQ(configure_array.getByIndex(2)->id, 2147746048);
00173     ASSERT_EQ(configure_array.getByIndex(2)->mask, 3774873472);
00174     ASSERT_EQ(configure_array.getByIndex(3)->id, 2147744768);
00175     ASSERT_EQ(configure_array.getByIndex(3)->mask, 3774873472);
00176     ASSERT_EQ(configure_array.getByIndex(4)->id, 2147739648);
00177     ASSERT_EQ(configure_array.getByIndex(4)->mask, 3774873472);
00178     ASSERT_EQ(configure_array.getByIndex(5)->id, 2147746816);
00179     ASSERT_EQ(configure_array.getByIndex(5)->mask, 3774873472);
00180     ASSERT_EQ(configure_array.getByIndex(6)->id, 2147746304);
00181     ASSERT_EQ(configure_array.getByIndex(6)->mask, 3774873472);
00182     ASSERT_EQ(configure_array.getByIndex(7)->id, 2147483648);
00183     ASSERT_EQ(configure_array.getByIndex(7)->mask, 3758096639);
00184     ASSERT_EQ(configure_array.getByIndex(8)->id, 2147489920);
00185     ASSERT_EQ(configure_array.getByIndex(8)->mask, 3758129024);
00186     ASSERT_EQ(configure_array.getByIndex(9)->id, 2147749888);
00187     ASSERT_EQ(configure_array.getByIndex(9)->mask, 3774873472);
00188 
00189     uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node, 4);
00190     configure_filters_assert = no_anon_test_confiruration.computeConfiguration
00191                                    (uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages);
00192     ASSERT_EQ(configure_filters_assert, 0);
00193     std::cout << "Filters are configured without anonymous configuration." << std::endl;
00194 
00195     configure_filters_assert = no_anon_test_confiruration.applyConfiguration();
00196     ASSERT_EQ(configure_filters_assert, 0);
00197     std::cout << "Filters are configured without anonymous configuration." << std::endl;
00198 
00199     const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration();
00200     configure_array_size = configure_array_2.getSize();
00201     ASSERT_EQ(configure_filters_assert, 0);
00202     ASSERT_EQ(configure_array_size, 4);
00203 
00204     for (uint16_t i = 0; i<configure_array_size; i++)
00205     {
00206         std::cout << "config.ID [" << i << "] = " << configure_array_2.getByIndex(i)->id << std::endl;
00207         std::cout << "config.MK [" << i << "] = " << configure_array_2.getByIndex(i)->mask << std::endl;
00208     }
00209 
00210     ASSERT_EQ(configure_array_2.getByIndex(0)->id, 2147739648);
00211     ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 3774868352);
00212     ASSERT_EQ(configure_array_2.getByIndex(1)->id, 2147745792);
00213     ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 3774872704);
00214     ASSERT_EQ(configure_array_2.getByIndex(2)->id, 2147489920);
00215     ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 3758129024);
00216     ASSERT_EQ(configure_array_2.getByIndex(3)->id, 2147745792);
00217     ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 3774868352);
00218 }
00219 #endif