/**
* Sends frame 3 (every 5000ms):
* GPS date & time
*/
void frsky_send_frame3(int uart)
{
/* send formatted frame */
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
frsky_send_startstop(uart);
}
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(raw.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
roundf(raw.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
raw.baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(raw.baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(raw.baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery.voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
frsky_send_startstop(uart);
}
/**
* Sends frame 1 (every 200ms):
* acceleration values, barometer altitude, temperature, battery voltage & current
*/
void frsky_send_frame1(int uart)
{
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
sensor_combined->baro_alt_meter[0]);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(sensor_combined->baro_temp_celcius[0]));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery_status->voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery_status->current_a < 0) ? 0 : roundf(battery_status->current_a * 10.0f));
frsky_send_startstop(uart);
}
/**
* Sends frame 3 (every 5000ms):
* GPS date & time
*/
void frsky_send_frame3(int uart)
{
/* get a local copy of the battery data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
/* send formatted frame */
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year);
frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min);
frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec);
frsky_send_startstop(uart);
}
/**
* 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));
}
/*
* send current consumptiom
*/
void AP_Frsky_Telem::send_current(void)
{
frsky_send_data(FRSKY_ID_CURRENT, _batt_amps);
}
/*
* send heading in degree based on AHRS and not GPS
*/
void AP_Frsky_Telem::send_heading(void)
{
frsky_send_data(FRSKY_ID_GPS_COURS_BP, _course_in_degrees);
}
/*
* send battery remaining
*/
void AP_Frsky_Telem::send_batt_remain(void)
{
frsky_send_data(FRSKY_ID_FUEL, _batt_remaining);
}
/*
* send battery voltage
*/
void AP_Frsky_Telem::send_batt_volts(void)
{
frsky_send_data(FRSKY_ID_VFAS, _batt_volts);
}
/*
* send gps altitude integer part
*/
void AP_Frsky_Telem::send_gps_alt_meter(void)
{
frsky_send_data(FRSKY_ID_GPS_ALT_BP, _alt_gps_meters);
}
/*
* send gps longitude minutes decimal part
*/
void AP_IO_BOARD::send_gps_lon_mm(void)
{
frsky_send_data(FRSKY_ID_GPS_LONG_AP, _lonmmmm);
}
/*
* send gps longitude minutes decimal part
*/
void AP_Frsky_Telem::send_gps_lon_mm(void)
{
frsky_send_data(FRSKY_ID_GPS_LONG_AP, _lonmmmm);
}
/*
* send gps altitude integer part
*/
void AP_IO_BOARD::send_gps_alt_meter(void)
{
frsky_send_data(FRSKY_ID_GPS_ALT_BP, _alt_gps_meters);
}
/**
* 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);
}
}
/*
* send gps North / South information
*/
void AP_Frsky_Telem::send_gps_lat_ns(void)
{
frsky_send_data(FRSKY_ID_GPS_LAT_NS, _lat_ns);
}
/*
* send gps East / West information
*/
void AP_IO_BOARD::send_gps_lon_ew(void)
{
frsky_send_data(FRSKY_ID_GPS_LONG_EW, _lon_ew);
}
/**
* Sends frame 2 (every 1000ms):
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
*/
void frsky_send_frame2(int uart)
{
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) {
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos->yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(fabsf(global_pos->lat));
lat_ns = (global_pos->lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(global_pos->lon));
lon_ew = (global_pos->lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e)
* 25.0f / 46.0f;
alt = global_pos->alt;
sec = tm_gps->tm_sec;
}
frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course);
frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f);
frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed);
frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(battery_status->remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
frsky_send_startstop(uart);
}
/*
* send gps altitude decimals
*/
void AP_IO_BOARD::send_gps_alt_cm(void)
{
frsky_send_data(FRSKY_ID_GPS_ALT_AP, _alt_gps_cm);
}
/*
* send gps lattitude degree and minute integer part; Initialize gps info
*/
void AP_Frsky_Telem::send_gps_lat_dd(void)
{
frsky_send_data(FRSKY_ID_GPS_LAT_BP, _latdddmm);
}
/*
* send gps speed integer part
*/
void AP_Frsky_Telem::send_gps_speed_meter(void)
{
frsky_send_data(FRSKY_ID_GPS_SPEED_BP, _speed_in_meter);
}
/*
* send gps lattitude minutes decimal part
*/
void AP_Frsky_Telem::send_gps_lat_mm(void)
{
frsky_send_data(FRSKY_ID_GPS_LAT_AP, _latmmmm);
}
/*
* send gps speed integer part
*/
void AP_IO_BOARD::send_gps_speed_meter(void)
{
frsky_send_data(FRSKY_ID_GPS_SPEED_BP, _speed_in_meter);
}
/*
* send gps longitude degree and minute integer part
*/
void AP_Frsky_Telem::send_gps_lon_dd(void)
{
frsky_send_data(FRSKY_ID_GPS_LONG_BP, _londddmm);
}
/*
* send number of gps satellite and gps status eg: 73 means 7 satellite and 3d lock
*/
void AP_Frsky_Telem::send_gps_sats()
{
frsky_send_data(FRSKY_ID_TEMP2, gps_sats);
}
/*
* send gps East / West information
*/
void AP_Frsky_Telem::send_gps_lon_ew(void)
{
frsky_send_data(FRSKY_ID_GPS_LONG_EW, _lon_ew);
}
/*
* send control_mode as Temperature 1 (TEMP1)
*/
void AP_Frsky_Telem::send_mode(void)
{
frsky_send_data(FRSKY_ID_TEMP1, _mode);
}
/*
* send gps speed decimal part
*/
void AP_Frsky_Telem::send_gps_speed_cm(void)
{
frsky_send_data(FRSKY_ID_GPS_SPEED_AP, _speed_in_centimeter);
}
/*
* send barometer altitude integer part . Initialize baro altitude
*/
void AP_Frsky_Telem::send_baro_alt_m(void)
{
frsky_send_data(FRSKY_ID_BARO_ALT_BP, _baro_alt_meters);
}
/*
* send gps altitude decimals
*/
void AP_Frsky_Telem::send_gps_alt_cm(void)
{
frsky_send_data(FRSKY_ID_GPS_ALT_AP, _alt_gps_cm);
}
/*
* send barometer altitude decimal part
*/
void AP_Frsky_Telem::send_baro_alt_cm(void)
{
frsky_send_data(FRSKY_ID_BARO_ALT_AP, _baro_alt_cm);
}
