ForceController.h

/*
 * This file is part of LSST M1M3 support system package.
 *
 * Developed for the LSST Telescope & Site Software Systems.
 * This product includes software developed by the LSST Project
 * (https://www.lsst.org).
 * See the COPYRIGHT file at the top-level directory of this distribution
 * for details of code ownership.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef FORCECONTROLLER_H_
#define FORCECONTROLLER_H_
 
#include <spdlog/spdlog.h>
#include <vector>
 
#include <SAL_MTM1M3C.h>
#include <SAL_MTMountC.h>
 
#include <AccelerationForceComponent.h>
#include <ActiveOpticForceComponent.h>
#include <AzimuthForceComponent.h>
#include <BalanceForceComponent.h>
#include <DistributedForces.h>
#include <ElevationForceComponent.h>
#include <FinalForceComponent.h>
#include <ForceActuatorApplicationSettings.h>
#include <ForceActuatorSettings.h>
#include <ForcesAndMoments.h>
#include <LimitTrigger.h>
#include <OffsetForceComponent.h>
#include <PID.h>
#include <SafetyController.h>
#include <StaticForceComponent.h>
#include <ThermalForceComponent.h>
#include <VelocityForceComponent.h>
#include <cRIO/DataTypes.h>
 
namespace LSST {
namespace M1M3 {
namespace SS {
 
struct ForceActuatorIndicesNeighbors {
    ForceActuatorIndicesNeighbors();
    int nearCount;
    int NearZIndices[FA_MAX_NEAR_COUNT];
    int FarIndices[FA_FAR_COUNT];
};
 
class ForceController {
public:
    ForceController(ForceActuatorApplicationSettings *forceActuatorApplicationSettings);
 
    void reset();
 
    void updateTMAElevationData(MTMount_elevationC *tmaElevationData);
 
    bool faRaiseFollowingErrorInTolerance();
 
    void updateAppliedForces();
 
    void processAppliedForces();
 
    bool applyAccelerationForces();
    void zeroAccelerationForces();
 
    void applyActiveOpticForces(float *z);
    void zeroActiveOpticForces();
 
    void applyAzimuthForces();
    void updateTMAAzimuthForces(MTMount_azimuthC *tmaAzimuthData);
    void zeroAzimuthForces();
 
    bool applyBalanceForces();
    bool zeroBalanceForces();
    void updatePID(int id, PIDParameters parameters);
 
    void resetPID(int id);
 
    void resetPIDs();
 
    void freezePIDs();
    void thawPIDs();
 
    void applyElevationForces();
    void zeroElevationForces();
 
    void applyOffsetForces(float *x, float *y, float *z);
    void applyOffsetForcesByMirrorForces(float xForce, float yForce, float zForce, float xMoment,
                                         float yMoment, float zMoment);
    void zeroOffsetForces();
 
    void applyActuatorOffset(char axis, int index, float offset);
 
    void applyStaticForces();
    void zeroStaticForces();
 
    void applyThermalForces();
    void updateThermalForces(float temperature);
    void zeroThermalForces();
 
    bool applyVelocityForces();
    void zeroVelocityForces();
 
    void enableDisableForceComponent(int forceComponentEnum, bool enabled);
 
private:
    void _sumAllForces();
    void _convertForcesToSetpoints();
 
    bool _checkMirrorMoments();
    bool _checkNearNeighbors();
    bool _checkMirrorWeight();
    bool _checkFarNeighbors();
 
    static double constexpr _sqrt2 = 1.4142135623730950488016887242097;
 
    ForceActuatorApplicationSettings *_forceActuatorApplicationSettings;
    SafetyController *_safetyController;
 
    AccelerationForceComponent _accelerationForceComponent;
    ActiveOpticForceComponent _activeOpticForceComponent;
    AzimuthForceComponent _azimuthForceComponent;
    BalanceForceComponent _balanceForceComponent;
    ElevationForceComponent _elevationForceComponent;
    OffsetForceComponent _offsetForceComponent;
    StaticForceComponent _staticForceComponent;
    ThermalForceComponent _thermalForceComponent;
    VelocityForceComponent _velocityForceComponent;
    FinalForceComponent _finalForceComponent;
 
    MTM1M3_appliedCylinderForcesC *_appliedCylinderForces;
    MTM1M3_appliedForcesC *_appliedForces;
    MTM1M3_logevent_forceActuatorStateC *_forceActuatorState;
    MTM1M3_logevent_forceSetpointWarningC *_forceSetpointWarning;
    MTM1M3_logevent_preclippedCylinderForcesC *_preclippedCylinderForces;
 
    MTM1M3_inclinometerDataC *_inclinometerData;
    MTM1M3_pidDataC *_pidData;
    MTM1M3_logevent_pidInfoC *_pidInfo;
    MTM1M3_hardpointActuatorDataC *_hardpointActuatorData;
    MTM1M3_accelerometerDataC *_accelerometerData;
    MTM1M3_gyroDataC *_gyroData;
 
    ForceActuatorIndicesNeighbors _neighbors[FA_COUNT];
 
    float _zero[FA_COUNT];
    float _mirrorWeight;
 
    class ForceLimitTrigger : public LimitTrigger<float, float> {
    public:
        ForceLimitTrigger() {
            _axis = 'U';
            _faId = -1;
            _counter = 0;
        }
 
        ForceLimitTrigger(char axis, int faId) {
            _axis = axis;
            _faId = faId;
            _counter = 0;
        }
 
    protected:
        bool trigger() override {
            _counter++;
            // problems are reported  when accumulated counter reaches those levels
            static int levels[4] = {1, 200, 500, 100000};
            for (int i = 0; i < 4; i++)
                if (levels[i] == _counter) return true;
            return ((_counter % levels[3]) == 0);
        }
 
        void execute(float limit, float fe) override {
            SPDLOG_WARN(
                    "Violated {} follow-up error FA ID {} measured error {} "
                    "({}th occurence), limit +-{}",
                    _axis, _faId, fe, _counter, limit);
        }
 
        void reset() override {
            if (_counter > 0) {
                SPDLOG_INFO(
                        "FA ID {} axis {} following error is back into limits "
                        "after {} failures",
                        _faId, _axis, _counter);
                _counter = 0;
            }
        }
 
    private:
        char _axis;
        int _faId;
        int _counter;
    };
 
    ForceLimitTrigger limitTriggerX[FA_X_COUNT];
    ForceLimitTrigger limitTriggerY[FA_Y_COUNT];
    ForceLimitTrigger limitTriggerZ[FA_Z_COUNT];
 
    static int32_t _toInt24(float force) { return (int32_t)(force * 1000.0); }
};
 
} /* namespace SS */
} /* namespace M1M3 */
} /* namespace LSST */
 
#endif /* FORCECONTROLLER_H_ */