I am using MavSDK in C++ to software that runs on a second Pi on the vehicle to control things autonomously. Forgive the newbie question, but how to access the thrusters. For instance, if I just want to turn the left thruster on for five seconds in order to test it, I can’t find exactly the right information on what method to use.
Here is what I have tried:
// Demonstrates how to move a boat around using the MAVSDK.
// build using command: g++ -std=c++17 proto_nav.cpp -I/usr/local/include/mavsdk -lmavsdk -o proto_nav
//
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/param/param.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/action/action.h>
#include <chrono>
#include <functional>
#include <future>
#include <iostream>
#include <thread>
using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;
void usage(const std::string& bin_name)
{
std::cerr << "Usage : " << bin_name << " <connection_url>\n"
<< "Connection URL format should be :\n"
<< " For TCP : tcp://[server_host][:server_port]\n"
<< " For UDP : udp://[bind_host][:bind_port]\n"
<< " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
<< "For example, to connect to the running autopilot on the BlueOS board use URL: udp://127.0.0.0:5777\n";
}
void print_stuff(mavsdk::Telemetry &telem)
{
Telemetry::FlightMode fmode = telem.flight_mode();
bool isarmed = telem.armed();
Telemetry::Position currentPos = telem.position();
Telemetry::Heading heading = telem.heading();
Telemetry::EulerAngle euler = telem.attitude_euler();
Telemetry::Position hpos = telem.home();
auto health = telem.health();
std::cout << (isarmed?"armed":"disar") << " mode=" << fmode
<< " at " << (int)(1000*currentPos.latitude_deg) << " " << (int)(1000*currentPos.longitude_deg)
<< " hdg=" << (int)heading.heading_deg
<< " alpha=" << (int)euler.pitch_deg << " beta=" << (int) euler.roll_deg << " psi=" <<(int)euler.yaw_deg
<< " home=" << (hpos.latitude_deg) << " " << (hpos.longitude_deg)
<< " arm=" << (health.is_armable?"y":"n" )
<< " g=" << (health.is_gyrometer_calibration_ok?"y":"n" )
<< " a=" << (health.is_accelerometer_calibration_ok?"y":"n" )
<< " m=" << (health.is_magnetometer_calibration_ok?"y":"n" )
<< " p=" << (health.is_local_position_ok?"y":"n" )
<< " g=" << (health.is_global_position_ok?"y":"n" )
<< " h=" << (health.is_home_position_ok?"y":"n" )
<< std::endl << std::flush;
}
int main(int argc, char** argv)
{
int maxN = 20;
std::string url = "tcp://127.0.0.1:5777";
std::string appname = argv[0];
bool success = false;
bool monitor_only = false;
if(argc >= 2 && strcmp(argv[1], "help") == 0) {
usage(appname);
return 1;
}
if (argc >= 2) {
if(strcmp(argv[1], "-m") == 0)
{
monitor_only = true;
}
else
{
url.assign(argv[1]);
}
}
if(argc >= 3 && strcmp(argv[2], "-m") == 0) {
monitor_only = true;
}
std::cout << "trying to run " << appname << " against autopilot at " << url
<< ", try the argument 'help' for more info." << std::endl;
mavsdk::Mavsdk mavsdk{mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::GroundStation}};
mavsdk::ConnectionResult connection_result = mavsdk.add_any_connection(url);
if (connection_result != mavsdk::ConnectionResult::Success) {
std::cerr << "Connection failed: " << connection_result << '\n';
return 1;
}
auto system = mavsdk.first_autopilot(3.0);
if (!system) {
std::cerr << "Timed out waiting for system\n";
return 1;
}
sleep_for(std::chrono::seconds(1));
std::cout << "Hello USV\n";
auto telemetry = mavsdk::Telemetry{system.value()};
auto param = mavsdk::Param{system.value()};
auto offboard = mavsdk::Offboard{system.value()};
auto action = Action{system.value()};
if(monitor_only)
{
while(true)
{
print_stuff(telemetry);
sleep_for(std::chrono::seconds(5));
}
}
int nHealthTry = 0;
while (!telemetry.health_all_ok()) {
std::cout << "Waiting for system to be ready \n";
print_stuff(telemetry);
sleep_for(std::chrono::seconds(1));
}
std::cout << "Here we go\n";
print_stuff(telemetry);
// We want to listen to the attitude of the drone at 1 Hz.
auto set_rate_result = telemetry.set_rate_attitude_euler(2.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 altitude while the vehicle is in flight
auto attitudeHandle = telemetry.subscribe_attitude_euler([](mavsdk::Telemetry::EulerAngle attitude) {
static int nAttitude = 0;
if(nAttitude++ % 10 == 0) {std::cout << "\n";}
std::cout << "got #" << nAttitude << " Pitch: " << attitude.pitch_deg << " deg" << " Roll: " << attitude.roll_deg << " deg\n";
});
int n = 0;
int gripperIndex = 15;
int mastIndex = 16;
int gripperValue = 1500;
int mastValue = 3000;
float turnrate = 0.0;
float minsats = 1.0;
float temp = 0.0;
int temp2 = 11;
std::cout << "Arming\n";
action.arm();
sleep_for(std::chrono::seconds(1));
print_stuff(telemetry);
for(int i=0;i<100;i++)
{
action.set_actuator(0, 0.5);
sleep_for(std::chrono::milliseconds(50));
}
print_stuff(telemetry);
action.set_actuator(0, 1500);
print_stuff(telemetry);
sleep_for(std::chrono::seconds(1));
std::cout << "Disarming\n";
action.disarm();
print_stuff(telemetry);
std::cout << "Stopping the prototype program\n";
telemetry.unsubscribe_attitude_euler(attitudeHandle);
print_stuff(telemetry);
std::cout << "\nHave a nice day\n";
}