static void mavlink_connect_cb(void *data, struct sol_mavlink *mavlink) { int err; enum sol_mavlink_mode mode; SOL_INF("mavlink connection stablished"); mode = sol_mavlink_get_mode(mavlink); if (mode != SOL_MAVLINK_MODE_GUIDED) { err = sol_mavlink_set_mode(mavlink, SOL_MAVLINK_MODE_GUIDED); if (err < 0) { SOL_ERR("Could not set mode: %s", sol_util_strerrora(-err)); } return; } if (!sol_mavlink_check_armed(mavlink)) { err = sol_mavlink_set_armed(mavlink, true); if (err < 0) { SOL_ERR("Could not arm vechicle: %s", sol_util_strerrora(-err)); } return; } takeoff(mavlink); }
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 disarmed_cb(void *data, struct sol_mavlink *mavlink) { enum sol_mavlink_mode mode; mode = sol_mavlink_get_mode(mavlink); if (mode == SOL_MAVLINK_MODE_LAND) printf(">>>> Landed...\n"); }
static void armed_cb(void *data, struct sol_mavlink *mavlink) { enum sol_mavlink_mode mode; SOL_DBG("vehicle just armed"); mode = sol_mavlink_get_mode(mavlink); if (mode == SOL_MAVLINK_MODE_GUIDED) { takeoff(mavlink); } }
static void mode_changed_cb(void *data, struct sol_mavlink *mavlink) { int err; enum sol_mavlink_mode mode = sol_mavlink_get_mode(mavlink); bool armed = sol_mavlink_check_armed(mavlink); if (mode == SOL_MAVLINK_MODE_GUIDED && !armed) { err = sol_mavlink_set_armed(mavlink, true); if (err < 0) { SOL_ERR("Could not arm vechicle: %s", sol_util_strerrora(-err)); } } }
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); } }