diff --git a/drone_controller/az_drone_controller.cpp b/drone_controller/az_drone_controller.cpp index 3bf4a8a..5074a84 100644 --- a/drone_controller/az_drone_controller.cpp +++ b/drone_controller/az_drone_controller.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include "az_config.h" @@ -12,9 +14,10 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent) : QObject(parent) - , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} - , // TODO!! Autopilot or CompanionComputer? - mDroneState(AZ_DRONE_STATE_DISCONNECTED) + , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer? + , mDroneState(AZ_DRONE_STATE_DISCONNECTED) + , mMissionRaw(NULL) + , mMissionItemSeqNum(0) { mFirstPosition.relative_altitude_m = -10000; @@ -32,6 +35,42 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent) // Healt info update from MAVSDK. connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection); + + isCopterType = QCoreApplication::arguments().at(2) == "quadcopter"; +} + + +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(frame); + new_item.command = static_cast(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; } @@ -142,6 +181,98 @@ 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); + } + + 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; +} + + bool AzDroneController::stateArm(void) { Action::Result result = mAction->arm(); @@ -158,11 +289,6 @@ bool AzDroneController::stateArm(void) bool AzDroneController::stateTakeoff(void) { - // Drone never reaches the target altitude with ArduPilot. This seems - // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4. - // Added extra check to AzDroneController::stateFlyMission() to not start the - // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached. - mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE); Action::Result result = mAction->takeoff(); cout << "[CONTROLLER] MAVSDK::Action::takeoff() failed. Reason: " << result << endl; @@ -172,14 +298,26 @@ bool AzDroneController::stateTakeoff(void) bool AzDroneController::stateFlyMission(void) { - if (mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) { + // Drone never reaches the target altitude with ArduPilot. This seems + // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4. + // Added extra check to AzDroneController::stateFlyMission() to not start the + // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached. + if (isCopterType && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) { cout << "[CONTROLLER] 90% of the takeoff altitide is not reached yet." << endl; return false; } // TODO!! Check with the team about fly altitude. Is altitudes in JSON file absolute or relative? - float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m; - return mMissionController->startMissions(flight_altitude_abs); + //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; } @@ -246,8 +384,19 @@ void AzDroneController::droneStateMachineSlot(void) void AzDroneController::newPositionSlot(Telemetry::Position position) { - cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg - << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl; + /* + static Telemetry::Position previousPosition; + + // Only print position if it has changed. + if (position == previousPosition) { + return; + } + + + previousPosition = position; + */ + //cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg + // << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl; // Save first position. It will be used later to set altitude for missions. // TODO!! Probably we want to use rangefinder or at least barometer with altitude from the map later. @@ -267,7 +416,8 @@ void AzDroneController::newPositionSlot(Telemetry::Position position) void AzDroneController::newHeadingSlot(double heading) { - cout << "[CONTROLLER] Heading: " << heading << endl; + (void)heading; + //cout << "[CONTROLLER] Heading: " << heading << endl; } diff --git a/drone_controller/az_drone_controller.h b/drone_controller/az_drone_controller.h index 6fa1cf1..eaac073 100644 --- a/drone_controller/az_drone_controller.h +++ b/drone_controller/az_drone_controller.h @@ -5,6 +5,7 @@ #include #include +#include #include #include "az_mission.h" @@ -19,6 +20,7 @@ typedef enum { AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, AZ_DRONE_STATE_GOT_ACTION_MODULE, AZ_DRONE_STATE_HEALTH_OK, + AZ_DRONE_STATE_MISSION_UPLOADED, AZ_DRONE_STATE_ARMED, AZ_DRONE_STATE_TAKE_OFF, AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, @@ -44,8 +46,18 @@ protected: bool stateGetActionModule(void); bool stateHealthOk(void); bool stateArm(void); + bool stateUploadMission(void); bool stateTakeoff(void); bool stateFlyMission(void); + void setRawMission(void); + MissionRaw::MissionItem makeRawMissionItem(float latitude_deg1e7, + float longitude_deg1e7, + int32_t altitude_m, + float do_photo, + MAV_FRAME frame, + MAV_CMD command, + float p2 = 0, + float p3 = 0); // Slots that are called by the emitted Qt signals. protected slots: @@ -93,7 +105,10 @@ protected: shared_ptr mSystem; Telemetry *mTelemetry; Action *mAction; + MissionRaw *mMissionRaw; Telemetry::Position mFirstPosition; Telemetry::Position mCurrentPosition; AzMissionController *mMissionController; + bool isCopterType; + int mMissionItemSeqNum; }; diff --git a/drone_controller/az_drone_controller_plane.cpp b/drone_controller/az_drone_controller_plane.cpp index c011fec..08723d9 100644 --- a/drone_controller/az_drone_controller_plane.cpp +++ b/drone_controller/az_drone_controller_plane.cpp @@ -34,7 +34,8 @@ void AzDroneControllerPlane::droneStateMachineSlot(void) {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneControllerPlane::stateGetTelemetryModule, "Getting telemetry module", 1000}, {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneControllerPlane::stateGetActionModule, "Getting action module", 1000}, {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneControllerPlane::stateHealthOk, "Drone health OK", 1000}, - {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateWaitAutoSwitch, "User switched to AUTO mode", 1000}, + {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateUploadMission, "Uploading the mission", 1000}, + {AZ_DRONE_STATE_MISSION_UPLOADED, &AzDroneControllerPlane::stateWaitAutoSwitch, "Waiting user to switch to ArduPilot AUTO mode", 1000}, {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::stateFlyMission, "Starting the mission", 1000}, {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 0}}; diff --git a/drone_controller/main.cpp b/drone_controller/main.cpp index 694875b..58256d0 100644 --- a/drone_controller/main.cpp +++ b/drone_controller/main.cpp @@ -25,7 +25,7 @@ int main(int argc, char *argv[]) qCritical() << "\tFirst argument: mission JSON file."; qCritical() << "\tSecond argument: \"quadcopter\" or \"plane\"."; qCritical() << "\tThird argument: \"udp\" or \"serial\" for connection type."; - qCritical() << "\tFor example ./autopilot mission.json plane udp"; + qCritical() << "\tFor example ./drone_controller mission.json plane udp"; return 1; }