mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 08:16:34 +00:00
Use MAVSDK::RawMission in AzMissionController
TODO!! - send mission indexes from AzMissionController to AzDroneController - handle finishing of the mission
This commit is contained in:
Tuomas Järvinen
@@ -1,121 +1,166 @@
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
|
||||
#include <mavsdk/mavsdk.h>
|
||||
#include <mavsdk/plugins/action/action.h>
|
||||
#include <mavsdk/plugins/mission/mission.h>
|
||||
#include <mavsdk/plugins/telemetry/telemetry.h>
|
||||
#include <mavsdk/plugins/mission_raw/mission_raw.h>
|
||||
#include <mavsdk/plugins/offboard/offboard.h>
|
||||
|
||||
#include "az_mission.h"
|
||||
#include "az_mission_controller.h"
|
||||
#include "az_utils.h"
|
||||
|
||||
AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
|
||||
|
||||
AzMissionController::AzMissionController(System &system, AzMission &mission, QObject *parent)
|
||||
: QObject(parent)
|
||||
, mMission(mission)
|
||||
, mAction(nullptr)
|
||||
, mAzMission(mission)
|
||||
, mMissionStarted(false)
|
||||
, mFlyingToReturnPoint(false)
|
||||
, mAbsoluteAltitude(-10000)
|
||||
, mMissionItemSeqNum(0)
|
||||
, mMissionRaw(nullptr)
|
||||
, mSystem(system)
|
||||
{}
|
||||
|
||||
void AzMissionController::setAction(Action *action)
|
||||
|
||||
MissionRaw::MissionItem AzMissionController::makeRawMissionItem(
|
||||
float latitudeDeg1e7,
|
||||
float longitudeDeg1e7,
|
||||
int32_t altitude,
|
||||
float doPhoto,
|
||||
MAV_FRAME frame,
|
||||
MAV_CMD command,
|
||||
float p2,
|
||||
float p3)
|
||||
{
|
||||
mAction = action;
|
||||
}
|
||||
(void)p2;
|
||||
(void)p3;
|
||||
|
||||
bool AzMissionController::startMissions(float absoluteAltitude)
|
||||
{
|
||||
mCurrentMissionIndex = -1;
|
||||
mMissionStarted = true;
|
||||
mAbsoluteAltitude = absoluteAltitude; // TODO!! Use altitudes from the JSON file.
|
||||
return flyToNextMissionItem();
|
||||
}
|
||||
MissionRaw::MissionItem new_item{};
|
||||
new_item.seq = mMissionItemSeqNum;
|
||||
new_item.frame = static_cast<uint32_t>(frame);
|
||||
new_item.command = static_cast<uint32_t>(command);
|
||||
new_item.param1 = doPhoto;
|
||||
new_item.x = latitudeDeg1e7 * 1e7;
|
||||
new_item.y = longitudeDeg1e7 * 1e7;
|
||||
new_item.z = altitude;
|
||||
new_item.mission_type = MAV_MISSION_TYPE_MISSION;
|
||||
new_item.autocontinue = 1;
|
||||
|
||||
void AzMissionController::stopMissions(void)
|
||||
{
|
||||
// TODO!! Needs to be improved. At least send signal to AzDroneController.
|
||||
mMissionStarted = false;
|
||||
}
|
||||
|
||||
bool AzMissionController::flyToNextMissionItem(void)
|
||||
{
|
||||
mCurrentMissionIndex++;
|
||||
|
||||
if (mCurrentMissionIndex == (int) mMission.getActionPoints().size()) {
|
||||
mFlyingToReturnPoint = true;
|
||||
qDebug() << "AzMissionController::flyToNextMissionItem() All action points handled.";
|
||||
qDebug() << "AzMissionController::flyToNextMissionItem() Flying to the JSON return point.";
|
||||
const AzCoordinate &coordinate = mMission.getReturnPoint();
|
||||
Action::Result result
|
||||
= mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
|
||||
|
||||
if (result == Action::Result::Success) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
cerr << "mAction->goto_location() failed. Reason: " << result << endl;
|
||||
return false;
|
||||
}
|
||||
if (mMissionItemSeqNum == 1) {
|
||||
new_item.current = 1;
|
||||
}
|
||||
|
||||
qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
|
||||
<< mMission.getActionPoints().size();
|
||||
mMissionItemSeqNum++;
|
||||
|
||||
const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
|
||||
qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
|
||||
<< coordinate.longitude;
|
||||
return new_item;
|
||||
}
|
||||
|
||||
Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
|
||||
|
||||
// TODO!! Check return value and print warnings and errors.
|
||||
if (result == Action::Result::Success) {
|
||||
emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
|
||||
return true;
|
||||
bool AzMissionController::uploadMission(void)
|
||||
{
|
||||
cout << "[MISSION] stateUploadMission() Setting raw mission starts" << endl;
|
||||
|
||||
mMissionItemSeqNum = 0;
|
||||
|
||||
if (mMissionRaw == nullptr) {
|
||||
mMissionRaw = new MissionRaw(mSystem);
|
||||
}
|
||||
else {
|
||||
cerr << "mAction->goto_location() failed. Reason: " << result << endl;
|
||||
|
||||
auto clearResult = mMissionRaw->clear_mission();
|
||||
if (clearResult != MissionRaw::Result::Success) {
|
||||
std::cout << "[MISSION] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
auto downloadResult = mMissionRaw->download_mission();
|
||||
if (downloadResult.first != MissionRaw::Result::Success) {
|
||||
std::cout << "[MISSION] stateUploadMission() Downloading mission failed" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// First point in case of ArduPilot is always home
|
||||
auto missionPlan = downloadResult.second;
|
||||
MissionRaw::MissionItem homePoint = missionPlan[0];
|
||||
missionPlan.clear();
|
||||
|
||||
// Going relative alt mission so we dont care about altitude
|
||||
auto latDeg = homePoint.x * 1e-7;
|
||||
auto lonDeg = homePoint.y * 1e-7;
|
||||
cout << "[MISSION] Home location: " << latDeg << ", " << lonDeg << endl;
|
||||
|
||||
// In case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
|
||||
missionPlan.push_back(makeRawMissionItem(
|
||||
0, // lat
|
||||
0, // lon
|
||||
50.0,
|
||||
0,
|
||||
MAV_FRAME_GLOBAL_RELATIVE_ALT,
|
||||
MAV_CMD_NAV_TAKEOFF));
|
||||
|
||||
// Setup speed during mission execution
|
||||
missionPlan.push_back(makeRawMissionItem(
|
||||
0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));
|
||||
|
||||
// Action points
|
||||
const vector<AzActionPoint> &azMissions = mAzMission.getActionPoints();
|
||||
for (uint i = 0; i < azMissions.size(); i++) {
|
||||
missionPlan.push_back(makeRawMissionItem(
|
||||
azMissions[i].getPoint().latitude,
|
||||
azMissions[i].getPoint().longitude,
|
||||
azMissions[i].getHeight(),
|
||||
0,
|
||||
MAV_FRAME_GLOBAL_TERRAIN_ALT,
|
||||
MAV_CMD_NAV_WAYPOINT));
|
||||
}
|
||||
|
||||
// Landing point
|
||||
missionPlan.push_back(makeRawMissionItem(
|
||||
mAzMission.getReturnPoint().latitude,
|
||||
mAzMission.getReturnPoint().longitude,
|
||||
50.00,
|
||||
0,
|
||||
MAV_FRAME_GLOBAL_TERRAIN_ALT,
|
||||
MAV_CMD_NAV_LAND));
|
||||
|
||||
//missionPlan.push_back(makeRawMissionItem(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));
|
||||
|
||||
for (const auto& item : missionPlan) {
|
||||
std::cout << "[MISSION] stateUploadMission() seq: " << (int)item.seq << '\n';
|
||||
}
|
||||
|
||||
auto uploadResult = mMissionRaw->upload_mission(missionPlan);
|
||||
if (uploadResult != MissionRaw::Result::Success) {
|
||||
std::cout << "[MISSION] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
mMissionRaw->set_current_mission_item(0);
|
||||
cout << "[MISSION] stateUploadMission() Setting raw mission ends" << endl;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AzMissionController::startMission()
|
||||
{
|
||||
MissionRaw::Result startResult = mMissionRaw->start_mission();
|
||||
cout << "[MISSION] Start mission result:" << startResult << endl;
|
||||
return startResult == MissionRaw::Result::Success;
|
||||
}
|
||||
|
||||
|
||||
void AzMissionController::stopMission(void)
|
||||
{
|
||||
mMissionRaw->pause_mission();
|
||||
}
|
||||
|
||||
|
||||
void AzMissionController::newPosition(Telemetry::Position pos)
|
||||
{
|
||||
(void)pos;
|
||||
|
||||
if (mMissionStarted == false) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (mFlyingToReturnPoint == true) {
|
||||
const AzCoordinate &point = mMission.getReturnPoint();
|
||||
float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, point.latitude, point.longitude);
|
||||
|
||||
qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km.";
|
||||
|
||||
if (distance <= 0.01) {
|
||||
qDebug() << "AzMissionController::newPosition() return point reached. Mission finished.";
|
||||
mMissionStarted = false;
|
||||
mFlyingToReturnPoint = false;
|
||||
emit finished();
|
||||
}
|
||||
}
|
||||
else {
|
||||
const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
|
||||
float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);
|
||||
|
||||
qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km.";
|
||||
|
||||
// TODO!! In final application we need to use the camera to find the target.
|
||||
if (QCoreApplication::arguments().contains("quadcopter") && distance <= 0.01) {
|
||||
qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
|
||||
flyToNextMissionItem();
|
||||
}
|
||||
else if (QCoreApplication::arguments().contains("plane")) {
|
||||
if (mPlaneCirclingTime.isValid() == false && distance <= 0.1) {
|
||||
qDebug() << "AzMissionController::newPosition() target reached. Starting circling.";
|
||||
mPlaneCirclingTime.restart();
|
||||
}
|
||||
else if (mPlaneCirclingTime.elapsed() > 35000) {
|
||||
qDebug() << "AzMissionController::newPosition() target reached. Ending circling.";
|
||||
mPlaneCirclingTime.invalidate();
|
||||
flyToNextMissionItem();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user