Controller.cpp

// Fill out your copyright notice in the Description page of Project Settings.

#include "LeController.h"
//#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
//#include "vehicles\multirotor\controllers\DroneCommon.hpp"
//#include "AirLib\include\vehicles\multirotor\controllers\DroneCommon.hpp"

using namespace msr::airlib;

ALeController::ALeController(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
	//UE_LOG(LogTemp, Warning, TEXT("ULeController init"));
}

bool ALeController::armDisarm(bool arm)
{
	return client.armDisarm(arm);
}

void ALeController::enableApiControl(bool enable)
{
	client.enableApiControl(true);
}

bool ALeController::hover()
{
	return client.hover();
}

bool ALeController::land(float max_wait_seconds)
{
	return client.land(max_wait_seconds);
}

bool ALeController::goHome()
{
	return client.goHome();
}

void ALeController::setSimulationMode(bool is_set)
{
	client.setSimulationMode(is_set);
}

bool ALeController::takeoff(float max_wait_ms)
{
	return client.takeoff(max_wait_ms);
}

bool ALeController::moveByAngle(float pitch, float roll, float z, float yaw, float duration)
{
	return client.moveByAngle(pitch,roll,z,yaw,duration);
}

bool ALeController::moveByVelocity(float vx, float vy, float vz, float duration)
{
	return client.moveByVelocity(vx, vy, vz, duration);	
}

bool ALeController::moveByVelocityEx(float vx, float vy, float vz, float duration,
	EDrivetrainType drivetrain, UPARAM(ref) FYawMode& yaw_mode)
{
	//convert our BP structs to their cpp exuivalents from DroneCommon
	using namespace msr::airlib;
	//DrivetrainType drv = DrivetrainType::ForwardOnly;
	DrivetrainType drv = DrivetrainType(drivetrain);
	YawMode yaw = YawMode(yaw_mode.is_rate,yaw_mode.yaw_or_rate);

	bool result = client.moveByVelocity(vx,vy,vz,duration, drv, yaw);
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;
	return result;
}

bool ALeController::moveByVelocityZ(float vx, float vy, float z, float duration)
{
	return client.moveByVelocityZ(vx,vy,z,duration);
}

bool ALeController::moveByVelocityZEx(float vx, float vy, float z, float duration,
	EDrivetrainType drivetrain, UPARAM(ref) FYawMode& yaw_mode)
{
	//convert our BP structs to their cpp exuivalents from DroneCommon
	using namespace msr::airlib;
	//DrivetrainType drv = DrivetrainType::ForwardOnly;
	DrivetrainType drv = DrivetrainType(drivetrain);
	YawMode yaw = YawMode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);

	bool result = client.moveByVelocityZ(vx, vy, z, duration, drv, yaw);
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;
	return result;
}

bool ALeController::moveOnPath(UPARAM(ref)TArray<FVector>& path, float velocity, float max_wait_seconds, EDrivetrainType drivetrain, UPARAM(ref)FYawMode & yaw_mode, float lookahead, float adaptive_lookahead)
{
	DrivetrainType drv = DrivetrainType(drivetrain);
	YawMode yaw = YawMode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);
	vector<Vector3r> ppath;
	for (int i = 0; i < path.Num(); i++)
	{		
		Vector3r v = Vector3r(path[i].X, path[i].Y, path[i].Z);
		ppath.push_back(v);
	}
		
	bool result = client.moveOnPath(ppath, velocity, max_wait_seconds,drv, yaw,lookahead, adaptive_lookahead);

	//vector3r takes values by ref so maybe there's no need to move vals back to TArray?
	/*for (int i = 0; i < path.Num(); i++)
	{
		path.
	}
	*/
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;

	return result;
}

bool ALeController::moveToPosition(float x, float y, float z, float velocity)
{
	return client.moveToPosition(x, y, z, velocity);
}

bool ALeController::moveToPositionEx(float x, float y, float z, float velocity, float max_wait_seconds, EDrivetrainType drivetrain, UPARAM(ref)FYawMode & yaw_mode, float lookahead, float adaptive_lookahead)
{
	DrivetrainType drv = DrivetrainType(drivetrain);
	YawMode yaw = YawMode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);

	bool result = client.moveToPosition(x, y, z, velocity, max_wait_seconds,drv,yaw,lookahead,adaptive_lookahead);
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;
	return result;
}

bool ALeController::moveToZ(float z, float velocity)
{
	return client.moveToZ(z,velocity);
}

bool ALeController::moveToZEx(float z, float velocity, float max_wait_seconds, UPARAM(ref)FYawMode & yaw_mode, float lookahead, float adaptive_lookahead)
{	
	YawMode yaw = YawMode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);

	bool result = client.moveToZ(z, velocity, max_wait_seconds, yaw, lookahead, adaptive_lookahead);
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;
	return result;
}

bool ALeController::moveByManual(float vx_max, float vy_max, float z_min, float duration)
{
	return client.moveByManual(vx_max, vy_max, z_min, duration);
}

bool ALeController::moveByManualEx(float vx_max, float vy_max, float z_min, float duration, EDrivetrainType drivetrain, UPARAM(ref)FYawMode & yaw_mode)
{
	DrivetrainType drv = DrivetrainType(drivetrain);
	YawMode yaw = YawMode(yaw_mode.is_rate, yaw_mode.yaw_or_rate);

	bool result = client.moveByManual(vx_max, vy_max, z_min, duration, drv, yaw);
	yaw_mode.is_rate = yaw.is_rate;
	yaw_mode.yaw_or_rate = yaw.yaw_or_rate;
	return result;
}

bool ALeController::rotateToYaw(float yaw)
{
	return client.rotateToYaw(yaw);
}

bool ALeController::rotateToYawEx(float yaw, float max_wait_seconds, float margin)
{
	return client.rotateToYaw(yaw,max_wait_seconds,margin);
}

bool ALeController::rotateByYawRate(float yaw_rate, float duration)
{
	return client.rotateByYawRate(yaw_rate,duration);
}


FVector ALeController::getPosition()
{
	Vector3r v = client.getPosition();	
	return FVector(v.x(), v.y(), v.z());
}

FVector ALeController::getVelocity()
{
	Vector3r v = client.getVelocity();
	return FVector(v.x(), v.y(), v.z());
}


FQuat ALeController::getOrientation()
{
	Quaternionr q = client.getOrientation();
	return FQuat(q.x(),q.y(),q.z(),q.w());
}


FVector ALeController::getGpsLocation()
{
	GeoPoint p = client.getGpsLocation();	
	return FVector(p.latitude,p.longitude,p.altitude);
}

bool ALeController::isSimulationMode()
{
	return client.isSimulationMode();
}

FString ALeController::getDebugInfo()
{
	string s = client.getDebugInfo();
	return FString(s.c_str());
}

bool ALeController::getLandedState()
{
	return client.getLandedState() == DroneControllerBase::LandedState::Landed;
}


int ALeController::timestampNow()
{
	UE_LOG(LogTemp, Warning, TEXT("Achtung! ULeController::timestampNow() uint64_t return type is not supported in BP"));
	return int(client.timestampNow());
}

//