aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_FrSky_Telem: removed dependency on inertialnav

Browse Source 
the AP_InertialNav library is deprecated in favor of AP_AHRS. We
should not introduce a new dependency on it
This commit is contained in:
Andrew Tridgell 2016-08-10 08:14:34 +10:00
parent b71e0d73b9
commit ab77fdfa02
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -26,11 +26,10 @@ extern const AP_HAL::HAL& hal;
AP_Frsky_Telem::msg_t _msg;
//constructor
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav) :
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng) :
_ahrs(ahrs),
_battery(battery),
_rng(rng),
_inav(inav),
_port(NULL),
_protocol(),
_initialised_uart(),
@ -179,7 +178,10 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
}
return;
} else if ((now - _passthrough.timer_vario) > 500) {
send_uint32(VARIO_FIRST_ID, (int32_t)roundf(_inav.get_velocity_z())); // vertical velocity in cm/s
Vector3f velNED;
if (_ahrs.get_velocity_NED(velNED)) {
send_uint32(VARIO_FIRST_ID, (int32_t)roundf(-velNED.z*100)); // vertical velocity in cm/s, +ve up
}
_passthrough.timer_vario = AP_HAL::millis();
return;
} else if ((now - _passthrough.timer_alt) > 1000) {
@ -706,11 +708,15 @@ uint32_t AP_Frsky_Telem::calc_home(void)
uint32_t AP_Frsky_Telem::calc_velandyaw(void)
{
uint32_t velandyaw;
Vector3f velNED {};
// if we can't get velocity then we use zero for vertical velocity
_ahrs.get_velocity_NED(velNED);
// vertical velocity in dm/s
velandyaw = prep_number(roundf(_inav.get_velocity_z() * 0.1f), 2, 1);
velandyaw = prep_number(roundf(velNED.z * 10), 2, 1);
// horizontal velocity in dm/s
velandyaw |= prep_number(roundf(_inav.get_velocity_xy() * 0.1f), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
// yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
return velandyaw;

4
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -18,7 +18,6 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_InertialNav/AP_InertialNav.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_SerialManager/AP_SerialManager.h>
@ -115,7 +114,7 @@ class AP_Frsky_Telem
{
public:
//constructor
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav);
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng);
// init - perform required initialisation
void init(const AP_SerialManager &serial_manager, const char *firmware_str, const char *frame_config_str, const uint8_t mav_type, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, uint8_t *control_mode, uint32_t *ap_value, uint32_t *control_sensors_present, uint32_t *control_sensors_enabled, uint32_t *control_sensors_health, int32_t *home_distance, int32_t *home_bearing);
@ -134,7 +133,6 @@ private:
AP_AHRS &_ahrs;
const AP_BattMonitor &_battery;
const RangeFinder &_rng;
const AP_InertialNav &_inav;
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
bool _initialised_uart;