/**
* 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 2 (every 1000ms):
* GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level
*/
void frsky_send_frame2(int uart)
{
/* get a local copy of the global position 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);
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
/* 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.global_valid) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
lat = frsky_format_gps(abs(global_pos.lat));
lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(abs(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(vehicle_status.battery_remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
frsky_send_startstop(uart);
}
/**
* Sends one data id/value pair.
*/
static void frsky_send_data(int uart, uint8_t id, int16_t data)
{
/* Cast data to unsigned, because signed shift might behave incorrectly */
uint16_t udata = data;
frsky_send_startstop(uart);
frsky_send_byte(uart, id);
frsky_send_byte(uart, udata); /* LSB */
frsky_send_byte(uart, udata >> 8); /* MSB */
}
/**
* 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 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);
}
/**
* 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);
}
