From 6abf4ec2b5926c337e88b6d6be5fd5cc36b109f1 Mon Sep 17 00:00:00 2001 From: yaapu Date: Thu, 1 Oct 2020 11:24:10 +1000 Subject: [PATCH] AP_Frsky_Telem: tidy creation of frsky data --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp | 13 +++------- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 22 +++++++--------- libraries/AP_Frsky_Telem/AP_Frsky_SPort.h | 2 +- .../AP_Frsky_SPort_Passthrough.cpp | 26 +++++++------------ 4 files changed, 24 insertions(+), 39 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp index 68a6ed49fc..080e88850a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -105,28 +105,23 @@ float AP_Frsky_Backend::format_gps(float dec) */ void AP_Frsky_Backend::calc_gps_position(void) { - float lat; - float lon; - float alt; - float speed; - if (AP::gps().status() >= 3) { const Location &loc = AP::gps().location(); //get gps instance 0 - lat = format_gps(fabsf(loc.lat/10000000.0f)); + float lat = format_gps(fabsf(loc.lat/10000000.0f)); _SPort_data.latdddmm = lat; _SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000; _SPort_data.lat_ns = (loc.lat < 0) ? 'S' : 'N'; - lon = format_gps(fabsf(loc.lng/10000000.0f)); + float lon = format_gps(fabsf(loc.lng/10000000.0f)); _SPort_data.londddmm = lon; _SPort_data.lonmmmm = (lon - _SPort_data.londddmm) * 10000; _SPort_data.lon_ew = (loc.lng < 0) ? 'W' : 'E'; - alt = loc.alt * 0.01f; + float alt = loc.alt * 0.01f; _SPort_data.alt_gps_meters = (int16_t)alt; _SPort_data.alt_gps_cm = (alt - _SPort_data.alt_gps_meters) * 100; - speed = AP::gps().ground_speed(); + float speed = AP::gps().ground_speed(); _SPort_data.speed_in_meter = speed; _SPort_data.speed_in_centimeter = (speed - _SPort_data.speed_in_meter) * 100; } else { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 4d44c867f9..6b9ce346c1 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -87,10 +87,10 @@ void AP_Frsky_SPort::send(void) case SENSOR_ID_GPS: // Sensor ID 3 switch (_SPort.gps_call) { case 0: - send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude + send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude break; case 1: - send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude + send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude break; case 2: send_sport_frame(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part @@ -136,28 +136,26 @@ void AP_Frsky_SPort::send(void) * prepare gps latitude/longitude data * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ -uint32_t AP_Frsky_SPort::calc_gps_latlng(bool *send_latitude) +uint32_t AP_Frsky_SPort::calc_gps_latlng(bool &send_latitude) { - uint32_t latlng; const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) // alternate between latitude and longitude - if ((*send_latitude) == true) { + if (send_latitude == true) { + send_latitude = false; if (loc.lat < 0) { - latlng = ((labs(loc.lat)/100)*6) | 0x40000000; + return ((labs(loc.lat)/100)*6) | 0x40000000; } else { - latlng = ((labs(loc.lat)/100)*6); + return ((labs(loc.lat)/100)*6); } - (*send_latitude) = false; } else { + send_latitude = true; if (loc.lng < 0) { - latlng = ((labs(loc.lng)/100)*6) | 0xC0000000; + return ((labs(loc.lng)/100)*6) | 0xC0000000; } else { - latlng = ((labs(loc.lng)/100)*6) | 0x80000000; + return ((labs(loc.lng)/100)*6) | 0x80000000; } - (*send_latitude) = true; } - return latlng; } /* diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 7e0836c883..1d22411de6 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -21,7 +21,7 @@ protected: uint8_t new_byte; } _passthrough; - uint32_t calc_gps_latlng(bool *send_latitude); + uint32_t calc_gps_latlng(bool &send_latitude); private: diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 9197bb423d..2aa0a892ad 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -154,8 +154,8 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) break; case GPS_LAT: // 0x800 GPS lat // sample both lat and lon at the same time - send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude - _passthrough.gps_lng_sample = calc_gps_latlng(&_passthrough.send_latitude); + send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude + _passthrough.gps_lng_sample = calc_gps_latlng(_passthrough.send_latitude); // force the scheduler to select GPS lon as packet that's been waiting the most // this guarantees that gps coords are sent at max // _scheduler.avg_polling_period*number_of_downlink_sensors time separation @@ -325,10 +325,8 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void) { const AP_GPS &gps = AP::gps(); - uint32_t gps_status; - // number of GPS satellites visible (limit to 15 (0xF) since the value is stored on 4 bits) - gps_status = (gps.num_sats() < GPS_SATS_LIMIT) ? gps.num_sats() : GPS_SATS_LIMIT; + uint32_t gps_status = (gps.num_sats() < GPS_SATS_LIMIT) ? gps.num_sats() : GPS_SATS_LIMIT; // GPS receiver status (limit to 0-3 (0x3) since the value is stored on 2 bits: NO_GPS = 0, NO_FIX = 1, GPS_OK_FIX_2D = 2, GPS_OK_FIX_3D or GPS_OK_FIX_3D_DGPS or GPS_OK_FIX_3D_RTK_FLOAT or GPS_OK_FIX_3D_RTK_FIXED = 3) gps_status |= ((gps.status() < GPS_STATUS_LIMIT) ? gps.status() : GPS_STATUS_LIMIT)< 82° uint8_t imu_temp = (uint8_t) roundf(constrain_float(AP::ins().get_temperature(0), AP_IMU_TEMP_MIN, AP_IMU_TEMP_MAX) - AP_IMU_TEMP_MIN); // control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits) - ap_status = (uint8_t)((gcs().custom_mode()+1) & AP_CONTROL_MODE_LIMIT); + uint32_t ap_status = (uint8_t)((gcs().custom_mode()+1) & AP_CONTROL_MODE_LIMIT); // simple/super simple modes flags ap_status |= (uint8_t)(gcs().simple_input_active())<printf("flying=%d\n",AP_Notify::flags.flying); - //hal.console->printf("ap_status=%08X\n",ap_status); return ap_status; } @@ -407,17 +400,17 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void) uint32_t home = 0; Location loc; Location home_loc; - bool get_position; + bool got_position = false; float _relative_home_altitude = 0; { AP_AHRS &_ahrs = AP::ahrs(); WITH_SEMAPHORE(_ahrs.get_semaphore()); - get_position = _ahrs.get_position(loc); + got_position = _ahrs.get_position(loc); home_loc = _ahrs.get_home(); } - if (get_position) { + if (got_position) { // check home_loc is valid if (home_loc.lat != 0 || home_loc.lng != 0) { // distance between vehicle and home_loc in meters @@ -468,10 +461,9 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) { const RangeFinder *_rng = RangeFinder::get_singleton(); - uint32_t attiandrng; AP_AHRS &_ahrs = AP::ahrs(); // roll from [-18000;18000] centidegrees to unsigned .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits) - attiandrng = ((uint16_t)roundf((_ahrs.roll_sensor + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT); + uint32_t attiandrng = ((uint16_t)roundf((_ahrs.roll_sensor + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT); // pitch from [-9000;9000] centidegrees to unsigned .2 degree increments [0;900] (just in case, limit to 1023 (0x3FF) since the value is stored on 10 bits) attiandrng |= ((uint16_t)roundf((_ahrs.pitch_sensor + 9000) * 0.05f) & ATTIANDRNG_PITCH_LIMIT)<