rcCalibrationCheck.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "../PreFlightCheck.hpp"

#include <parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/input_rc.h>

/**
 * Maximum deadzone value
 */
#define RC_INPUT_MAX_DEADZONE_US    500

/**
 * Minimum value
 */
#define RC_INPUT_LOWEST_MIN_US  500

/**
 * Maximum value
 */
#define RC_INPUT_HIGHEST_MAX_US 2500

int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
{
    char nbuf[20];
    param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
        _parameter_handles_rev, _parameter_handles_dz;

    unsigned map_fail_count = 0;

    const char *rc_map_mandatory[] = {  /*"RC_MAP_MODE_SW",*/
        /* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
        nullptr /* end marker */
    };

    unsigned j = 0;

    /* if VTOL, check transition switch mapping */
    if (isVTOL) {
        param_t trans_parm = param_find("RC_MAP_TRANS_SW");

        if (trans_parm == PARAM_INVALID) {
            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC_MAP_TRANS_SW PARAMETER MISSING."); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
            map_fail_count++;

        } else {
            int32_t transition_switch;
            param_get(trans_parm, &transition_switch);

            if (transition_switch < 1) {
                if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Transition switch RC_MAP_TRANS_SW not set."); }

                map_fail_count++;
            }
        }
    }

    /* first check channel mappings */
    while (rc_map_mandatory[j] != nullptr) {

        param_t map_parm = param_find(rc_map_mandatory[j]);

        if (map_parm == PARAM_INVALID) {
            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
            map_fail_count++;
            j++;
            continue;
        }

        int32_t mapping;
        param_get(map_parm, &mapping);

        if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) {
            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
            map_fail_count++;
        }

        if (mapping == 0) {
            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
            map_fail_count++;
        }

        j++;
    }

    unsigned total_fail_count = 0;
    unsigned channels_failed = 0;

    for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
        /* should the channel be enabled? */
        uint8_t count = 0;

        /* initialize values to values failing the check */
        float param_min = 0.0f;
        float param_max = 0.0f;
        float param_trim = 0.0f;
        float param_rev = 0.0f;
        float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;

        /* min values */
        sprintf(nbuf, "RC%d_MIN", i + 1);
        _parameter_handles_min = param_find(nbuf);
        param_get(_parameter_handles_min, &param_min);

        /* trim values */
        sprintf(nbuf, "RC%d_TRIM", i + 1);
        _parameter_handles_trim = param_find(nbuf);
        param_get(_parameter_handles_trim, &param_trim);

        /* max values */
        sprintf(nbuf, "RC%d_MAX", i + 1);
        _parameter_handles_max = param_find(nbuf);
        param_get(_parameter_handles_max, &param_max);

        /* channel reverse */
        sprintf(nbuf, "RC%d_REV", i + 1);
        _parameter_handles_rev = param_find(nbuf);
        param_get(_parameter_handles_rev, &param_rev);

        /* channel deadzone */
        sprintf(nbuf, "RC%d_DZ", i + 1);
        _parameter_handles_dz = param_find(nbuf);
        param_get(_parameter_handles_dz, &param_dz);

        /* assert min..center..max ordering */
        if (param_min < RC_INPUT_LOWEST_MIN_US) {
            count++;

            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
        }

        if (param_max > RC_INPUT_HIGHEST_MAX_US) {
            count++;

            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
        }

        if (param_trim < param_min) {
            count++;

            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
        }

        if (param_trim > param_max) {
            count++;

            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
        }

        /* assert deadzone is sane */
        if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
            if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); }

            /* give system time to flush error message in case there are more */
            px4_usleep(100000);
            count++;
        }

        total_fail_count += count;

        if (count) {
            channels_failed++;
        }
    }

    if (channels_failed) {
        px4_sleep(2);

        if (report_fail) {
            mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
                         total_fail_count,
                         (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
        }

        px4_usleep(100000);
    }

    return total_fail_count + map_fail_count;
}