ARDrone::ARDrone(std::string sessionId, std::string userId, std::string appId,
const std::string ardIp,
const unsigned short ardATCmdsPort,
const unsigned short ardNavdataPort,
const unsigned short ardVideoPort,
const unsigned short ardControlPort,
ARDroneConnections* connectionHandler,
Navdata* navdata,
Video* video)
: m_sessionId(sessionId)
, m_userId(userId)
, m_appId(appId)
, m_ardIp(ardIp)
, m_ardATCmdsPort(ardATCmdsPort)
, m_ardNavdataPort(ardNavdataPort)
, m_ardVideoPort(ardVideoPort)
, m_ardControlPort(ardControlPort)
, m_altitudeMax(2.0f)
, m_verticalSpeed(.7f)
, m_rotationSpeed(3.0f)
, m_euler_angle_max(0.26f)
, m_isWithoutShell(false)
, m_isOutdoor(false)
, m_connectionsHandler(connectionHandler)
, m_indexCmd(1)
, m_navdata(navdata)
, m_video(video)
{
m_video->setCallbackInitFunc([this]() {this->m_connectionsHandler->sendInitVideoData();});
AT_CONFIG("custom:session_id", sessionId);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
AT_CONFIG("custom:profile_id", userId);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
AT_CONFIG("custom:application_id", appId);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
setAltitudeMax(m_altitudeMax);
setVerticalSpeed(m_verticalSpeed);
setRotationSpeed(m_rotationSpeed);
setSpeed(m_euler_angle_max);
setIsWithoutShell(false);
setIsOutdoor(false);
// Initialize the ARDrone video stream (Codecs, etc..)
setDefaultConfig();
initNavdata();
}
/**
* Adds a field to the message. Unknown fields are rejected
* @return bool if field was known to the message
*/
bool addField(unsigned char fieldDefNum, unsigned char size, unsigned char baseType, unsigned char arch, char * data) {
//TODO: Compare size with expected size
//TODO: Compare baseType with expected baseType
bool fieldWasAdded = true;
switch (fieldDefNum) {
case 253: setTimestamp(read0x86(data,arch));
break;
case 0: setPositionLat(read0x85(data,arch));
break;
case 1: setPositionLong(read0x85(data,arch));
break;
case 2: setAltitude(read0x88(data,arch,5,500,0x84));
break;
case 3: setHeartRate(read0x02(data,arch));
break;
case 4: setCadence(read0x02(data,arch));
break;
case 5: setDistance(read0x88(data,arch,100,0,0x86));
break;
case 6: setSpeed(read0x88(data,arch,1000,0,0x84));
break;
case 7: setPower(read0x84(data,arch));
break;
case 9: setGrade(read0x88(data,arch,100,0,0x83));
break;
case 10: setResistance(read0x02(data,arch));
break;
case 11: setTimeFromCourse(read0x88(data,arch,1000,0,0x85));
break;
case 12: setCycleLength(read0x88(data,arch,100,0,0x02));
break;
case 13: setTemperature(read0x01(data,arch));
break;
case 17: setNumSpeed1s(read0x02(data,arch));
break;
case 18: setCycles(read0x02(data,arch));
break;
case 19: setTotalCycles(read0x86(data,arch));
break;
case 28: setCompressedAccumulatedPower(read0x84(data,arch));
break;
case 29: setAccumulatedPower(read0x86(data,arch));
break;
case 30: setLeftRightBalance(read0x02(data,arch));
break;
case 31: setGpsAccuracy(read0x02(data,arch));
break;
case 32: setVerticalSpeed(read0x88(data,arch,1000,0,0x83));
break;
case 33: setCalories(read0x84(data,arch));
break;
case 43: setLeftTorqueEffectiveness(read0x88(data,arch,2,0,0x02));
break;
case 44: setRightTorqueEffectiveness(read0x88(data,arch,2,0,0x02));
break;
case 45: setLeftPedalSmoothness(read0x88(data,arch,2,0,0x02));
break;
case 46: setRightPedalSmoothness(read0x88(data,arch,2,0,0x02));
break;
case 47: setCombinedPedalSmoothness(read0x88(data,arch,2,0,0x02));
break;
case 52: setCadence256(read0x88(data,arch,256,0,0x84));
break;
default:
fieldWasAdded = false;
break;
}
return fieldWasAdded;
};