/**
* 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 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);
}
}
/**
* 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);
}
void AP_Frsky_Telem::calc_gps_position()
{
_course_in_degrees = (_ahrs.yaw_sensor / 100) % 360;
const AP_GPS &gps = _ahrs.get_gps();
float lat;
float lon ;
float alt ;
float speed;
_pos_gps_ok = (gps.status() >= 3);
if (_pos_gps_ok) {
Location loc = gps.location();//get gps instance 0
lat = frsky_format_gps(fabsf(loc.lat/10000000.0));
_latdddmm = lat;
_latmmmm = (lat - _latdddmm) * 10000;
_lat_ns = (loc.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(loc.lng/10000000.0));
_londddmm = lon;
_lonmmmm = (lon - _londddmm) * 10000;
_lon_ew = (loc.lng < 0) ? 'W' : 'E';
alt = loc.alt * 0.01f;
_alt_gps_meters = (int16_t)alt;
_alt_gps_cm = (alt - abs(_alt_gps_meters)) * 100;
speed = gps.ground_speed();
_speed_in_meter = speed;
_speed_in_centimeter = (speed - _speed_in_meter) * 100;
} else {
_latdddmm = 0;
_latmmmm = 0;
_lat_ns = 0;
_londddmm = 0;
_lonmmmm = 0;
_alt_gps_meters = 0;
_alt_gps_cm = 0;
_speed_in_meter = 0;
_speed_in_centimeter = 0;
}
}
