Open
Description
MAVSDK version: v2.12.12-7-g7f2a9dde
Hello,
I have a little issue by using the Param-Server
I have setup a code where contnuisly a set of parameters is updated.
So if I start, the value is set fine, but setting it another time the client does not update the value.
Also I tried it in the autopilotserver but same happens here.
May somebody help me.
int main(int argc, char** argv)
{
// We run the server plugins on a seperate thread so we can use the main
// thread as a ground station.
std::thread autopilotThread([]() {
mavsdk::Mavsdk mavsdkTester{
mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::Autopilot}};
auto result = mavsdkTester.add_any_connection("udp://127.0.0.1:14551");
if (result == mavsdk::ConnectionResult::Success) {
std::cout << "Connected autopilot server side!" << std::endl;
}
auto server_component = mavsdkTester.server_component();
// Create server plugins
auto paramServer = mavsdk::ParamServer{server_component};
auto telemServer = mavsdk::TelemetryServer{server_component};
auto actionServer = mavsdk::ActionServer{server_component};
// These are needed for MAVSDK at the moment
paramServer.provide_param_int("CAL_ACC0_ID", 1);
paramServer.provide_param_int("CAL_GYRO0_ID", 1);
paramServer.provide_param_int("CAL_MAG0_ID", 1);
paramServer.provide_param_int("SYS_HITL", 0);
paramServer.provide_param_int("MIS_TAKEOFF_ALT", 0);
// Add a custom param
paramServer.provide_param_int("MY_PARAM", 1);
paramServer.provide_param_int("MY_PARAM2", 2);
// Allow the vehicle to change modes, takeoff and arm
actionServer.set_allowable_flight_modes({true, true, true});
actionServer.set_allow_takeoff(true);
actionServer.set_armable(true, true);
// Create vehicle telemetry info
// Publish home already, so that it is available.
// Create a mission raw server
// This will allow us to receive missions from a GCS
// Set current item to complete to progress the current item state
// As we're acting as an autopilot, lets just make the vehicle jump to 10m altitude on
// successful takeoff
int i=0;
while (true) {
i++;
std::this_thread::sleep_for(std::chrono::seconds(1));
// Publish the telemetry
// Just a silly test.
telemServer.publish_unix_epoch_time(i);
paramServer.provide_param_int("MY_PARAM2", i);
std::cout<< "i has the value of "<< i << std::endl;
}
});
// Now this is the main thread, we run client plugins to act as the GCS
// to communicate with the autopilot server plugins.
mavsdk::Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
auto result = mavsdk.add_any_connection("udp://:14551");
if (result == mavsdk::ConnectionResult::Success) {
std::cout << "Connected!" << std::endl;
}
auto prom = std::promise<std::shared_ptr<mavsdk::System>>{};
auto fut = prom.get_future();
Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
auto system = mavsdk.systems().back();
if (system->has_autopilot()) {
std::cout << "Discovered Autopilot from Client" << std::endl;
// Unsubscribe again as we only want to find one system.
mavsdk.unsubscribe_on_new_system(handle);
prom.set_value(system);
} else {
std::cout << "No MAVSDK found" << std::endl;
}
});
if (fut.wait_for(std::chrono::seconds(10)) == std::future_status::timeout) {
std::cout << "No autopilot found, exiting" << std::endl;
return 1;
}
//std::this_thread::sleep_for(std::chrono::seconds(10));
auto system = fut.get();
// Create plugins
auto action = mavsdk::Action{system};
auto param = mavsdk::Param{system};
auto telemetry = mavsdk::Telemetry{system};
//std::this_thread::sleep_for(std::chrono::seconds(5));
auto mission = mavsdk::Mission{system};
std::this_thread::sleep_for(std::chrono::seconds(1));
// Check for our custom param we have set in the server thread
auto res = param.get_param_int("MY_PARAM");
auto result2 = param.get_param_int("MY_PARAM2");
std::cout << "This is PARAM"<< res.second<< std::endl;
std::cout << "This is PARAM2"<< result2.second<< std::endl;
if (res.first == mavsdk::Param::Result::Success) {
std::cout << "Found Param MY_PARAM: " << res.second << std::endl;
}
std::this_thread::sleep_for(std::chrono::seconds(3));
// We want to listen to the altitude of the drone at 1 Hz.
auto set_rate_result = telemetry.set_rate_position(1.0);
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
std::cout << "Setting rate failed:" << set_rate_result << std::endl;
return 1;
}
// Set up callback to monitor Unix time
telemetry.subscribe_unix_epoch_time([](uint64_t time_us) {
std::cout << "Unix epoch time: " << time_us << " us" << std::endl;
});
while (true){
auto resul = param.get_param_int("MY_PARAM2");
std::cout << resul.second<< std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
}
while (true) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
return 0;
}
This is the output:
"
i has the value of 6
Unix epoch time: 6 us
2
i has the value ot 7
Unix epoch time: 7 us
2
i has the value o 8
Unix epoch time: 8 us
2
i has the value o 9
Unix epoch time: 9 us
"