azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 08:06:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use MAVSDK::RawMission in AzMissionController

Browse Source 
TODO!!
- send mission indexes from AzMissionController to AzDroneController
- handle finishing of the mission
This commit is contained in:
Tuomas Järvinen
2024-12-01 21:51:02 +01:00
parent 37e8cfd3fe
commit 38953d0ba6
5 changed files with 170 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/az_drone_controller.cpp
+10 -148
View File
@@ -12,27 +12,16 @@
#include "az_config.h"
#include "az_drone_controller.h"
AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
AzDroneController::AzDroneController(AzMission &azMission, QObject *parent)
: QObject(parent)
, mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
, mDroneState(AZ_DRONE_STATE_DISCONNECTED)
, mMissionRaw(NULL)
, mMissionItemSeqNum(0)
, mMissionController(nullptr)
, mAzMission(azMission)
{
mFirstPosition.relative_altitude_m = -10000;
mMissionController = new AzMissionController(mission, this);
// Mission progress from the AzMissionController. Slot will be used to find the targets later.
connect(
mMissionController,
&AzMissionController::missionProgressChanged,
this,
&AzDroneController::missionIndexChangedSlot);
// Mission controller signals end of the missions. This will be used to fly to the return point in JSON.
connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
// Healt info update from MAVSDK.
connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
@@ -40,40 +29,6 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
}
MissionRaw::MissionItem AzDroneController::makeRawMissionItem(
float latitude_deg1e7,
float longitude_deg1e7,
int32_t altitude_m,
float do_photo,
MAV_FRAME frame,
MAV_CMD command,
float p2,
float p3)
{
(void)p2;
(void)p3;
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 = do_photo;
new_item.x = latitude_deg1e7 * 1e7;
new_item.y = longitude_deg1e7 * 1e7;
new_item.z = altitude_m;
new_item.mission_type = MAV_MISSION_TYPE_MISSION;
new_item.autocontinue = 1;
if (mMissionItemSeqNum == 1) {
new_item.current = 1;
}
mMissionItemSeqNum++;
return new_item;
}
void AzDroneController::start()
{
// Must wait that main event loop is launched in main()
@@ -160,12 +115,7 @@ bool AzDroneController::stateGetActionModule(void)
{
mAction = new Action(mSystem);
if (mAction != nullptr) {
mMissionController->setAction(mAction);
return true;
}
return false;
return mAction == nullptr ? false : true;
}
@@ -183,93 +133,11 @@ bool AzDroneController::stateHealthOk(void)
bool AzDroneController::stateUploadMission(void)
{
cout << "[CONTROLLER] stateUploadMission() Setting raw mission starts" << endl;
mMissionItemSeqNum = 0;
if (mMissionRaw == NULL) {
mMissionRaw = new MissionRaw(*mSystem);
if (mMissionController == nullptr) {
mMissionController = new AzMissionController(*mSystem.get(), mAzMission, this);
}
mMissionRaw->clear_mission();
if (mMissionRaw == NULL) {
mMissionRaw = new MissionRaw(*mSystem);
}
auto clearResult = mMissionRaw->clear_mission();
if (clearResult != MissionRaw::Result::Success) {
std::cout << "[CONTROLLER] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
return false;
}
auto downloadResult = mMissionRaw->download_mission();
if (downloadResult.first != MissionRaw::Result::Success) {
std::cout << "[CONTROLLER] 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;
// 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));
missionPlan.push_back(makeRawMissionItem(
latDeg + 0.001,
lonDeg + 0.001,
50.00,
0,
MAV_FRAME_GLOBAL_TERRAIN_ALT,
MAV_CMD_NAV_WAYPOINT));
missionPlan.push_back(makeRawMissionItem(
latDeg + 0.001,
lonDeg - 0.001,
50.00,
0,
MAV_FRAME_GLOBAL_TERRAIN_ALT,
MAV_CMD_NAV_WAYPOINT));
missionPlan.push_back(makeRawMissionItem(
latDeg - 0.001, lonDeg, 50.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_WAYPOINT));
missionPlan.push_back(makeRawMissionItem(
latDeg, lonDeg, 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 << "[CONTROLLER] stateUploadMission() seq: " << (int)item.seq << '\n';
}
auto uploadResult = mMissionRaw->upload_mission(missionPlan);
if (uploadResult != MissionRaw::Result::Success) {
std::cout << "[CONTROLLER] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
return false;
}
mMissionRaw->set_current_mission_item(0);
cout << "[CONTROLLER] stateUploadMission() Setting raw mission ends" << endl;
return true;
return mMissionController->uploadMission();
}
@@ -311,13 +179,7 @@ bool AzDroneController::stateFlyMission(void)
//float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
//return mMissionController->startMissions(flight_altitude_abs);
// start mission, this set autopilot auto mode.
// ignore result, we dont care for now
MissionRaw::Result startResult = mMissionRaw->start_mission();
cout << "Start mission result:" << startResult << endl;
cout << "AzDroneController::stateFlyMission() ends\n";
return startResult == MissionRaw::Result::Success;
return mMissionController->startMission();
}
@@ -408,7 +270,7 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
mCurrentPosition = position;
// Send new position to the mission controller.
if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
if (mDroneState == AZ_DRONE_STATE_FLY_MISSION && mMissionController) {
mMissionController->newPosition(position);
}
}