Skip to content

Commit

Permalink
code style
Browse files Browse the repository at this point in the history
  • Loading branch information
Ilia-Loginov committed Aug 14, 2024
1 parent 9fd733a commit 75157f9
Showing 1 changed file with 50 additions and 45 deletions.
95 changes: 50 additions & 45 deletions test/mavsdk_tests/mavsdk_preparing.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,7 @@ using std::chrono::seconds;
using std::this_thread::sleep_for;

template<typename Rep>
bool poll_condition_with_timeout_(
std::function<bool()> fun, std::chrono::duration<Rep> duration)
bool poll_condition_with_timeout(std::function<bool()> fun, std::chrono::duration<Rep> duration)
{
static constexpr unsigned check_resolution = 100;

Expand All @@ -32,44 +31,49 @@ bool poll_condition_with_timeout_(
while (!fun()) {
std::this_thread::sleep_for(duration_us / check_resolution);
const auto elapsed_time_us = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() -
start_time);
start_time);

if (elapsed_time_us > duration_us) {
std::cout << "Timeout, waiting for the vehicle for "
<< elapsed_time_us.count() * std::chrono::steady_clock::period::num
/ static_cast<double>(std::chrono::steady_clock::period::den)
<< " seconds\n";
<< elapsed_time_us.count() * std::chrono::steady_clock::period::num
/ static_cast<double>(std::chrono::steady_clock::period::den)
<< " seconds\n";
return false;
}
}

return true;
}

int main(int argc, char **argv)
{
const std::string arg_url {"--url"};
const std::string arg_command {"--command"};
const std::string command_set_param {"set_sys_autostart"};
const std::string command_check {"check"};
const std::string command_reboot {"reboot"};

std::string connection_url {};
std::string command {};
int param_value {};
const std::string arg_url{"--url"};
const std::string arg_command{"--command"};
const std::string command_set_param{"set_sys_autostart"};
const std::string command_check{"check"};
const std::string command_reboot{"reboot"};

std::string connection_url{};
std::string command{};
int param_value{};
mavsdk::Mavsdk mavsdk{};

for (int i = 0; i < argc; ++i) {
const std::string argv_string(argv[i]);

if (argv_string == arg_url && (argc > (i + 1))) {
connection_url = argv[i + 1];
i++;
}
else if (argv_string == arg_command && (argc > (i + 1))) {

} else if (argv_string == arg_command && (argc > (i + 1))) {
command = argv[i + 1];
i++;

if (command == command_set_param) {
if ((argc > (i + 1))){
if ((argc > (i + 1))) {
param_value = std::atoi(argv[i + 1]);
i++;

} else {
command.erase();
}
Expand All @@ -83,80 +87,81 @@ int main(int argc, char **argv)
return 1;
}

Mavsdk mavsdk{};
std::cout << "connection url " << connection_url << std::endl;
std::cout << "Connection url " << connection_url << std::endl;
ConnectionResult connection_result = mavsdk.add_any_connection(connection_url);
if (connection_result != ConnectionResult::Success)
{

if (connection_result != ConnectionResult::Success) {
std::cerr << "Connect was failed" << std::endl;
return 1;
}

std::cout << "Waiting for system connect" << std::endl;

if(!poll_condition_with_timeout_(
[&]() { return mavsdk.systems().size() > 0; }, std::chrono::seconds(25)))
{

auto res_polling = poll_condition_with_timeout([&]() { return mavsdk.systems().size() > 0;}, std::chrono::seconds(25));

if (not res_polling) {
std::cerr << "Polling was failed" << std::endl;
return 1;
}

auto system = mavsdk.systems().at(0);

if(!system)
{
if (!system) {
std::cerr << "The system wasn't be found" << std::endl;
return 1;
}

if (command_check == command)
{
if (command_check == command) {
std::cout << "Success! The connection was checked." << std::endl;
return 0;
}

if (command_set_param == command)
{
if (command_set_param == command) {
//TODO: For some reason when we set the new value it sometimes takes several attempts to change a parameter.
int attempt = 1;
const int max_attemp = 5;
const auto param_name = "SYS_AUTOSTART";
for(auto i = 0; i <= max_attemp; i++)
{

for (attempt = 0; attempt <= max_attemp; attempt++) {
using namespace std::chrono_literals;
auto param = Param(system);


if (param.set_param_int(param_name, param_value) != Param::Result::Success)
{
if (param.set_param_int(param_name, param_value) != Param::Result::Success) {
std::cerr << "Fail setting param: " << param_name << ", value: "
<< param_value <<std::endl;
<< param_value << std::endl;
return 1;
}

auto res = param.get_param_int(param_name);
if (res.first == Param::Result::Success)
if (res.second == param_value)
break;

if (res.first == Param::Result::Success && res.second == param_value) {
break;
}

std::this_thread::sleep_for(1s);
}
if (i <= max_attemp) {

if (attempt <= max_attemp) {
std::cout << "Param " << param_name << " was successly set after " << attempt
<< " attempt. Tne new value " << param_value << std::endl;
<< " attempt. Tne new value " << param_value << std::endl;

} else {
std::cerr << "Fail. Setting param: " << param_name << " wasn't set to value: "
<< param_value << "after " << max_attemp << " attempts. " <<std::endl;
return 1;
<< param_value << "after " << max_attemp << " attempts. " << std::endl;
return 1;
}
}

if (command_reboot == command) {
auto action = Action{system};
if ( Action::Result::Success != action.reboot())
{

if (Action::Result::Success != action.reboot()) {
std::cerr << "Reboot doesn't work" << std::endl;
return 1;
}

std::cout << "Rebooting...\n";
}

Expand Down

0 comments on commit 75157f9

Please sign in to comment.