when I call Telemetry::home(), I get nan values. I think this is why Telemetry::health() returns not armable because is_home_position_ok is false .
While running QGC just to play with the system, I am running the application (shown below) on the Pi. This is the result:
disar mode=Manual at 0 0 hdg=0 alpha=0 beta=0 psi=0 home=nan nan arm=n g=n a=n m=n p=n g=n h=n
armed mode=Manual at 30380 -97726 hdg=26 alpha=0 beta=-179 psi=26 home=nan nan arm=n g=y a=y m=y p=y g=y h=n
armed mode=Manual at 30380 -97726 hdg=26 alpha=0 beta=-179 psi=26 home=nan nan arm=n g=y a=y m=y p=y g=y h=n
armed mode=Manual at 30380 -97726 hdg=26 alpha=0 beta=-179 psi=26 home=nan nan arm=n g=y a=y m=y p=y g=y h=n
//
// Demonstrates how to get basic sensor data using the MAVSDK.
// build using command: g++ -std=c++17 proto_mavsdk.cpp -I/usr/local/include/mavsdk -lmavsdk -o proto_mavsdk
//
#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";
}
//
// Does Offboard control using NED co-ordinates.
//
// returns true if everything went well in Offboard control
//
bool offb_ctrl_ned(mavsdk::Offboard& offboard)
{
std::cout << "Starting Offboard velocity control in NED coordinates\n";
// Send it once before starting offboard, otherwise it will be rejected.
const Offboard::VelocityNedYaw stay{};
offboard.set_velocity_ned(stay);
Offboard::Result offboard_result = offboard.start();
if (offboard_result != Offboard::Result::Success) {
std::cerr << "Offboard start failed: " << offboard_result << '\n';
return false;
}
std::cout << "Offboard started\n";
std::cout << "Turn to face East\n";
Offboard::VelocityNedYaw turn_east{};
turn_east.yaw_deg = 90.0f;
offboard.set_velocity_ned(turn_east);
sleep_for(std::chrono::seconds(1)); // Let yaw settle.
{
const float step_size = 0.01f;
const float one_cycle = 2.0f * (float)M_PI;
const unsigned steps = 2 * unsigned(one_cycle / step_size);
std::cout << "Go North and back South\n";
for (unsigned i = 0; i < steps; ++i) {
float vx = 5.0f * sinf(i * step_size);
Offboard::VelocityNedYaw north_and_back_south{};
north_and_back_south.north_m_s = vx;
north_and_back_south.yaw_deg = 90.0f;
offboard.set_velocity_ned(north_and_back_south);
sleep_for(std::chrono::milliseconds(10));
}
}
std::cout << "Turn to face West\n";
Offboard::VelocityNedYaw turn_west{};
turn_west.yaw_deg = 270.0f;
offboard.set_velocity_ned(turn_west);
sleep_for(std::chrono::seconds(2));
std::cout << "Go up 2 m/s, turn to face South\n";
Offboard::VelocityNedYaw up_and_south{};
up_and_south.down_m_s = -2.0f;
up_and_south.yaw_deg = 180.0f;
offboard.set_velocity_ned(up_and_south);
sleep_for(std::chrono::seconds(4));
std::cout << "Go down 1 m/s, turn to face North\n";
Offboard::VelocityNedYaw down_and_north{};
up_and_south.down_m_s = 1.0f;
offboard.set_velocity_ned(down_and_north);
sleep_for(std::chrono::seconds(4));
offboard_result = offboard.stop();
bool success = offboard_result == Offboard::Result::Success;
if (!success) {
std::cerr << "Offboard stop failed: " << offboard_result << '\n';
}
else
{
std::cout << "Offboard stopped\n";
}
return success;
}
bool control_actuator(mavsdk::Action &action, int index, int value)
{
const Action::Result set_actuator_result = action.set_actuator(index, value);
bool success = set_actuator_result == Action::Result::Success;
if(success) {
std::cerr << "Set actuator #" << index << " to " << value << ".\n";
} else {
std::cerr << "Setting actuator #" << index << " to " << value << " failed:" << set_actuator_result << "\n";
}
return success;
}
bool get_param(mavsdk::Param ¶m, std::string paramname, float *value)
{
auto result = param.get_param_float(paramname);
bool success = result.first == mavsdk::Param::Result::Success;
if (success) {
std::cout << "Found Param " << paramname <<":" << result.second << std::endl;
*value = result.second;
} else {
std::cout << "Got error getting " << paramname << ":" << result.first << std::endl;
}
return success;
}
bool set_param(mavsdk::Param ¶m, std::string paramname, float value)
{
auto result = param.set_param_float(paramname, value);
bool success = result == mavsdk::Param::Result::Success;
if (success) {
std::cout << "Set Param " << paramname << " to" << value << std::endl;
} else {
std::cout << "Error setting " << paramname << ":" << result << std::endl;
}
return success;
}
bool set_param(mavsdk::Param ¶m, std::string paramname, int value)
{
auto result = param.set_param_int(paramname, value);
bool success = result == mavsdk::Param::Result::Success;
if (success) {
std::cout << "Set Param " << paramname << " to" << value << std::endl;
} else {
std::cout << "Error setting " << paramname << ":" << result << std::endl;
}
return success;
}
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));
if(nHealthTry == 2) {
telemetry.set_rate_home(1.0);
const Offboard::VelocityNedYaw homecommandish{};
offboard.set_velocity_ned(homecommandish);
}
if(nHealthTry == 4) {
std::cout << "System not ready, maybe need to takeoff!\n" ;
action.takeoff();
}
if(nHealthTry++>8) {
std::cout << "System not ready but doing stuff anyways\n" ;
break;
}
}
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::string paramname1 = "ACRO_TURN_RATE";
// std::string paramname2 = "AHRS_GPS_MINSATS";
while (n < 10) {
std::this_thread::sleep_for(std::chrono::seconds(2));
n++;
minsats += 1.0;
turnrate += 1.0;
std::cout <<"=== " << n << " doing stuff\n";
// success = get_param(param, paramname1, &temp);
// success = set_param(param, paramname1, turnrate);
// success = set_param(param, paramname2, temp2);
int delta = n > 5 ? (-80*(n-5)) : (80*n);
success = control_actuator(action, gripperIndex, gripperValue+n);
success = control_actuator(action, mastIndex, mastValue+n);
print_stuff(telemetry);
}
success = offb_ctrl_ned(offboard);
if(!success) {
std::cerr << "Controlling heading failed:" << "?" << '\n';
}
std::cout << "Stopping the prototype program\n";
telemetry.unsubscribe_attitude_euler(attitudeHandle);
print_stuff(telemetry);
std::cout << "\nHave a nice day\n";
}