/**************************************************************************** * * 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 CRSFTelemetry::CRSFTelemetry(int uart_fd) : _vehicle_gps_position_sub(orb_subscribe(ORB_ID(vehicle_gps_position))), _battery_status_sub(orb_subscribe(ORB_ID(battery_status))), _vehicle_attitude_sub(orb_subscribe(ORB_ID(vehicle_attitude))), _vehicle_status_sub(orb_subscribe(ORB_ID(vehicle_status))), _uart_fd(uart_fd) { } CRSFTelemetry::~CRSFTelemetry() { orb_unsubscribe(_vehicle_gps_position_sub); orb_unsubscribe(_battery_status_sub); orb_unsubscribe(_vehicle_attitude_sub); orb_unsubscribe(_vehicle_status_sub); } 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 (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) != 0) { 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 (orb_copy(ORB_ID(vehicle_gps_position), _vehicle_gps_position_sub, &vehicle_gps_position) != 0) { 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 (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude) != 0) { 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 (orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status) != 0) { 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); }