static void mission_reached_cb(void *data, struct sol_mavlink *mavlink) { int err; enum sol_mavlink_mode mode; struct sol_mavlink_position home; mode = sol_mavlink_get_mode(mavlink); err = sol_mavlink_get_home_position(mavlink, &home); if (err < 0) { SOL_ERR("Could not get home position: %s", sol_util_strerrora(-err)); return; } if (mode != SOL_MAVLINK_MODE_GUIDED) return; err = sol_mavlink_land(mavlink, &home); if (err < 0) { SOL_ERR("Could not land vehicle: %s", sol_util_strerrora(-err)); return; } printf(">>>> Successful takeoff, now landing.\n"); }
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_curr_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_goto(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); } }