diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp index 476990e010..68a6ed49fc 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -49,7 +49,8 @@ void AP_Frsky_Backend::loop(void) float AP_Frsky_Backend::get_vspeed_ms(void) { - {// release semaphore as soon as possible + { + // release semaphore as soon as possible AP_AHRS &_ahrs = AP::ahrs(); Vector3f v; WITH_SEMAPHORE(_ahrs.get_semaphore()); @@ -70,10 +71,10 @@ float AP_Frsky_Backend::get_vspeed_ms(void) void AP_Frsky_Backend::calc_nav_alt(void) { _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s - + Location loc; float current_height = 0; // in centimeters above home - + AP_AHRS &_ahrs = AP::ahrs(); WITH_SEMAPHORE(_ahrs.get_semaphore()); if (_ahrs.get_position(loc)) { @@ -86,7 +87,7 @@ void AP_Frsky_Backend::calc_nav_alt(void) _SPort_data.alt_nav_meters = (int16_t)current_height; _SPort_data.alt_nav_cm = (current_height - _SPort_data.alt_nav_meters) * 100; -} +} /* * format the decimal latitude/longitude to the required degrees/minutes @@ -141,6 +142,6 @@ void AP_Frsky_Backend::calc_gps_position(void) } AP_AHRS &_ahrs = AP::ahrs(); - _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS + _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index e14c2ed190..7468c1b457 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -3,7 +3,8 @@ #include #include -class AP_Frsky_Backend { +class AP_Frsky_Backend +{ public: AP_Frsky_Backend(AP_HAL::UARTDriver *port) : @@ -15,10 +16,14 @@ public: virtual void send() = 0; // SPort is at 57600, D overrides this - virtual uint32_t initial_baud() const { return 57600; } + virtual uint32_t initial_baud() const + { + return 57600; + } // get next telemetry data for external consumers of SPort data - virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) { + virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) + { return false; } @@ -38,8 +43,7 @@ protected: // methods to convert flight controller data to FrSky D or SPort format float format_gps(float dec); - struct - { + struct { int32_t vario_vspd; char lat_ns, lon_ew; uint16_t latdddmm; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_D.h b/libraries/AP_Frsky_Telem/AP_Frsky_D.h index a6a1ca5925..e289c7aef9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_D.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_D.h @@ -2,7 +2,8 @@ #include "AP_Frsky_Backend.h" -class AP_Frsky_D : public AP_Frsky_Backend { +class AP_Frsky_D : public AP_Frsky_Backend +{ public: @@ -11,7 +12,10 @@ public: protected: void send() override; - uint32_t initial_baud() const override { return 9600; } + uint32_t initial_baud() const override + { + return 9600; + } private: diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index c3c4f2b34a..7e0836c883 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -2,7 +2,8 @@ #include "AP_Frsky_Backend.h" -class AP_Frsky_SPort : public AP_Frsky_Backend { +class AP_Frsky_SPort : public AP_Frsky_Backend +{ public: @@ -14,8 +15,7 @@ protected: void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); - struct PACKED - { + struct PACKED { bool send_latitude; // sizeof(bool) = 4 ? uint32_t gps_lng_sample; uint8_t new_byte; @@ -25,8 +25,7 @@ protected: private: - struct - { + struct { bool sport_status; bool gps_refresh; bool vario_refresh; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index ad064c9f6f..9197bb423d 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -9,7 +9,7 @@ #include #include -/* +/* for FrSky SPort Passthrough */ // data bits preparation @@ -119,18 +119,18 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) { bool packet_ready = false; switch (idx) { - case TEXT: - packet_ready = !queue_empty; - break; - case AP_STATUS: - packet_ready = gcs().vehicle_initialised(); - break; - case BATT_2: - packet_ready = AP::battery().num_instances() > 1; - break; - default: - packet_ready = true; - break; + case TEXT: + packet_ready = !queue_empty; + break; + case AP_STATUS: + packet_ready = gcs().vehicle_initialised(); + break; + case BATT_2: + packet_ready = AP::battery().num_instances() > 1; + break; + default: + packet_ready = true; + break; } return packet_ready; @@ -144,47 +144,47 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) { // send packet switch (idx) { - case TEXT: // 0x5000 status text - if (get_next_msg_chunk()) { - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); - } - break; - case ATTITUDE: // 0x5006 Attitude and range - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); - 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); - // 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 - _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000; - break; - case GPS_LON: // 0x800 GPS lon - send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude - break; - case VEL_YAW: // 0x5005 Vel and Yaw - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw()); - break; - case AP_STATUS: // 0x5001 AP status - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); - break; - case GPS_STATUS: // 0x5002 GPS Status - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); - break; - case HOME: // 0x5004 Home - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); - break; - case BATT_2: // 0x5008 Battery 2 status - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1)); - break; - case BATT_1: // 0x5003 Battery 1 status - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0)); - break; - case PARAM: // 0x5007 parameters - send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); - break; + case TEXT: // 0x5000 status text + if (get_next_msg_chunk()) { + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); + } + break; + case ATTITUDE: // 0x5006 Attitude and range + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); + 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); + // 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 + _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000; + break; + case GPS_LON: // 0x800 GPS lon + send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude + break; + case VEL_YAW: // 0x5005 Vel and Yaw + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw()); + break; + case AP_STATUS: // 0x5001 AP status + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); + break; + case GPS_STATUS: // 0x5002 GPS Status + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); + break; + case HOME: // 0x5004 Home + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); + break; + case BATT_2: // 0x5008 Battery 2 status + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1)); + break; + case BATT_1: // 0x5003 Battery 1 status + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0)); + break; + case PARAM: // 0x5007 parameters + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); + break; } } @@ -269,7 +269,7 @@ bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void) // send messages twice extra_chunks = 1; } - + if (_msg_chunk.repeats++ > extra_chunks ) { _msg_chunk.repeats = 0; if (_msg_chunk.char_index == 0) { @@ -297,7 +297,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_param(void) } _paramID++; - switch(_paramID) { + switch (_paramID) { case FRAME_TYPE: param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h break; @@ -332,12 +332,12 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void) // 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)< GPS_STATUS_LIMIT) ? gps.status()-GPS_STATUS_LIMIT : 0)<printf("flying=%d\n",AP_Notify::flags.flying); - //hal.console->printf("ap_status=%08X\n",ap_status); + //hal.console->printf("flying=%d\n",AP_Notify::flags.flying); + //hal.console->printf("ap_status=%08X\n",ap_status); return ap_status; } @@ -417,7 +417,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void) home_loc = _ahrs.get_home(); } - if (get_position) { + if (get_position) { // check home_loc is valid if (home_loc.lat != 0 || home_loc.lng != 0) { // distance between vehicle and home_loc in meters @@ -450,7 +450,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void) WITH_SEMAPHORE(_ahrs.get_semaphore()); // horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed) const AP_Airspeed *aspeed = _ahrs.get_airspeed(); - if (aspeed && aspeed->enabled()) { + if (aspeed && aspeed->enabled()) { velandyaw |= prep_number(roundf(aspeed->get_airspeed() * 10), 2, 1)<