crsf_telemetry.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2018 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 "crsf_telemetry.h"
#include <lib/rc/crsf.h>

CRSFTelemetry::CRSFTelemetry(int uart_fd) :
    _uart_fd(uart_fd)
{
}

bool CRSFTelemetry::update(const hrt_abstime &now)
{
    const int update_rate_hz = 10;

    if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
        return false;
    }

    bool sent = false;

    switch (_next_type) {
    case 0:
        sent = send_battery();
        break;

    case 1:
        sent = send_gps();
        break;

    case 2:
        sent = send_attitude();
        break;

    case 3:
        sent = send_flight_mode();
        break;
    }

    _last_update = now;
    _next_type = (_next_type + 1) % num_data_types;

    return sent;
}

bool CRSFTelemetry::send_battery()
{
    battery_status_s battery_status;

    if (!_battery_status_sub.update(&battery_status)) {
        return false;
    }

    uint16_t voltage = battery_status.voltage_filtered_v * 10;
    uint16_t current = battery_status.current_filtered_a * 10;
    int fuel = battery_status.discharged_mah;
    uint8_t remaining = battery_status.remaining * 100;
    return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
}

bool CRSFTelemetry::send_gps()
{
    vehicle_gps_position_s vehicle_gps_position;

    if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
        return false;
    }

    int32_t latitude = vehicle_gps_position.lat;
    int32_t longitude = vehicle_gps_position.lon;
    uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
    uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
    uint16_t altitude = vehicle_gps_position.alt + 1000;
    uint8_t num_satellites = vehicle_gps_position.satellites_used;

    return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
                       gps_heading, altitude, num_satellites);
}

bool CRSFTelemetry::send_attitude()
{
    vehicle_attitude_s vehicle_attitude;

    if (!_vehicle_attitude_sub.update(&vehicle_attitude)) {
        return false;
    }

    matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
    int16_t pitch = attitude(1) * 1e4f;
    int16_t roll = attitude(0) * 1e4f;
    int16_t yaw = attitude(2) * 1e4f;
    return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw);
}

bool CRSFTelemetry::send_flight_mode()
{
    vehicle_status_s vehicle_status;

    if (!_vehicle_status_sub.update(&vehicle_status)) {
        return false;
    }

    const char *flight_mode = "(unknown)";

    switch (vehicle_status.nav_state) {
    case vehicle_status_s::NAVIGATION_STATE_MANUAL:
        flight_mode = "Manual";
        break;

    case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
        flight_mode = "Altitude";
        break;

    case vehicle_status_s::NAVIGATION_STATE_POSCTL:
        flight_mode = "Position";
        break;

    case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
        flight_mode = "Return";
        break;

    case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
        flight_mode = "Mission";
        break;

    case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
    case vehicle_status_s::NAVIGATION_STATE_DESCEND:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
        flight_mode = "Auto";
        break;

    case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
    case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
        flight_mode = "Failure";
        break;

    case vehicle_status_s::NAVIGATION_STATE_ACRO:
        flight_mode = "Acro";
        break;

    case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
        flight_mode = "Terminate";
        break;

    case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
        flight_mode = "Offboard";
        break;

    case vehicle_status_s::NAVIGATION_STATE_STAB:
        flight_mode = "Stabilized";
        break;

    case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
        flight_mode = "Rattitude";
        break;
    }

    return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode);
}