ActuatorTesting.cc

Go to the documentation of this file.

#include "ActuatorTesting.h"
#include "MAVLinkLib.h"
#include "Common.h"
#include "AppMessages.h"
#include "Vehicle.h"
#include "QGCLoggingCategory.h"
 
QGC_LOGGING_CATEGORY(ActuatorTestingLog, "Vehicle.Actuators.ActuatorTesting")
 
using namespace ActuatorTesting;
 
ActuatorTest::ActuatorTest(Vehicle* vehicle)
    : _vehicle(vehicle)
{
    _watchdogTimer.setInterval(100);
    _watchdogTimer.setSingleShot(false);
    connect(&_watchdogTimer, &QTimer::timeout, this, &ActuatorTest::watchdogTimeout);
    _watchdogTimer.start();
}
 
ActuatorTest::~ActuatorTest()
{
    _actuators->clearAndDeleteContents();
    delete _allMotorsActuator;
}
 
void ActuatorTest::updateFunctions(const QList<Actuator*> &actuators)
{
    _actuators->clearAndDeleteContents();
 
    if (_allMotorsActuator) {
      _allMotorsActuator->deleteLater();
    }
 
    _allMotorsActuator = nullptr;
 
    Actuator* motorActuator{nullptr};
    for (const auto& actuator : actuators) {
        if (actuator->isMotor()) {
            motorActuator = actuator;
        }
        _actuators->append(actuator);
    }
    if (motorActuator) {
        _allMotorsActuator = new Actuator(this, tr("All Motors"), motorActuator->min(), motorActuator->max(), motorActuator->defaultValue(),
                motorActuator->function(), true);
    }
    resetStates();
 
    emit actuatorsChanged();
}
 
void ActuatorTest::resetStates()
{
    _states.clear();
    _currentState = -1;
    for (int i = 0; i < _actuators->count(); ++i) {
        _states.append(ActuatorState{});
    }
}
 
void ActuatorTest::watchdogTimeout()
{
    for (int i = 0; i < _states.size(); ++i) {
        if (_states[i].state == ActuatorState::State::Active) {
            if (_states[i].lastUpdated.elapsed() > 100) {
                qCWarning(ActuatorTestingLog) << "Stopping actuator due to timeout:" << i;
                _states[i].state = ActuatorState::State::StopRequest;
            }
        }
    }
    sendNext();
}
 
void ActuatorTest::setChannelTo(int index, float value)
{
    if (!_active || index >= _states.size()) {
        return;
    }
    qCDebug(ActuatorTestingLog) << "setting actuator: index:" << index << "value:" << value;
 
    _states[index].value = value;
    _states[index].state = ActuatorState::State::Active;
    _states[index].lastUpdated.start();
    sendNext();
}
 
void ActuatorTest::stopControl(int index)
{
    if (index >= _states.size() || index < -1) {
        return;
    }
    qCDebug(ActuatorTestingLog) << "stop actuator control: index:" << index;
 
    if (index == -1) {
        for (int i = 0; i < _states.size(); ++i) {
            if (_states[i].state == ActuatorState::State::Active) {
                _states[i].state = ActuatorState::State::StopRequest;
            }
        }
    } else {
        if (_states[index].state == ActuatorState::State::Active) {
            _states[index].state = ActuatorState::State::StopRequest;
        }
    }
    sendNext();
}
 
void ActuatorTest::setActive(bool active)
{
    qCDebug(ActuatorTestingLog) << "setting active: " << active;
    if (!active) {
        stopControl(-1);
    }
    _active = active;
}
 
void ActuatorTest::ackHandlerEntry(void* resultHandlerData, int /*compId*/, const mavlink_command_ack_t& ack, VehicleTypes::MavCmdResultFailureCode_t failureCode)
{
    ActuatorTest* actuatorTest = (ActuatorTest*)resultHandlerData;
    actuatorTest->ackHandler(static_cast<MAV_RESULT>(ack.result), failureCode);
}
 
void ActuatorTest::ackHandler(MAV_RESULT commandResult, VehicleTypes::MavCmdResultFailureCode_t failureCode)
{
    // upon receiving an (n)ack, continuously cycle through the active actuators, one at a time
    _commandInProgress = false;
    if (failureCode == VehicleTypes::MavCmdResultFailureNoResponseToCommand) {
        // on timeout, just try the next one
        sendNext();
    } else if (commandResult == MAV_RESULT_ACCEPTED) {
        if (_currentState != -1 && _states[_currentState].state == ActuatorState::State::Stopping) {
            _states[_currentState].state = ActuatorState::State::NotActive;
        }
        sendNext();
    } else { // failure
        if (_currentState != -1) {
            _states[_currentState].state = ActuatorState::State::NotActive;
        }
 
        if (!_hadFailure) {
            QString message;
            if (commandResult == MAV_RESULT_TEMPORARILY_REJECTED) {
                message = tr("Actuator test command temporarily rejected");
            } else if (commandResult == MAV_RESULT_DENIED) {
                message = tr("Actuator test command denied");
            } else if (commandResult == MAV_RESULT_UNSUPPORTED) {
                message = tr("Actuator test command not supported");
            } else {
                message = tr("Actuator test command failed");
            }
            QGC::showAppMessage(message);
            _hadFailure = true;
            emit hadFailureChanged();
        }
        sendNext();
    }
}
 
void ActuatorTest::sendNext()
{
    if (_commandInProgress) {
        return;
    }
 
    // find the next actuator not in state NotActive
    for (int i = 0; i < _states.size(); ++i) {
        _currentState = (_currentState + 1) % _states.size();
        Actuator* actuator = _actuators->value<Actuator*>(_currentState);
        if (_states[_currentState].state == ActuatorState::State::Active) {
            sendMavlinkRequest(actuator->function(), _states[_currentState].value, 1.f);
            break;
        } else if (_states[_currentState].state == ActuatorState::State::StopRequest) {
            _states[_currentState].state = ActuatorState::State::Stopping;
            sendMavlinkRequest(actuator->function(), NAN, 0.f);
            break;
        }
    }
}
 
void ActuatorTest::sendMavlinkRequest(int function, float value, float timeout)
{
    qCDebug(ActuatorTestingLog) << "Sending actuator test function:" << function << "value:" << value;
 
    // TODO: consider using a lower command timeout
 
    Vehicle::MavCmdAckHandlerInfo_t handlerInfo = {};
    handlerInfo.resultHandler       = ackHandlerEntry;
    handlerInfo.resultHandlerData   = this;
 
    _vehicle->sendMavCommandWithHandler(
            &handlerInfo,
            MAV_COMP_ID_AUTOPILOT1,           // the ID of the autopilot
            MAV_CMD_ACTUATOR_TEST,            // the mavlink command
            value,                            // value
            timeout,                          // timeout
            0,                                // unused parameter
            0,                                // unused parameter
            1000+function,                    // function
            0,                                // unused parameter
            0);
    _commandInProgress = true;
}