void VideoController::digitalVideo(bool showState, uint32_t ipAddress)
{
if (showState)
{
pid_t pid = fork();
if (pid == -1)
{
COMM_EXCEPTION(LaunchException, "Failed to fork server process");
}
if (pid == 0)
{
ip::address_v4 addr(ipAddress);
std::string str = addr.to_string();
std::cout << "Send video to " << str << std::endl;
int execResult = execlp(
"/etc/cherokey-robot/run_video_wifi", "run_video_wifi",
str.c_str(), NULL);
exit(execResult);
}
else
{
videoProcessPID = pid;
}
}
else
{
stopChild();
}
}
IMUReader::IMUReader()
{
if ((file = open(I2CDEV, O_RDWR)) < 0)
{
COMM_EXCEPTION(InternalError, "Can't open I2C device");
}
currentAngles = { 0, 0, 0 };
initThread();
}
VideoController::VideoController()
: videoProcessPID(-1)
{
auto instance = ConfigManager::getInstance();
if (!instance)
{
COMM_EXCEPTION(NullPointerException, "Configuration manager "
"instance is null.");
}
auto pinConfig = instance->getPinsInfo();
videoTXPower = std::unique_ptr<pc::GPIOPin>(
new pc::GPIOPin(pinConfig.videoTxPowerPin,
pc::GPIO_DIRECTION::output));
}
VoltageReader::VoltageReader()
{
auto instance = ConfigManager::getInstance();
if (!instance)
{
COMM_EXCEPTION(NullPointerException, "Configuration manager "
"instance is null.");
}
auto adcInfo = instance->getAdcInfo();
voltageChannel = adcInfo.voltageChannel;
currentChannel = adcInfo.currentChannel;
sensorInfo = instance->getVoltageSensorInfo();
initThread();
}
void VideoController::compositeVideo(bool showState)
{
if (showState)
{
pid_t pid = fork();
if (pid == -1)
{
COMM_EXCEPTION(LaunchException, "Failed to fork server process");
}
if (pid == 0)
{
int execResult = execlp(
"/etc/cherokey-robot/run_video_composite", NULL);
exit(execResult);
}
else
{
videoProcessPID = pid;
}
try
{
videoTXPower->setLogicalLevel(pc::GPIO_LOGIC_LEVEL::high);
}
catch (std::exception&)
{
stopChild();
throw;
}
}
else
{
stopChild();
videoTXPower->setLogicalLevel(pc::GPIO_LOGIC_LEVEL::low);
}
}
void IMUReader::run()
{
initSensors();
cs::SensorData sensorMessage;
sensorMessage.set_sensor_id((int) SensorIds::IMU_sensor);
sensorMessage.set_sensor_desc(IMU_SENSOR_NAME);
auto values = sensorMessage.mutable_sensor_values();
auto gyroData = values->Add();
gyroData->set_associated_name("gyro_data");
gyroData->set_data_type(cs::COORD_3D);
auto gyroCoords = gyroData->mutable_coord_value();
int64_t loopCount = 0;
GyroState gyroState = { 0 };
bool calibration = true;
int64_t calibrationDelay = 500;
AHRSProcessor ahrsProcessor(100);
auto instance = ConfigManager::getInstance();
if (!instance)
{
COMM_EXCEPTION(NullPointerException, "Can't get instance of "
"configuration manager");
}
float pitch, roll, yaw;
QUATERNION offsetQ;
showCalibration(true);
auto delayTime = std::chrono::milliseconds(10);
auto t = std::chrono::high_resolution_clock::now();
#if WRITE_SENSORS_DATA
std::ofstream stream("raw_data.txt");
#endif
while (!stopVariable)
{
IMUSensorsData sensorsData = { 0 };
readSensors(sensorsData, gyroState, calibration);
std::this_thread::sleep_until(t + delayTime);
t += delayTime;
#if WRITE_SENSORS_DATA
stream << sensorsData.rawAccelX << " " << sensorsData.rawAccelY <<
" " << sensorsData.rawAccelZ << " " << sensorsData.rawGyroX <<
" " << sensorsData.rawGyroY << " " << sensorsData.rawGyroZ <<
std::endl;
#endif
if (!calibration)
{
ahrsProcessor.updateState(sensorsData, roll, pitch, yaw);
putCurrentAngles(roll, pitch, yaw);
}
else
{
IMUSensorsData tmpData = { 0 };
tmpData.rawAccelX = sensorsData.rawAccelX;
tmpData.rawAccelY = sensorsData.rawAccelY;
tmpData.rawAccelZ = sensorsData.rawAccelZ;
ahrsProcessor.updateState(tmpData, offsetQ);
}
gyroCoords->set_x(roll);
gyroCoords->set_y(pitch);
gyroCoords->set_z(yaw);
int messageSize = sensorMessage.ByteSize();
std::vector<int8_t> outArray(messageSize);
sensorMessage.SerializeToArray(&outArray[0], messageSize);
if (!calibration && loopCount % 10 == 0)
{
//std::cout << sensorsData.rawCompassX << " " << sensorsData.rawCompassY << " " <<
// sensorsData.rawCompassZ << std::endl;
sendData(outArray);
}
loopCount++;
if (loopCount == calibrationDelay)
{
calibration = false;
gyroState.offsetX /= calibrationDelay;
gyroState.offsetY /= calibrationDelay;
gyroState.offsetZ /= calibrationDelay;
ahrsProcessor.setGyroOffsets(gyroState.offsetX, gyroState.offsetY,
gyroState.offsetZ);
ahrsProcessor.setOffsetQuaternion(offsetQ);
showCalibration(false);
}
}
#if WRITE_SENSORS_DATA
stream.close();
#endif
}
