static void
position_changed_cb(void *data, struct sol_mavlink *mavlink)
{
int err;
struct sol_mavlink_position pos;
err = sol_mavlink_get_current_position(mavlink, &pos);
if (err < 0) {
SOL_ERR("Could not get current position: %s", sol_util_strerrora(-err));
return;
}
if (sol_mavlink_is_armed(mavlink))
printf("lat: %f, long: %f, alt: %f\n", pos.latitude, pos.longitude,
pos.altitude);
}
static void
mission_reached_cb(void *data, struct sol_mavlink *mavlink)
{
int err;
enum sol_mavlink_mode mode;
struct sol_mavlink_position home, curr;
struct sol_mavlink_position dest = { DEST_LAT, DEST_LONG, DEST_ALT };
mode = sol_mavlink_get_mode(mavlink);
err = sol_mavlink_get_current_position(mavlink, &curr);
if (err < 0) {
SOL_ERR("Could not get current position: %s", sol_util_strerrora(-err));
return;
}
if (mode != SOL_MAVLINK_MODE_GUIDED)
return;
if (GT_MARGIN(curr.altitude, DEST_ALT)) {
printf(">>>> Going back home.\n");
err = sol_mavlink_get_home_position(mavlink, &home);
if (err < 0) {
SOL_ERR("Could not get home position: %s", sol_util_strerrora(-err));
return;
}
err = sol_mavlink_land(mavlink, &home);
if (err < 0) {
SOL_ERR("Could not land vehicle: %s", sol_util_strerrora(-err));
}
} else if (GT_MARGIN(curr.altitude, TAKEOFF_ALT)) {
err = sol_mavlink_go_to(mavlink, &dest);
if (err < 0) {
SOL_ERR("Could not send vehicle to: (%f, %f, %f) - %s",
dest.latitude, dest.longitude, dest.altitude,
sol_util_strerrora(-err));
return;
}
printf(">>>> Successful takeoff, starting a new mission, heading to: "
"(%f, %f, %f)\n", dest.latitude, dest.longitude, dest.altitude);
}
}
