zr-v4/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

/*

   Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>

   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 <http://www.gnu.org/licenses/>.
*/

/* 
   FRSKY Telemetry library
   for the moment it only handle hub port telemetry
   the sport reference are only here to simulate the frsky module and use opentx simulator. it will eventually be removed
*/

#include <AP_Frsky_Telem.h>

extern const AP_HAL::HAL& hal;

void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, uint8_t frsky_type)
{
    if (port == NULL) {
	return;
    }
    _port = port;    
    _port->begin(9600);
    _initialised = true;    
}

void AP_Frsky_Telem::frsky_send_byte(uint8_t value)
{
    const uint8_t x5E[] = { 0x5D, 0x3E };
    const uint8_t x5D[] = { 0x5D, 0x3D };
    switch (value) {
    case 0x5E:
	_port->write( x5E, sizeof(x5E));
	break;
	
    case 0x5D:
	_port->write( x5D, sizeof(x5D));
	break;
	
    default:
	_port->write(&value, sizeof(value));
	break;
    }
}


/**
 * Sends a 0x5E start/stop byte.
 */
void AP_Frsky_Telem::frsky_send_hub_startstop()
{
    static const uint8_t c = 0x5E;
    _port->write(&c, sizeof(c));
}

/**
 * Sends one data id/value pair.
 */
void  AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data)
{
    /* Cast data to unsigned, because signed shift might behave incorrectly */
    uint16_t udata = data;

    frsky_send_hub_startstop();
    frsky_send_byte(id);

    frsky_send_byte(udata); /* LSB */
    frsky_send_byte(udata >> 8); /* MSB */

}

/**
 * Sends frame 1 (every 200ms):
 *  barometer altitude,  battery voltage & current
 */
void AP_Frsky_Telem::frsky_send_frame1(uint8_t mode)
{
    struct Location loc;
    float battery_amps = _battery.current_amps();
    float baro_alt = 0; // in meters
    bool posok = _ahrs.get_position(loc);
    if  (posok) {
	baro_alt = loc.alt * 0.01f; // convert to meters
        if (!loc.flags.relative_alt) {
            baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set
        }
    }
    const AP_GPS &gps = _ahrs.get_gps();

    // GPS status is sent as num_sats*10 + status, to fit into a
    // uint8_t
    uint8_t T2 = gps.num_sats() * 10 + gps.status();
  
    frsky_send_data(FRSKY_ID_TEMP1, mode);
    frsky_send_data(FRSKY_ID_TEMP2, T2);

    /*
      Note that this isn't actually barometric altitdue, it is the
      AHRS estimate of altitdue above home.
     */
    uint16_t baro_alt_meters = (uint16_t)baro_alt;
    uint16_t baro_alt_cm = (baro_alt - baro_alt_meters) * 100;
    frsky_send_data(FRSKY_ID_BARO_ALT_BP, baro_alt_meters);
    frsky_send_data(FRSKY_ID_BARO_ALT_AP, baro_alt_cm);
  
    frsky_send_data(FRSKY_ID_FUEL, roundf(_battery.capacity_remaining_pct()));
 
    frsky_send_data(FRSKY_ID_VFAS, roundf(_battery.voltage() * 10.0f));
    frsky_send_data(FRSKY_ID_CURRENT, (battery_amps < 0) ? 0 : roundf(battery_amps * 10.0f));
  
}

/**
 * Formats the decimal latitude/longitude to the required degrees/minutes.
 */
float  AP_Frsky_Telem::frsky_format_gps(float dec)
{
    uint8_t dm_deg = (uint8_t) dec;
    return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
}

/**
 * Sends frame 2 (every 1000ms):
 * course(heading), latitude, longitude, ground speed, GPS altitude
 */
void  AP_Frsky_Telem::frsky_send_frame2()
{

    // we send the heading based on the ahrs instead of GPS course which is not very usefull
    uint16_t course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
    frsky_send_data(FRSKY_ID_GPS_COURS_BP, course_in_degrees);

    const AP_GPS &gps = _ahrs.get_gps();
    bool posok = (gps.status() >= 3);
    if  (posok){
	// send formatted frame 
	float lat = 0, lon = 0,  alt = 0, speed= 0;
	char lat_ns = 0, lon_ew = 0;
	Location loc = gps.location();//get gps instance 0
	
	lat = frsky_format_gps(fabsf(loc.lat/10000000.0));
	uint16_t latdddmm = lat;
	uint16_t latmmmm = (lat - latdddmm) * 10000;
	lat_ns = (loc.lat < 0) ? 'S' : 'N';
	
	lon = frsky_format_gps(fabsf(loc.lng/10000000.0));
	uint16_t londddmm = lon;
	uint16_t lonmmmm = (lon - londddmm) * 10000;
	lon_ew = (loc.lng < 0) ? 'W' : 'E';

	alt = loc.alt / 100;
	uint16_t alt_gps_meters = alt;
	uint16_t alt_gps_cm = (alt - alt_gps_meters) * 100;

	speed = gps.ground_speed ();
	uint16_t speed_in_meter = speed;
	uint16_t speed_in_centimeter = (speed - speed_in_meter) * 100;
  
  
	frsky_send_data(FRSKY_ID_GPS_LAT_BP, latdddmm);
	frsky_send_data(FRSKY_ID_GPS_LAT_AP, latmmmm);
	frsky_send_data(FRSKY_ID_GPS_LAT_NS, lat_ns);

	frsky_send_data(FRSKY_ID_GPS_LONG_BP, londddmm);
	frsky_send_data(FRSKY_ID_GPS_LONG_AP, lonmmmm);
	frsky_send_data(FRSKY_ID_GPS_LONG_EW, lon_ew);
  
	frsky_send_data(FRSKY_ID_GPS_SPEED_BP, speed_in_meter);
	frsky_send_data(FRSKY_ID_GPS_SPEED_AP, speed_in_centimeter);
  
	frsky_send_data(FRSKY_ID_GPS_ALT_BP, alt_gps_meters);
	frsky_send_data(FRSKY_ID_GPS_ALT_AP, alt_gps_cm);
  
    }
}


/*
  check for input bytes from sport
 */
void AP_Frsky_Telem::check_sport_input(void)
{
    
}


/*
  send telemetry frames. Should be called at 50Hz. The high rate is to
  allow this code to poll for serial bytes coming from the receiver
  for the SPort protocol
 */
void AP_Frsky_Telem::send_frames(uint8_t control_mode, enum FrSkyProtocol protocol)
{
    if (!_initialised) {
        return;
    }
    
    if (protocol == FrSkySPORT) {
        // check for sport bytes
        check_sport_input();
    }
    
    uint32_t now = hal.scheduler->millis();

    // send frame1 every 200ms
    if (now - _last_frame1_ms > 200) {
        _last_frame1_ms = now;
        frsky_send_frame1(control_mode);        
    }

    // send frame2 every second
    if (now - _last_frame2_ms > 1000) {
        _last_frame2_ms = now;
        frsky_send_frame2();
    }
}