/** * 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; } }