MKInterface.cpp

Go to the documentation of this file.

/*
 * MKInterface.cpp
 *
 *  Created on: Nov 23, 2011
 *      Author: mriedel
 */

#include <tk_mkinterface/MKInterface.hpp>

#include <tk_mkinterface/MKCalibrator.hpp>

#define MIN_THRUST_CMD 28 // 8 + 20 (delta_trust in SaturateMotors())
#define MAX_THRUST_CMD 210 // 230 - 20 (delta_trust in SaturateMotors())
#define PITCH_ROLL_CMD_DEG_SCALE 4.0

// Battery Limits stay Hard-Coded
#define BATTERY_LEVEL_EMPTY   131
#define BATTERY_LEVEL_LOW     138
#define BATTERY_LEVEL_CHARGED 180



// TODO. Implement this nicely!!!
#define INIT_ATTITUDE_PROP_GAIN 3.0
#define INIT_ATTITUDE_DERIV_GAIN 4.0
#define INIT_ATTITUDE_INTEG_GAIN 0.0

#define INIT_YAW_RATE_GAIN 30.0
#define INIT_YAW_ACC_GAIN 20.0

#define INIT_ATTITUDE_GAIN_MULT 1.0

namespace TELEKYB_NAMESPACE {

MKInterface::MKInterface()
        : safeModule(NULL), connection(NULL), rosInterface(NULL)
{
        // Build conditions
        std::vector<MKSingleValuePacket> conditions;

        MKSingleValuePacket robotIDPacket(MKDataDefines::ROBOT_ID, options.tUavId->getValue());
        MKSingleValuePacket firmRevPacket(MKDataDefines::FIRMWARE_REVISION, options.tUavFirmware->getValue());

        // RobotID
        conditions.push_back(robotIDPacket);
        conditions.push_back(firmRevPacket);

        // try to establish connection
        connection = MKInterfaceConnection::findConnection(options.tSerialDeviceDirectory->getValue(),
                        options.tSerialDeviceNameRegex->getValue(), conditions);



        // Only build Interface AFTER Creating connection and only if it was found!
        if (connection) {
                // safe Module
                safeModule = new MKSafeMod(this, connection);

                rosInterface = new MKROSInterface(*this, options.tUavId->getValue());


                if (options.tInitialDriftEstim->getValue()) {
                        // perform DriftEstimation
                        performDriftEstim();
                }

                // TODO: Default Mirror is Off!
                MKSingleValuePacket mirrorPacket(MKDataDefines::MIRROR_DATA_ACTIVE, MKDataDefines::MKINT_LOGICAL_OFF);
                connection->setValue(mirrorPacket);

                // Set Values!
                if (!connection->setValue(MKSingleValuePacket(MKDataDefines::YAW_CTRL_TYPE, 1))) {
                        ROS_ERROR("Could not set YAW_CTRL_TYPE!");
                }

                if (!connection->setValue(MKSingleValuePacket(
                                MKDataDefines::PROP_GAIN, (MKInt)(INIT_ATTITUDE_PROP_GAIN * INIT_ATTITUDE_GAIN_MULT)))) {
                        ROS_ERROR("Could not set PROP_GAIN!");
                }
                if (!connection->setValue(MKSingleValuePacket(
                                MKDataDefines::DERIV_GAIN, (MKInt) (INIT_ATTITUDE_DERIV_GAIN * INIT_ATTITUDE_GAIN_MULT)))) {
                        ROS_ERROR("Could not set DERIV_GAIN!");
                }
                if (!connection->setValue(MKSingleValuePacket(
                                MKDataDefines::INTEG_GAIN, (MKInt) (INIT_ATTITUDE_INTEG_GAIN * INIT_ATTITUDE_GAIN_MULT)))) {
                        ROS_ERROR("Could not set INTEG_GAIN!");
                }
                if (!connection->setValue(MKSingleValuePacket(
                                MKDataDefines::YAW_RATE_GAIN, (MKInt) (INIT_YAW_RATE_GAIN * INIT_ATTITUDE_GAIN_MULT)))) {
                        ROS_ERROR("Could not set YAW_RATE_GAIN!");
                }
                if (!connection->setValue(MKSingleValuePacket(
                                MKDataDefines::YAW_ACC_GAIN, (MKInt) (INIT_YAW_ACC_GAIN * INIT_ATTITUDE_GAIN_MULT)))) {
                        ROS_ERROR("Could not set YAW_ACC_GAIN!");
                }

                // Set Offsets if configured
                // X
                if (!options.tUavOffsetRawAccX->isOnInitialValue()) {
                        if (!connection->setValue(MKSingleValuePacket(
                                        MKDataDefines::OFFSET_RAW_ACC_X, (MKInt) (options.tUavOffsetRawAccX->getValue())))) {
                                ROS_ERROR("Could not set OFFSET_RAW_ACC_X!");
                        }
//                      else {
//                              ROS_WARN("Successfully set OFFSET_RAW_ACC_X!");
//                      }
                }

                // Y
                if (!options.tUavOffsetRawAccY->isOnInitialValue()) {
                        if (!connection->setValue(MKSingleValuePacket(
                                        MKDataDefines::OFFSET_RAW_ACC_Y, (MKInt) (options.tUavOffsetRawAccY->getValue())))) {
                                ROS_ERROR("Could not set OFFSET_RAW_ACC_Y!");
                        }
//                      else {
//                              ROS_WARN("Successfully set OFFSET_RAW_ACC_Y!");
//                      }
                }

                rosInterface->activateCommandsCB();
        }
}

MKInterface::~MKInterface()
{
        if (safeModule) { delete safeModule; }
        if (rosInterface) { delete rosInterface; }
}

bool MKInterface::hasConnection() const
{
        return (connection != NULL);
}

void MKInterface::handleCommand(double estMass, double pitch, double roll, double yawrate, double thrust)
{
        //ROS_INFO("Test");
        safeModule->resetCmdTimer();
        // reset Timer
        // Pointer always valid (safemod before rosinterface!)
        if (!safeModule->isRunning()) {
                ROS_WARN("Safemodule not running. Starting...");
                safeModule->start();
        }



        // Message contains metric unit data.
        double zeroStickValue = options.tZeroStickValue->getValue();
        double mass = options.tUavMass->getValue();
        double gravity = options.tGravity->getValue();
        //double gravity = arma::norm(mEnvironment->getGravity(),2);

//      ROS_INFO("Gravity: %f", gravity);
//      ROS_INFO("Mass: %f Estimate: %f", mass, msg->mass);
//
//      ROS_INFO("Roll: %f, Pitch %f, Yaw %f, Thrust %f", msg->roll, msg->pitch, msg->yaw, msg->thrust);
//      ROS_INFO("ZeroStickValue: %i", zeroStickValue);



        double cmdThrust =  std::min(255.0,std::max(-1.0 * thrust * zeroStickValue/mass/gravity,0.0)); // numbers under LL_CMD_NUM are reserved to protocol

        //ROS_INFO("thrust(1): %f", cmdThrust);

        double hoveringCmd = estMass*zeroStickValue/mass;
        double semiDim =  std::min(fabs(MAX_THRUST_CMD - hoveringCmd),fabs(hoveringCmd - MIN_THRUST_CMD));
        cmdThrust = std::min(std::max(hoveringCmd - semiDim,cmdThrust),hoveringCmd + semiDim);
        //ROS_INFO("thrust(2): %f", cmdThrust);

        double cmdPitch = std::min(127.0,std::max(-127.0,pitch*180/M_PI*PITCH_ROLL_CMD_DEG_SCALE));
        double cmdRoll = std::min(127.0,std::max(-127.0,roll*180/M_PI*PITCH_ROLL_CMD_DEG_SCALE));
        double cmdYawRate = std::min(127.0, std::max(-127.0,yawrate*180/M_PI));


        MKCommandsPacket commands;
        commands.pitch = (MKChar)cmdPitch;
        commands.roll = (MKChar)cmdRoll;
        commands.yawrate = (MKChar)cmdYawRate;
        commands.thrust = (MKChar)cmdThrust;

        //ROS_INFO("Raw Commands: Roll: %f, Pitch: %f, Yaw: %f, Thrust: %f", cmdRoll, cmdPitch, cmdYawRate, cmdThrust);

        connection->sendCommand(commands);
}

MKInterfaceConnection* MKInterface::getConnection() const
{
        return connection;
}

bool MKInterface::performDriftEstim()
{
        if (!connection) {
                ROS_ERROR("Cannot Perform Drift Estimation without a valid connection.");
                return false;
        }

        MKCalibrator calibrator(connection);
        return calibrator.doGyroDriftEstim();
}

void MKInterface::safeModDidBecomeActive()
{
        rosInterface->deActiavteCommandsCB();
}

void MKInterface::safeModFinished()
{
        // turn off motors
        connection->setValue(MKSingleValuePacket(MKDataDefines::MOTOR_STATE, MotorState::Off));

        rosInterface->activateCommandsCB();
}

bool MKInterface::checkBattery(MKInt& batteryValue, bool& landRequest)
{
        bool updateOK = false;
        landRequest = false;
        MKValue* battery = connection->getMKDataRef().getValueByID(MKDataDefines::BATT_VOLT);
        if (connection->updateValue(MKDataDefines::BATT_VOLT)) {
                updateOK = true;
                batteryValue = battery->getValue();

                if (batteryValue < BATTERY_LEVEL_LOW) {
                        if (batteryValue < BATTERY_LEVEL_EMPTY) {
                                // Land
                                ROS_ERROR("MKInterface %d: Battery Empty! LAND NOW! Trying to initiate land!", options.tUavId->getValue());
                                landRequest = true;
                        } else {
                                // Just warn
                                ROS_WARN("MKInterface %d: Battery Level (%d) very low. Land soon!", options.tUavId->getValue(), batteryValue);
                        }
                }
        }

        return updateOK;
}

void MKInterface::setEmergency()
{
        safeModule->setEmergency();
}

const MKInterfaceOptions& MKInterface::getOptions() const
{
        return options;
}

}