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

Merge branch 'master' of https://github.com/diydrones/ardupilot

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-25 19:59:32 -08:00
commit a987b5e4fa
123 changed files with 1824 additions and 928 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/system.cpp
View File

@ -167,7 +167,7 @@ void Rover::init_ardupilot()
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
camera_mount.init(&DataFlash, serial_manager);
#endif
/*

10
APMrover2/wscript
View File

@ -1,15 +1,12 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
vehicle = bld.path.name
ardupilotwaf.vehicle_stlib(
bld,
bld.ap_stlib(
name=vehicle + '_libs',
vehicle=vehicle,
libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
libraries=bld.ap_common_vehicle_libraries() + [
'APM_Control',
'AP_Arming',
'AP_Camera',
@ -27,8 +24,7 @@ def build(bld):
use='mavlink',
)
ardupilotwaf.program(
bld,
bld.ap_program(
program_name='ardurover',
use=vehicle + '_libs',
)

10
AntennaTracker/wscript
View File

@ -1,22 +1,18 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
vehicle = bld.path.name
ardupilotwaf.vehicle_stlib(
bld,
bld.ap_stlib(
name=vehicle + '_libs',
vehicle=vehicle,
libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
libraries=bld.ap_common_vehicle_libraries() + [
'PID',
],
use='mavlink',
)
ardupilotwaf.program(
bld,
bld.ap_program(
program_name='antennatracker',
use=vehicle + '_libs',
)

5
ArduCopter/ArduCopter.cpp
View File

@ -284,6 +284,11 @@ void Copter::fast_loop()
// check if we've landed or crashed
update_land_and_crash_detectors();
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
// log sensor health
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();

3
ArduCopter/Copter.h
View File

@ -108,12 +108,9 @@
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
// AP_HAL to Arduino compatibility layer
// Configuration
#include "defines.h"
#include "config.h"
#include "config_channels.h"
// Local modules
#include "Parameters.h"

13
ArduCopter/GCS_Mavlink.cpp
View File

@ -1056,6 +1056,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE:
{
#if MOUNT == ENABLED
copter.camera_mount.handle_param_value(msg);
#endif
break;
}
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
{
handle_mission_write_partial_list(copter.mission, msg);
@ -1717,7 +1725,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_FRAME_GLOBAL_INT:
default:
loc.flags.relative_alt = false;
// Copter does not support navigation to absolute altitudes. This convert the WGS84 altitude
// to a home-relative altitude before passing it to the navigation controller
loc.alt -= copter.ahrs.get_home().alt;
loc.flags.relative_alt = true;
loc.flags.terrain_alt = false;
break;
}

22
ArduCopter/config_channels.h
View File

@ -1,22 +0,0 @@
#ifndef __ARDUCOPTER_CONFIG_MOTORS_H__
#define __ARDUCOPTER_CONFIG_MOTORS_H__
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
#include "config.h" // Parent Config File
#include "APM_Config.h" // User Overrides
#endif // __ARDUCOPTER_CONFIG_MOTORS_H__

2
ArduCopter/system.cpp
View File

@ -207,7 +207,7 @@ void Copter::init_ardupilot()
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
camera_mount.init(&DataFlash, serial_manager);
#endif
#if PRECISION_LANDING == ENABLED

10
ArduCopter/wscript
View File

@ -1,15 +1,12 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
vehicle = bld.path.name
ardupilotwaf.vehicle_stlib(
bld,
bld.ap_stlib(
name=vehicle + '_libs',
vehicle=vehicle,
libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
libraries=bld.ap_common_vehicle_libraries() + [
'AP_ADSB',
'AC_AttitudeControl',
'AC_Fence',
@ -36,8 +33,7 @@ def build(bld):
use='mavlink',
)
ardupilotwaf.program(
bld,
bld.ap_program(
program_name='arducopter',
use=vehicle + '_libs',
)

2
ArduPlane/system.cpp
View File

@ -201,7 +201,7 @@ void Plane::init_ardupilot()
#if MOUNT == ENABLED
// initialise camera mount
camera_mount.init(serial_manager);
camera_mount.init(&DataFlash, serial_manager);
#endif
#if FENCE_TRIGGERED_PIN > 0

10
ArduPlane/wscript
View File

@ -1,15 +1,12 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
vehicle = bld.path.name
ardupilotwaf.vehicle_stlib(
bld,
bld.ap_stlib(
name=vehicle + '_libs',
vehicle=vehicle,
libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
libraries=bld.ap_common_vehicle_libraries() + [
'APM_Control',
'APM_OBC',
'AP_ADSB',
@ -37,8 +34,7 @@ def build(bld):
use='mavlink',
)
ardupilotwaf.program(
bld,
bld.ap_program(
program_name='arduplane',
use=vehicle + '_libs',
)

5
Tools/CPUInfo/wscript
View File

@ -1,11 +1,8 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.program(
bld,
bld.ap_program(
use='ap',
blddestdir='tools',
)

37
Tools/Frame_params/Parrot_Bebop2.param
View File

@ -8,27 +8,20 @@ BATT_CAPACITY,2700
COMPASS_EXTERNAL,1
COMPASS_ORIENT,2
FRAME,3
INS_GYRO_FILTER,40
MOT_THST_EXPO,0.05
MOT_THST_MAX,1.00
MOT_YAW_HEADROOM,200
POS_XY_P,1
RATE_PIT_D,0.0022
RATE_PIT_FILT_HZ,40
RATE_PIT_I,0.126
RATE_PIT_IMAX,2000
RATE_PIT_P,0.126
RATE_RLL_D,0.0005
RATE_RLL_FILT_HZ,40
RATE_RLL_I,0.11
RATE_RLL_IMAX,2000
RATE_RLL_P,0.11
INS_ACCEL_FILTER,20
INS_GYRO_FILTER,20
MOT_THST_EXPO,0.3
MOT_THST_MAX,1
RATE_PIT_D,0.0007353554
RATE_PIT_I,0.09863888
RATE_PIT_P,0.09863888
RATE_RLL_D,0.0004876749
RATE_RLL_I,0.1183951
RATE_RLL_P,0.1183951
RATE_YAW_D,0
RATE_YAW_FILT_HZ,3.0
RATE_YAW_I,0.103
RATE_YAW_IMAX,1000
RATE_YAW_P,1.03
RATE_YAW_FILT_HZ,1.001376
RATE_YAW_I,0.09345812
RATE_YAW_P,0.9345812
STB_PIT_P,15
STB_RLL_P,10
STB_YAW_P,12
THR_MIN,130
STB_RLL_P,15
STB_YAW_P,14.25551

5
Tools/Hello/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
Tools/Replay/wscript
View File

@ -1,11 +1,8 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.program(
bld,
bld.ap_program(
use='ap',
blddestdir='tools',
)

45
Tools/ardupilotwaf/ardupilotwaf.py
View File

@ -3,6 +3,7 @@
from __future__ import print_function
from waflib import Logs, Options, Utils
from waflib.Configure import conf
import os.path
SOURCE_EXTS = [
@ -50,11 +51,11 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [
'StorageManager',
]
def _get_legacy_defines(name):
def _get_legacy_defines(sketch_name):
return [
'APM_BUILD_DIRECTORY=' + name,
'SKETCH="' + name + '"',
'SKETCHNAME="' + name + '"',
'APM_BUILD_DIRECTORY=APM_BUILD_' + sketch_name,
'SKETCH="' + sketch_name + '"',
'SKETCHNAME="' + sketch_name + '"',
]
IGNORED_AP_LIBRARIES = [
@ -63,7 +64,8 @@ IGNORED_AP_LIBRARIES = [
'GCS_Console',
]
def get_all_libraries(bld):
@conf
def ap_get_all_libraries(bld):
libraries = []
for lib_node in bld.srcnode.ant_glob('libraries/*', dir=True):
name = lib_node.name
@ -75,7 +77,12 @@ def get_all_libraries(bld):
libraries.extend(['AP_HAL', 'AP_HAL_Empty'])
return libraries
def program(bld, blddestdir='bin',
@conf
def ap_common_vehicle_libraries(bld):
return COMMON_VEHICLE_DEPENDENT_LIBRARIES
@conf
def ap_program(bld, blddestdir='bin',
use_legacy_defines=True,
program_name=None,
**kw):
@ -90,7 +97,7 @@ def program(bld, blddestdir='bin',
program_name = bld.path.name
if use_legacy_defines:
kw['defines'].extend(_get_legacy_defines(program_name))
kw['defines'].extend(_get_legacy_defines(bld.path.name))
kw['features'] = common_features(bld) + kw.get('features', [])
@ -103,9 +110,10 @@ def program(bld, blddestdir='bin',
**kw
)
def example(bld, **kw):
@conf
def ap_example(bld, **kw):
kw['blddestdir'] = 'examples'
program(bld, **kw)
ap_program(bld, **kw)
# NOTE: Code in libraries/ is compiled multiple times. So ensure each
# compilation is independent by providing different index for each.
@ -124,13 +132,14 @@ def common_features(bld):
features.append('static_linking')
return features
def vehicle_stlib(bld, **kw):
@conf
def ap_stlib(bld, **kw):
if 'name' not in kw:
bld.fatal('Missing name for vehicle_stlib')
bld.fatal('Missing name for ap_stlib')
if 'vehicle' not in kw:
bld.fatal('Missing vehicle for vehicle_stlib')
bld.fatal('Missing vehicle for ap_stlib')
if 'libraries' not in kw:
bld.fatal('Missing libraries for vehicle_stlib')
bld.fatal('Missing libraries for ap_stlib')
sources = []
libraries = kw['libraries'] + bld.env.AP_LIBRARIES
@ -149,7 +158,8 @@ def vehicle_stlib(bld, **kw):
bld.stlib(**kw)
def find_tests(bld, use=[]):
@conf
def ap_find_tests(bld, use=[]):
if not bld.env.HAS_GTEST:
return
@ -163,7 +173,7 @@ def find_tests(bld, use=[]):
includes = [bld.srcnode.abspath() + '/tests/']
for f in bld.path.ant_glob(incl='*.cpp'):
program(
ap_program(
bld,
features=features,
includes=includes,
@ -174,14 +184,15 @@ def find_tests(bld, use=[]):
use_legacy_defines=False,
)
def find_benchmarks(bld, use=[]):
@conf
def ap_find_benchmarks(bld, use=[]):
if not bld.env.HAS_GBENCHMARK:
return
includes = [bld.srcnode.abspath() + '/benchmarks/']
for f in bld.path.ant_glob(incl='*.cpp'):
program(
ap_program(
bld,
features=common_features(bld) + ['gbenchmark'],
includes=includes,

5
libraries/AC_PID/examples/AC_PID_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_ADC/examples/AP_ADC_test/wscript
View File

@ -1,13 +1,10 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
if bld.env.BOARD in ['sitl']:
return
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -272,6 +272,16 @@ public:
return false;
}
// returns the expected NED magnetic field
virtual bool get_expected_mag_field_NED(Vector3f &ret) const {
return false;
}
// returns the estimated magnetic field offsets in body frame
virtual bool get_mag_field_correction(Vector3f &ret) const {
return false;
}
// return a position relative to home in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true

46
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -515,6 +515,52 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
}
}
// returns the expected NED magnetic field
bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
EKF1.getMagNED(vec);
return true;
case EKF_TYPE2:
EKF2.getMagNED(-1,vec);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
}
// returns the estimated magnetic field offsets in body frame
bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE1:
default:
EKF1.getMagXYZ(vec);
return true;
case EKF_TYPE2:
EKF2.getMagXYZ(-1,vec);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
}
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity)

6
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -197,6 +197,12 @@ public:
// boolean false is returned if variances are not available
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// returns the expected NED magnetic field
bool get_mag_field_NED(Vector3f& ret) const;
// returns the estimated magnetic field offsets in body frame
bool get_mag_field_correction(Vector3f &ret) const;
void setTakeoffExpected(bool val);
void setTouchdownExpected(bool val);

5
libraries/AP_AHRS/examples/AHRS_Test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

8
libraries/AP_AccelCal/AP_AccelCal.cpp
View File

@ -22,6 +22,9 @@ const extern AP_HAL::HAL& hal;
static bool _start_collect_sample;
static void _snoop(const mavlink_message_t* msg);
uint8_t AP_AccelCal::_num_clients = 0;
AP_AccelCal_Client* AP_AccelCal::_clients[AP_ACCELCAL_MAX_NUM_CLIENTS] {};
void AP_AccelCal::update()
{
if (!get_calibrator(0)) {
@ -245,13 +248,10 @@ void AP_AccelCal::collect_sample()
}
void AP_AccelCal::register_client(AP_AccelCal_Client* client) {
if (client == NULL || _num_clients == AP_ACCELCAL_MAX_NUM_CLIENTS) {
if (client == NULL || _num_clients >= AP_ACCELCAL_MAX_NUM_CLIENTS) {
return;
}
if (_started) {
fail();
}
for(uint8_t i=0; i<_num_clients; i++) {
if(_clients[i] == client) {

12
libraries/AP_AccelCal/AP_AccelCal.h
View File

@ -11,7 +11,6 @@ class AP_AccelCal_Client;
class AP_AccelCal {
public:
AP_AccelCal():
_num_clients(0),
_started(false),
_saving(false)
{ update_status(); }
@ -25,18 +24,19 @@ public:
// Run an iteration of all registered calibrations
void update();
// interface to the clients for registration
void register_client(AP_AccelCal_Client* client);
// get the status of the calibrator server as a whole
accel_cal_status_t get_status() { return _status; }
// interface to the clients for registration
static void register_client(AP_AccelCal_Client* client);
private:
GCS_MAVLINK *_gcs;
uint8_t _step;
accel_cal_status_t _status;
uint8_t _num_clients;
AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
static uint8_t _num_clients;
static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
// called on calibration success
void success();

5
libraries/AP_Airspeed/examples/Airspeed/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Baro/examples/BARO_generic/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Common/examples/AP_Common/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Compass/examples/AP_Compass_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Declination/examples/AP_Declination_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_GPS/examples/GPS_AUTO_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/AnalogIn/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/Printf/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/RCInput/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/RCInputToRCOutput/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/RCOutput/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/Storage/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL/examples/UART_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/Blink/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/Console/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/LCDTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/RCInputTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/Scheduler/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/Semaphore/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/Storage/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/UARTDriver/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_Linux/benchmarks/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.find_benchmarks(
bld,
bld.ap_find_benchmarks(
use='ap',
)

5
libraries/AP_HAL_Linux/examples/BusTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_HAL_PX4/examples/simple/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

187
libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp Normal file
View File

@ -0,0 +1,187 @@
/*
get system network addresses
based on code from Samba
Copyright (C) Andrew Tridgell 1998
Copyright (C) Jeremy Allison 2007
Copyright (C) Jelmer Vernooij <jelmer@samba.org> 2007
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
#include <ifaddrs.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
void freeifaddrs(struct ifaddrs *ifp)
{
if (ifp != NULL) {
free(ifp->ifa_name);
free(ifp->ifa_addr);
free(ifp->ifa_netmask);
free(ifp->ifa_dstaddr);
freeifaddrs(ifp->ifa_next);
free(ifp);
}
}
static struct sockaddr *sockaddr_dup(struct sockaddr *sa)
{
struct sockaddr *ret;
socklen_t socklen;
socklen = sizeof(struct sockaddr_storage);
ret = (struct sockaddr *)calloc(1, socklen);
if (ret == NULL)
return NULL;
memcpy(ret, sa, socklen);
return ret;
}
/* this works for Linux 2.2, Solaris 2.5, SunOS4, HPUX 10.20, OSF1
V4.0, Ultrix 4.4, SCO Unix 3.2, IRIX 6.4 and FreeBSD 3.2.
It probably also works on any BSD style system. */
int getifaddrs(struct ifaddrs **ifap)
{
struct ifconf ifc;
char buff[8192];
int fd, i, n;
struct ifreq *ifr=NULL;
struct ifaddrs *curif;
struct ifaddrs *lastif = NULL;
*ifap = NULL;
if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
return -1;
}
ifc.ifc_len = sizeof(buff);
ifc.ifc_buf = buff;
if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
close(fd);
return -1;
}
ifr = ifc.ifc_req;
n = ifc.ifc_len / sizeof(struct ifreq);
/* Loop through interfaces, looking for given IP address */
for (i=n-1; i>=0; i--) {
if (ioctl(fd, SIOCGIFFLAGS, &ifr[i]) == -1) {
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif = (struct ifaddrs *)calloc(1, sizeof(struct ifaddrs));
if (curif == NULL) {
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif->ifa_name = strdup(ifr[i].ifr_name);
if (curif->ifa_name == NULL) {
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
curif->ifa_flags = ifr[i].ifr_flags;
curif->ifa_dstaddr = NULL;
curif->ifa_data = NULL;
curif->ifa_next = NULL;
curif->ifa_addr = NULL;
if (ioctl(fd, SIOCGIFADDR, &ifr[i]) != -1) {
curif->ifa_addr = sockaddr_dup(&ifr[i].ifr_addr);
if (curif->ifa_addr == NULL) {
free(curif->ifa_name);
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
}
curif->ifa_netmask = NULL;
if (ioctl(fd, SIOCGIFNETMASK, &ifr[i]) != -1) {
curif->ifa_netmask = sockaddr_dup(&ifr[i].ifr_addr);
if (curif->ifa_netmask == NULL) {
if (curif->ifa_addr != NULL) {
free(curif->ifa_addr);
}
free(curif->ifa_name);
free(curif);
freeifaddrs(*ifap);
close(fd);
return -1;
}
}
if (lastif == NULL) {
*ifap = curif;
} else {
lastif->ifa_next = curif;
}
lastif = curif;
}
close(fd);
return 0;
}
const char *get_ipv4_broadcast(void)
{
struct ifaddrs *ifap = NULL;
if (getifaddrs(&ifap) != 0) {
return NULL;
}
struct ifaddrs *ia;
for (ia=ifap; ia; ia=ia->ifa_next) {
struct sockaddr_in *sin = (struct sockaddr_in *)ia->ifa_addr;
struct sockaddr_in *nmask = (struct sockaddr_in *)ia->ifa_netmask;
struct in_addr bcast;
if (strcmp(ia->ifa_name, "lo") == 0) {
continue;
}
bcast.s_addr = sin->sin_addr.s_addr | ~nmask->sin_addr.s_addr;
const char *ret = inet_ntoa(bcast);
freeifaddrs(ifap);
return ret;
}
freeifaddrs(ifap);
return NULL;
}
#ifdef MAIN_PROGRAM
int main(void)
{
printf("%s\n", get_ipv4_broadcast());
return 0;
}
#endif

12
libraries/AP_HAL_QURT/mainapp/mainapp.cpp
View File

@ -23,6 +23,8 @@ static uint64_t start_time;
#define STORAGE_DIR "/var/APM"
#define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
extern const char *get_ipv4_broadcast(void);
// time since startup in microseconds
static uint64_t micros64()
{
@ -82,6 +84,7 @@ static void get_storage(void)
*/
static void socket_check(void)
{
static const char *bcast = NULL;
uint8_t buf[300];
ssize_t ret = sock.recv(buf, sizeof(buf), 0);
if (ret > 0) {
@ -99,9 +102,16 @@ static void socket_check(void)
}
}
uint32_t nbytes;
if (bcast == NULL) {
bcast = get_ipv4_broadcast();
if (bcast == NULL) {
bcast = "255.255.255.255";
}
printf("Broadcasting to %s\n", bcast);
}
if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) {
if (!connected) {
sock.sendto(buf, nbytes, "255.255.255.255", 14550);
sock.sendto(buf, nbytes, bcast, 14550);
} else {
sock.send(buf, nbytes);
}

8
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -368,6 +368,8 @@ AP_InertialSensor::AP_InertialSensor() :
}
memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
AP_AccelCal::register_client(this);
}
/*
@ -1279,8 +1281,6 @@ void AP_InertialSensor::acal_init()
if (_accel_calibrator == NULL) {
_accel_calibrator = new AccelCalibrator[INS_MAX_INSTANCES];
}
_acal->register_client(this);
}
// update accel calibrator
@ -1334,7 +1334,7 @@ void AP_InertialSensor::_acal_save_calibrations()
// determine trim from aligned sample vs misaligned sample
Vector3f cross = (misaligned_sample%aligned_sample);
float dot = (misaligned_sample*aligned_sample);
Quaternion q(sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
q.normalize();
_trim_roll = q.get_euler_roll();
_trim_pitch = q.get_euler_pitch();
@ -1343,7 +1343,7 @@ void AP_InertialSensor::_acal_save_calibrations()
break;
default:
_new_trim = false;
//noop
/* no break */
}
if (fabsf(_trim_roll) > radians(10) ||

5
libraries/AP_InertialSensor/examples/INS_generic/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_InertialSensor/examples/VibTest/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Math/benchmarks/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.find_benchmarks(
bld,
bld.ap_find_benchmarks(
use='ap',
)

5
libraries/AP_Math/examples/eulers/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Math/examples/location/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Math/examples/polygon/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Math/examples/rotations/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Math/tests/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.find_tests(
bld,
bld.ap_find_tests(
use='ap',
)

5
libraries/AP_Mission/examples/AP_Mission_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
View File

@ -1,13 +1,10 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
# TODO: Test code doesn't build. Fix or delete the test.
return
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Motors/examples/AP_Motors_test/wscript
View File

@ -1,13 +1,10 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
# TODO: Test code doesn't build. Fix or delete the test.
return
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

212
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -1,212 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_Gimbal.h"
#if AP_AHRS_NAVEKF_AVAILABLE
#include <stdio.h>
#include <AP_Common/AP_Common.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_NavEKF/AP_SmallEKF.h>
#include <AP_Math/AP_Math.h>
void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
decode_feedback(msg);
update_state();
if (_ekf.getStatus() && !isCopterFlipped() && !is_zero(_gimbalParams.K_gimbalRate)){
send_control(chan);
}
Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
//::printf("est=%1.1f %1.1f %1.1f %d\t", eulerEst.x,eulerEst.y,eulerEst.z,_ekf.getStatus());
//::printf("joint_angles=(%+1.2f %+1.2f %+1.2f)\t", _measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
//::printf("delta_ang=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_angles.x,_measurement.delta_angles.y,_measurement.delta_angles.z);
//::printf("delta_vel=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_velocity.x,_measurement.delta_velocity.y,_measurement.delta_velocity.z);
//::printf("rate=(%+1.3f %+1.3f %+1.3f)\t",gimbalRateDemVec.x,gimbalRateDemVec.y,gimbalRateDemVec.z);
//::printf("target=(%+1.3f %+1.3f %+1.3f)\t",_angle_ef_target_rad.x,_angle_ef_target_rad.y,_angle_ef_target_rad.z);
//::printf("\n");
}
void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
{
mavlink_msg_gimbal_report_decode(msg, &_report_msg);
_measurement.delta_time = _report_msg.delta_time;
_measurement.delta_angles.x = _report_msg.delta_angle_x;
_measurement.delta_angles.y = _report_msg.delta_angle_y;
_measurement.delta_angles.z = _report_msg.delta_angle_z;
_measurement.delta_velocity.x = _report_msg.delta_velocity_x,
_measurement.delta_velocity.y = _report_msg.delta_velocity_y;
_measurement.delta_velocity.z = _report_msg.delta_velocity_z;
_measurement.joint_angles.x = _report_msg.joint_roll;
_measurement.joint_angles.y = _report_msg.joint_el;
_measurement.joint_angles.z = _report_msg.joint_az;
//apply joint angle compensation
_measurement.joint_angles -= _gimbalParams.joint_angles_offsets;
_measurement.delta_velocity -= _gimbalParams.delta_velocity_offsets;
_measurement.delta_angles -= _gimbalParams.delta_angles_offsets;
}
/*
send a gimbal report to the GCS for display purposes
*/
void AP_Gimbal::send_report(mavlink_channel_t chan) const
{
mavlink_msg_gimbal_report_send(chan,
0, 0, // send as broadcast
_report_msg.delta_time,
_report_msg.delta_angle_x,
_report_msg.delta_angle_y,
_report_msg.delta_angle_z,
_report_msg.delta_velocity_x,
_report_msg.delta_velocity_y,
_report_msg.delta_velocity_z,
_report_msg.joint_roll,
_report_msg.joint_el,
_report_msg.joint_az);
}
void AP_Gimbal::update_state()
{
// Run the gimbal attitude and gyro bias estimator
_ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
// get the gimbal quaternion estimate
Quaternion quatEst;
_ekf.getQuat(quatEst);
// Add the control rate vectors
gimbalRateDemVec.zero();
gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
gimbalRateDemVec += getGimbalRateDemVecGyroBias();
}
Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
{
// Define rotation from vehicle to gimbal using a 312 rotation sequence
Matrix3f Tvg;
float cosPhi = cosf(_measurement.joint_angles.x);
float cosTheta = cosf(_measurement.joint_angles.y);
float sinPhi = sinf(_measurement.joint_angles.x);
float sinTheta = sinf(_measurement.joint_angles.y);
float sinPsi = sinf(_measurement.joint_angles.z);
float cosPsi = cosf(_measurement.joint_angles.z);
Tvg[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
Tvg[1][0] = -sinPsi*cosPhi;
Tvg[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
Tvg[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
Tvg[1][1] = cosPsi*cosPhi;
Tvg[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
Tvg[0][2] = -sinTheta*cosPhi;
Tvg[1][2] = sinPhi;
Tvg[2][2] = cosTheta*cosPhi;
// multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
Vector3f gimbalRateDemVecYaw;
gimbalRateDemVecYaw.z = - _gimbalParams.K_gimbalRate * _measurement.joint_angles.z;
// Get filtered vehicle turn rate in earth frame
vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.delta_time * _ahrs.get_yaw_rate_earth();
Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
// calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
float maxRate = _gimbalParams.K_gimbalRate * yawErrorLimit;
float vehicle_rate_mag_ef = vehicle_rate_ef.length();
float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate;
if (vehicle_rate_mag_ef > maxRate) {
if (vehicle_rate_ef.z>0.0f){
gimbalRateDemVecYaw += _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
} else {
gimbalRateDemVecYaw -= _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
}
}
// rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
return gimbalRateDemVecYaw;
}
Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst)
{
// Calculate the gimbal 321 Euler angle estimates relative to earth frame
Vector3f eulerEst = quatEst.to_vector312();
// Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
Quaternion quatDem;
quatDem.from_vector312( _angle_ef_target_rad.x,
_angle_ef_target_rad.y,
eulerEst.z);
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;
// Convert to a delta rotation using a small angle approximation
quatErr.normalize();
Vector3f deltaAngErr;
float scaler;
if (quatErr[0] >= 0.0f) {
scaler = 2.0f;
} else {
scaler = -2.0f;
}
deltaAngErr.x = scaler * quatErr[1];
deltaAngErr.y = scaler * quatErr[2];
deltaAngErr.z = scaler * quatErr[3];
// multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate;
return gimbalRateDemVecTilt;
}
Vector3f AP_Gimbal::getGimbalRateDemVecForward(const Quaternion &quatEst)
{
// quaternion demanded at the previous time step
static float lastDem;
// calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
float delta = _angle_ef_target_rad.y - lastDem;
lastDem = _angle_ef_target_rad.y;
Vector3f gimbalRateDemVecForward;
gimbalRateDemVecForward.y = delta / _measurement.delta_time;
return gimbalRateDemVecForward;
}
Vector3f AP_Gimbal::getGimbalRateDemVecGyroBias()
{
Vector3f gyroBias;
_ekf.getGyroBias(gyroBias);
return gyroBias;
}
void AP_Gimbal::send_control(mavlink_channel_t chan)
{
mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
}
void AP_Gimbal::update_target(Vector3f newTarget)
{
// Low-pass filter
_angle_ef_target_rad.y = _angle_ef_target_rad.y + 0.02f*(newTarget.y - _angle_ef_target_rad.y);
// Update tilt
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y,radians(-90),radians(0));
}
Vector3f AP_Gimbal::getGimbalEstimateEF()
{
Quaternion quatEst;
_ekf.getQuat(quatEst);
return quatEst.to_vector312();
}
bool AP_Gimbal::isCopterFlipped()
{
return (_ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f);
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

95
libraries/AP_Mount/AP_Gimbal.h
View File

@ -1,95 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/************************************************************
* AP_Gimbal -- library to control a 3 axis rate gimbal. *
* *
* Author: Arthur Benemann, Paul Riseborough; *
* *
************************************************************/
#ifndef __AP_GIMBAL_H__
#define __AP_GIMBAL_H__
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Mount.h"
#include <AP_NavEKF/AP_SmallEKF.h>
#include <AP_NavEKF/AP_NavEKF.h>
class AP_Gimbal
{
public:
//Constructor
AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t compid, const AP_Mount::gimbal_params &gimbalParams) :
_ekf(ahrs),
_ahrs(ahrs),
_gimbalParams(gimbalParams),
vehicleYawRateFilt(0.0f),
yawRateFiltPole(10.0f),
yawErrorLimit(0.1f)
{
_compid = compid;
memset(&_report_msg, 0, sizeof(_report_msg));
}
void update_target(Vector3f newTarget);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
void send_report(mavlink_channel_t chan) const;
Vector3f getGimbalEstimateEF();
struct Measurament {
float delta_time;
Vector3f delta_angles;
Vector3f delta_velocity;
Vector3f joint_angles;
} _measurement;
SmallEKF _ekf; // state of small EKF for gimbal
const AP_AHRS_NavEKF &_ahrs; // Main EKF
Vector3f gimbalRateDemVec; // degrees/s
Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians
private:
const AP_Mount::gimbal_params &_gimbalParams;
// filtered yaw rate from the vehicle
float vehicleYawRateFilt;
// circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
// this frequency must not be larger than the update rate (Hz).
// reducing it makes the gimbal yaw less responsive to vehicle yaw
// increasing it makes the gimbal yawe more responsive to vehicle yaw
float const yawRateFiltPole;
// amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
// reducing this makes the gimbal respond more to vehicle yaw disturbances
float const yawErrorLimit;
uint8_t _compid;
mavlink_gimbal_report_t _report_msg;
void send_control(mavlink_channel_t chan);
void update_state();
void decode_feedback(mavlink_message_t *msg);
bool isCopterFlipped();
// Control loop functions
Vector3f getGimbalRateDemVecYaw(const Quaternion &quatEst);
Vector3f getGimbalRateDemVecTilt(const Quaternion &quatEst);
Vector3f getGimbalRateDemVecForward(const Quaternion &quatEst);
Vector3f getGimbalRateDemVecGyroBias();
};
#endif // AP_AHRS_NAVEKF_AVAILABLE
#endif // __AP_MOUNT_H__

108
libraries/AP_Mount/AP_Mount.cpp
View File

@ -5,7 +5,7 @@
#include "AP_Mount.h"
#include "AP_Mount_Backend.h"
#include "AP_Mount_Servo.h"
#include "AP_Mount_MAVLink.h"
#include "AP_Mount_SoloGimbal.h"
#include "AP_Mount_Alexmos.h"
#include "AP_Mount_SToRM32.h"
#include "AP_Mount_SToRM32_serial.h"
@ -200,80 +200,15 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
// @User: Standard
AP_GROUPINFO("_TYPE", 19, AP_Mount, state[0]._type, 0),
// @Param: _OFF_JNT_X
// @DisplayName: MAVLink Mount's roll angle offsets
// @Description: MAVLink Mount's roll angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
// 20 formerly _OFF_JNT
// @Param: _OFF_JNT_Y
// @DisplayName: MAVLink Mount's pitch angle offsets
// @Description: MAVLink Mount's pitch angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
// 21 formerly _OFF_ACC
// @Param: _OFF_JNT_Z
// @DisplayName: MAVLink Mount's yaw angle offsets
// @Description: MAVLink Mount's yaw angle offsets
// @Units: radians
// @Range: 0 0.5
// @User: Advanced
AP_GROUPINFO("_OFF_JNT", 20, AP_Mount, state[0]._gimbalParams.joint_angles_offsets, 0),
// 22 formerly _OFF_GYRO
// @Param: _OFF_ACC_X
// @DisplayName: MAVLink Mount's roll velocity offsets
// @Description: MAVLink Mount's roll velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
// 23 formerly _K_RATE
// @Param: _OFF_ACC_Y
// @DisplayName: MAVLink Mount's pitch velocity offsets
// @Description: MAVLink Mount's pitch velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
// @Param: _OFF_ACC_Z
// @DisplayName: MAVLink Mount's yaw velocity offsets
// @Description: MAVLink Mount's yaw velocity offsets
// @Units: m/s
// @Range: 0 2
// @User: Advanced
AP_GROUPINFO("_OFF_ACC", 21, AP_Mount, state[0]._gimbalParams.delta_velocity_offsets, 0),
// @Param: _OFF_GYRO_X
// @DisplayName: MAVLink Mount's roll gyro offsets
// @Description: MAVLink Mount's roll gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_GYRO_Y
// @DisplayName: MAVLink Mount's pitch gyro offsets
// @Description: MAVLink Mount's pitch gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
// @Param: _OFF_GYRO_Z
// @DisplayName: MAVLink Mount's yaw gyro offsets
// @Description: MAVLink Mount's yaw gyro offsets
// @Units: radians/sec
// @Range: 0 0.5
// @User: Advanced
AP_GROUPINFO("_OFF_GYRO", 22, AP_Mount, state[0]._gimbalParams.delta_angles_offsets, 0),
// @Param: _K_RATE
// @DisplayName: MAVLink Mount's rate gain
// @Description: MAVLink Mount's rate gain
// @Range: 0 10
// @User: Advanced
AP_GROUPINFO("_K_RATE", 23, AP_Mount, state[0]._gimbalParams.K_gimbalRate, 5.0f),
// 20 ~ 24 reserved for future parameters
// 24 is AVAILABLE
#if AP_MOUNT_MAX_INSTANCES > 1
// @Param: 2_DEFLT_MODE
@ -475,13 +410,15 @@ AP_Mount::AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc)
}
// init - detect and initialise all mounts
void AP_Mount::init(const AP_SerialManager& serial_manager)
void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
{
// check init has not been called before
if (_num_instances != 0) {
return;
}
_dataflash = dataflash;
// default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
if (!state[0]._type.configured()) {
if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) ||
@ -508,8 +445,8 @@ void AP_Mount::init(const AP_SerialManager& serial_manager)
#if AP_AHRS_NAVEKF_AVAILABLE
// check for MAVLink mounts
} else if (mount_type == Mount_Type_MAVLink) {
_backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance);
} else if (mount_type == Mount_Type_SoloGimbal) {
_backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance);
_num_instances++;
#endif
@ -551,6 +488,17 @@ void AP_Mount::update()
}
}
// used for gimbals that need to read INS data at full rate
void AP_Mount::update_fast()
{
// update each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->update_fast();
}
}
}
// get_mount_type - returns the type of mount
AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const
{
@ -674,7 +622,17 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *m
if (_backends[instance] != NULL) {
_backends[instance]->handle_gimbal_report(chan, msg);
}
}
}
}
// handle PARAM_VALUE
void AP_Mount::handle_param_value(mavlink_message_t *msg)
{
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->handle_param_value(msg);
}
}
}
// send a GIMBAL_REPORT message to the GCS

27
libraries/AP_Mount/AP_Mount.h
View File

@ -29,6 +29,7 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <DataFlash/DataFlash.h>
// maximum number of mounts
#define AP_MOUNT_MAX_INSTANCES 1
@ -36,7 +37,7 @@
// declare backend classes
class AP_Mount_Backend;
class AP_Mount_Servo;
class AP_Mount_MAVLink;
class AP_Mount_SoloGimbal;
class AP_Mount_Alexmos;
class AP_Mount_SToRM32;
class AP_Mount_SToRM32_serial;
@ -51,7 +52,7 @@ class AP_Mount
// declare backends as friends
friend class AP_Mount_Backend;
friend class AP_Mount_Servo;
friend class AP_Mount_MAVLink;
friend class AP_Mount_SoloGimbal;
friend class AP_Mount_Alexmos;
friend class AP_Mount_SToRM32;
friend class AP_Mount_SToRM32_serial;
@ -62,28 +63,24 @@ public:
enum MountType {
Mount_Type_None = 0, /// no mount
Mount_Type_Servo = 1, /// servo controlled mount
Mount_Type_MAVLink = 2, /// MAVLink controlled mount
Mount_Type_SoloGimbal = 2, /// Solo's gimbal
Mount_Type_Alexmos = 3, /// Alexmos mount
Mount_Type_SToRM32 = 4, /// SToRM32 mount using MAVLink protocol
Mount_Type_SToRM32_serial = 5 /// SToRM32 mount using custom serial protocol
};
struct gimbal_params {
AP_Vector3f delta_angles_offsets;
AP_Vector3f delta_velocity_offsets;
AP_Vector3f joint_angles_offsets;
AP_Float K_gimbalRate;
};
// Constructor
AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc);
// init - detect and initialise all mounts
void init(const AP_SerialManager& serial_manager);
void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager);
// update - give mount opportunity to update servos. should be called at 10hz or higher
void update();
// used for gimbals that need to read INS data at full rate
void update_fast();
// get_mount_type - returns the type of mount
AP_Mount::MountType get_mount_type() const { return get_mount_type(_primary); }
AP_Mount::MountType get_mount_type(uint8_t instance) const;
@ -126,6 +123,9 @@ public:
void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); }
void control_msg(uint8_t instance, mavlink_message_t* msg);
// handle a PARAM_VALUE message
void handle_param_value(mavlink_message_t *msg);
// handle a GIMBAL_REPORT message
void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
@ -182,10 +182,9 @@ protected:
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)
struct Location _roi_target; // roi target location
struct gimbal_params _gimbalParams;
} state[AP_MOUNT_MAX_INSTANCES];
DataFlash_Class *_dataflash;
};
#endif // __AP_MOUNT_H__

6
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -44,6 +44,9 @@ public:
// update mount position - should be called periodically
virtual void update() = 0;
// used for gimbals that need to read INS data at full rate
virtual void update_fast() {}
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
virtual bool has_pan_control() const = 0;
@ -71,6 +74,9 @@ public:
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
// handle a PARAM_VALUE message
virtual void handle_param_value(mavlink_message_t *msg) {}
// send a GIMBAL_REPORT message to the GCS
virtual void send_gimbal_report(mavlink_channel_t chan) {}

73
libraries/AP_Mount/AP_Mount_MAVLink.cpp → libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -1,30 +1,40 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_Mount_MAVLink.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include "AP_Mount_SoloGimbal.h"
#include "SoloGimbal.h"
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdio.h>
#include "AP_Gimbal.h"
#if MOUNT_DEBUG
#include <stdio.h>
#endif
AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
extern const AP_HAL::HAL& hal;
AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance),
_initialised(false),
_gimbal(frontend._ahrs, MAV_COMP_ID_GIMBAL, _state._gimbalParams)
_gimbal(frontend._ahrs)
{}
// init - performs any required initialisation for this instance
void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager)
void AP_Mount_SoloGimbal::init(const AP_SerialManager& serial_manager)
{
_initialised = true;
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
}
void AP_Mount_SoloGimbal::update_fast()
{
_gimbal.update_fast();
}
// update mount position - should be called periodically
void AP_Mount_MAVLink::update()
void AP_Mount_SoloGimbal::update()
{
// exit immediately if not initialised
if (!_initialised) {
@ -35,25 +45,36 @@ void AP_Mount_MAVLink::update()
switch(get_mode()) {
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT:
_gimbal.set_lockedToBody(true);
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
_gimbal.set_lockedToBody(false);
const Vector3f &target = _state._neutral_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
_gimbal.set_lockedToBody(false);
// do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
_gimbal.set_lockedToBody(false);
// update targets using pilot's rc inputs
update_targets_from_rc();
break;
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
_gimbal.set_lockedToBody(false);
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
}
@ -66,14 +87,14 @@ void AP_Mount_MAVLink::update()
}
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
bool AP_Mount_MAVLink::has_pan_control() const
bool AP_Mount_SoloGimbal::has_pan_control() const
{
// we do not have yaw control
return false;
}
// set_mode - sets mount's mode
void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
{
// exit immediately if not initialised
if (!_initialised) {
@ -85,27 +106,49 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
}
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan)
{
Vector3f est = _gimbal.getGimbalEstimateEF();
mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100);
if (_gimbal.aligned()) {
mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100);
}
}
/*
handle a GIMBAL_REPORT message
*/
void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
{
_gimbal.update_target(_angle_ef_target_rad);
_gimbal.receive_feedback(chan,msg);
if(!_params_saved && _frontend._dataflash != NULL && _frontend._dataflash->logging_started()) {
_gimbal.fetch_params(); //last parameter save might not be stored in dataflash so retry
_params_saved = true;
}
if (_gimbal.get_log_dt() > 1.0f/25.0f) {
_gimbal.write_logs(_frontend._dataflash);
}
}
void AP_Mount_SoloGimbal::handle_param_value(mavlink_message_t *msg)
{
_gimbal.handle_param_value(_frontend._dataflash, msg);
}
/*
handle a GIMBAL_REPORT message
*/
void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg)
{
_gimbal.disable_torque_report();
}
/*
send a GIMBAL_REPORT message to the GCS
*/
void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
void AP_Mount_SoloGimbal::send_gimbal_report(mavlink_channel_t chan)
{
_gimbal.send_report(chan);
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

25
libraries/AP_Mount/AP_Mount_MAVLink.h → libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -4,8 +4,9 @@
MAVLink enabled mount backend class
*/
#ifndef __AP_MOUNT_MAVLINK_H__
#define __AP_MOUNT_MAVLINK_H__
#ifndef __AP_MOUNT_SOLOGIMBAL_H__
#define __AP_MOUNT_SOLOGIMBAL_H__
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
@ -17,14 +18,15 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <RC_Channel/RC_Channel.h>
#include "AP_Mount_Backend.h"
#include "AP_Gimbal.h"
#include "SoloGimbal.h"
class AP_Mount_MAVLink : public AP_Mount_Backend
class AP_Mount_SoloGimbal : public AP_Mount_Backend
{
public:
// Constructor
AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
// init - performs any required initialisation for this instance
virtual void init(const AP_SerialManager& serial_manager);
@ -43,17 +45,26 @@ public:
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
virtual void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg);
virtual void handle_param_value(mavlink_message_t *msg);
// send a GIMBAL_REPORT message to the GCS
virtual void send_gimbal_report(mavlink_channel_t chan);
virtual void update_fast();
private:
// internal variables
bool _initialised; // true once the driver has been initialised
AP_Gimbal _gimbal;
// Write a gimbal measurament and estimation data packet
void Log_Write_Gimbal(SoloGimbal &gimbal);
bool _params_saved;
SoloGimbal _gimbal;
};
#endif // AP_AHRS_NAVEKF_AVAILABLE
#endif // __AP_MOUNT_MAVLINK_H__
#endif // __AP_MOUNT_SOLOGIMBAL_H__

489
libraries/AP_Mount/SoloGimbal.cpp Normal file
View File

@ -0,0 +1,489 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include "SoloGimbal.h"
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
bool SoloGimbal::present()
{
if (_state != GIMBAL_STATE_NOT_PRESENT && AP_HAL::millis()-_last_report_msg_ms > 3000) {
// gimbal went away
_state = GIMBAL_STATE_NOT_PRESENT;
return false;
}
return _state != GIMBAL_STATE_NOT_PRESENT;
}
bool SoloGimbal::aligned()
{
return present() && _state == GIMBAL_STATE_PRESENT_RUNNING;
}
gimbal_mode_t SoloGimbal::get_mode()
{
if ((_gimbalParams.initialized() && _gimbalParams.get_K_rate()==0.0f) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
return GIMBAL_MODE_IDLE;
} else if (!_ekf.getStatus()) {
return GIMBAL_MODE_POS_HOLD;
} else if (_calibrator.running() || _lockedToBody) {
return GIMBAL_MODE_POS_HOLD_FF;
} else {
return GIMBAL_MODE_STABILIZE;
}
}
void SoloGimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
mavlink_gimbal_report_t report_msg;
mavlink_msg_gimbal_report_decode(msg, &report_msg);
uint32_t tnow_ms = AP_HAL::millis();
_last_report_msg_ms = tnow_ms;
_gimbalParams.set_channel(chan);
if (report_msg.target_system != 1) {
_state = GIMBAL_STATE_NOT_PRESENT;
}
switch(_state) {
case GIMBAL_STATE_NOT_PRESENT:
// gimbal was just connected or we just rebooted, transition to PRESENT_INITIALIZING
_gimbalParams.reset();
_gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
_state = GIMBAL_STATE_PRESENT_INITIALIZING;
break;
case GIMBAL_STATE_PRESENT_INITIALIZING:
_gimbalParams.update();
if (_gimbalParams.initialized()) {
// parameters done initializing, finalize initialization and transition to aligning
extract_feedback(report_msg);
_ang_vel_mag_filt = 20;
_filtered_joint_angles = _measurement.joint_angles;
_vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
_ekf.reset();
_state = GIMBAL_STATE_PRESENT_ALIGNING;
}
break;
case GIMBAL_STATE_PRESENT_ALIGNING:
_gimbalParams.update();
extract_feedback(report_msg);
update_estimators();
if (_ekf.getStatus()) {
// EKF done aligning, transition to running
_state = GIMBAL_STATE_PRESENT_RUNNING;
}
break;
case GIMBAL_STATE_PRESENT_RUNNING:
_gimbalParams.update();
extract_feedback(report_msg);
update_estimators();
break;
}
send_controls(chan);
}
void SoloGimbal::send_controls(mavlink_channel_t chan)
{
if (_state == GIMBAL_STATE_PRESENT_RUNNING) {
// get the gimbal quaternion estimate
Quaternion quatEst;
_ekf.getQuat(quatEst);
// run rate controller
_ang_vel_dem_rads.zero();
switch(get_mode()) {
case GIMBAL_MODE_POS_HOLD_FF: {
_ang_vel_dem_rads += get_ang_vel_dem_body_lock();
_ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length();
if (_ang_vel_dem_radsLen > radians(400)) {
_ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen;
}
mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
_ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
break;
}
case GIMBAL_MODE_STABILIZE: {
_ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
_ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
float ang_vel_dem_norm = _ang_vel_dem_rads.length();
if (ang_vel_dem_norm > radians(400)) {
_ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm;
}
mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
_ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
break;
}
default:
case GIMBAL_MODE_IDLE:
case GIMBAL_MODE_POS_HOLD:
break;
}
}
// set GMB_POS_HOLD
if (get_mode() == GIMBAL_MODE_POS_HOLD) {
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
} else {
_gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
}
// set GMB_MAX_TORQUE
float max_torque;
_gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
if (max_torque != _max_torque && max_torque != 0) {
_max_torque = max_torque;
}
if (!hal.util->get_soft_armed() || joints_near_limits()) {
_gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
} else {
_gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
}
}
void SoloGimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg)
{
_measurement.delta_time = report_msg.delta_time;
_measurement.delta_angles.x = report_msg.delta_angle_x;
_measurement.delta_angles.y = report_msg.delta_angle_y;
_measurement.delta_angles.z = report_msg.delta_angle_z;
_measurement.delta_velocity.x = report_msg.delta_velocity_x,
_measurement.delta_velocity.y = report_msg.delta_velocity_y;
_measurement.delta_velocity.z = report_msg.delta_velocity_z;
_measurement.joint_angles.x = report_msg.joint_roll;
_measurement.joint_angles.y = report_msg.joint_el;
_measurement.joint_angles.z = report_msg.joint_az;
if (_calibrator.get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
_calibrator.new_sample(_measurement.delta_velocity,_measurement.delta_time);
}
_measurement.delta_angles -= _gimbalParams.get_gyro_bias() * _measurement.delta_time;
_measurement.joint_angles -= _gimbalParams.get_joint_bias();
_measurement.delta_velocity -= _gimbalParams.get_accel_bias() * _measurement.delta_time;
Vector3f accel_gain = _gimbalParams.get_accel_gain();
_measurement.delta_velocity.x *= accel_gain.x == 0.0f ? 1.0f : accel_gain.x;
_measurement.delta_velocity.y *= accel_gain.y == 0.0f ? 1.0f : accel_gain.y;
_measurement.delta_velocity.z *= accel_gain.z == 0.0f ? 1.0f : accel_gain.z;
// update _ang_vel_mag_filt, used for accel sample readiness
Vector3f ang_vel = _measurement.delta_angles / _measurement.delta_time;
Vector3f ekf_gyro_bias;
_ekf.getGyroBias(ekf_gyro_bias);
ang_vel -= ekf_gyro_bias;
float alpha = constrain_float(_measurement.delta_time/(_measurement.delta_time+0.5f),0.0f,1.0f);
_ang_vel_mag_filt += (ang_vel.length()-_ang_vel_mag_filt)*alpha;
_ang_vel_mag_filt = MIN(_ang_vel_mag_filt,20.0f);
// get complementary filter inputs
_vehicle_to_gimbal_quat.from_vector312(_measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
// update log deltas
_log_dt += _measurement.delta_time;
_log_del_ang += _measurement.delta_angles;
_log_del_vel += _measurement.delta_velocity;
}
void SoloGimbal::update_estimators()
{
if (_state == GIMBAL_STATE_NOT_PRESENT || _state == GIMBAL_STATE_PRESENT_INITIALIZING) {
return;
}
// Run the gimbal attitude and gyro bias estimator
_ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
update_joint_angle_est();
}
void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
const AP_InertialSensor &ins = _ahrs.get_ins();
if (ins_index < ins.get_gyro_count()) {
if (!ins.get_delta_angle(ins_index,dAng)) {
dAng = ins.get_gyro(ins_index) / ins.get_sample_rate();
}
}
}
void SoloGimbal::update_fast() {
const AP_InertialSensor &ins = _ahrs.get_ins();
if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) {
// dual gyro mode - average first two gyros
Vector3f dAng;
readVehicleDeltaAngle(0, dAng);
_vehicle_delta_angles += dAng*0.5f;
readVehicleDeltaAngle(1, dAng);
_vehicle_delta_angles += dAng*0.5f;
} else {
// single gyro mode - one of the first two gyros are unhealthy or don't exist
// just read primary gyro
Vector3f dAng;
readVehicleDeltaAngle(ins.get_primary_gyro(), dAng);
_vehicle_delta_angles += dAng;
}
}
void SoloGimbal::update_joint_angle_est()
{
static const float tc = 1.0f;
float dt = _measurement.delta_time;
float alpha = constrain_float(dt/(dt+tc),0.0f,1.0f);
Matrix3f Tvg; // vehicle frame to gimbal frame
_vehicle_to_gimbal_quat.inverse().rotation_matrix(Tvg);
Vector3f delta_angle_bias;
_ekf.getGyroBias(delta_angle_bias);
delta_angle_bias *= dt;
Vector3f joint_del_ang;
gimbal_ang_vel_to_joint_rates((_measurement.delta_angles-delta_angle_bias) - Tvg*_vehicle_delta_angles, joint_del_ang);
_filtered_joint_angles += joint_del_ang;
_filtered_joint_angles += (_measurement.joint_angles-_filtered_joint_angles)*alpha;
_vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
_vehicle_delta_angles.zero();
}
Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
{
static const float tc = 0.1f;
static const float yawErrorLimit = radians(5.7f);
float dt = _measurement.delta_time;
float alpha = dt/(dt+tc);
Matrix3f Tve = _ahrs.get_rotation_body_to_ned();
Matrix3f Teg;
quatEst.inverse().rotation_matrix(Teg);
//_vehicle_yaw_rate_ef_filt = _ahrs.get_yaw_rate_earth();
// filter the vehicle yaw rate to remove noise
_vehicle_yaw_rate_ef_filt += (_ahrs.get_yaw_rate_earth() - _vehicle_yaw_rate_ef_filt) * alpha;
float yaw_rate_ff = 0;
// calculate an earth-frame yaw rate feed-forward that prevents gimbal from exceeding the maximum yaw error
if (_vehicle_yaw_rate_ef_filt > _gimbalParams.get_K_rate()*yawErrorLimit) {
yaw_rate_ff = _vehicle_yaw_rate_ef_filt-_gimbalParams.get_K_rate()*yawErrorLimit;
} else if (_vehicle_yaw_rate_ef_filt < -_gimbalParams.get_K_rate()*yawErrorLimit) {
yaw_rate_ff = _vehicle_yaw_rate_ef_filt+_gimbalParams.get_K_rate()*yawErrorLimit;
}
// filter the feed-forward to remove noise
//_yaw_rate_ff_ef_filt += (yaw_rate_ff - _yaw_rate_ff_ef_filt) * alpha;
Vector3f gimbalRateDemVecYaw;
gimbalRateDemVecYaw.z = yaw_rate_ff - _gimbalParams.get_K_rate() * _filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f);
gimbalRateDemVecYaw.z /= constrain_float(Tve.c.z,0.5f,1.0f);
// rotate the rate demand into gimbal frame
gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
return gimbalRateDemVecYaw;
}
Vector3f SoloGimbal::get_ang_vel_dem_tilt(const Quaternion &quatEst)
{
// Calculate the gimbal 321 Euler angle estimates relative to earth frame
Vector3f eulerEst = quatEst.to_vector312();
// Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
Quaternion quatDem;
quatDem.from_vector312( _att_target_euler_rad.x,
_att_target_euler_rad.y,
eulerEst.z);
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;
// Convert to a delta rotation
quatErr.normalize();
Vector3f deltaAngErr;
quatErr.to_axis_angle(deltaAngErr);
// multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.get_K_rate();
return gimbalRateDemVecTilt;
}
Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst)
{
// quaternion demanded at the previous time step
static float lastDem;
// calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
float delta = _att_target_euler_rad.y - lastDem;
lastDem = _att_target_euler_rad.y;
Vector3f gimbalRateDemVecForward;
gimbalRateDemVecForward.y = delta / _measurement.delta_time;
return gimbalRateDemVecForward;
}
Vector3f SoloGimbal::get_ang_vel_dem_gyro_bias()
{
Vector3f gyroBias;
_ekf.getGyroBias(gyroBias);
return gyroBias + _gimbalParams.get_gyro_bias();
}
Vector3f SoloGimbal::get_ang_vel_dem_body_lock()
{
// Define rotation from vehicle to gimbal using a 312 rotation sequence
Matrix3f Tvg;
_vehicle_to_gimbal_quat_filt.inverse().rotation_matrix(Tvg);
// multiply the joint angles by a gain to calculate a rate vector required to keep the joints centred
Vector3f gimbalRateDemVecBodyLock;
gimbalRateDemVecBodyLock = _filtered_joint_angles * -_gimbalParams.get_K_rate();
joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock);
// Add a feedforward term from vehicle gyros
gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro();
return gimbalRateDemVecBodyLock;
}
void SoloGimbal::update_target(Vector3f newTarget)
{
// Low-pass filter
_att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y);
// Update tilt
_att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0));
}
void SoloGimbal::write_logs(DataFlash_Class* dataflash)
{
if (dataflash == NULL) return;
uint32_t tstamp = AP_HAL::millis();
Vector3f eulerEst;
Quaternion quatEst;
_ekf.getQuat(quatEst);
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
struct log_Gimbal1 pkt1 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
time_ms : tstamp,
delta_time : _log_dt,
delta_angles_x : _log_del_ang.x,
delta_angles_y : _log_del_ang.y,
delta_angles_z : _log_del_ang.z,
delta_velocity_x : _log_del_vel.x,
delta_velocity_y : _log_del_vel.y,
delta_velocity_z : _log_del_vel.z,
joint_angles_x : _measurement.joint_angles.x,
joint_angles_y : _measurement.joint_angles.y,
joint_angles_z : _measurement.joint_angles.z
};
dataflash->WriteBlock(&pkt1, sizeof(pkt1));
struct log_Gimbal2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
time_ms : tstamp,
est_sta : (uint8_t) _ekf.getStatus(),
est_x : eulerEst.x,
est_y : eulerEst.y,
est_z : eulerEst.z,
rate_x : _ang_vel_dem_rads.x,
rate_y : _ang_vel_dem_rads.y,
rate_z : _ang_vel_dem_rads.z,
target_x: _att_target_euler_rad.x,
target_y: _att_target_euler_rad.y,
target_z: _att_target_euler_rad.z
};
dataflash->WriteBlock(&pkt2, sizeof(pkt2));
_log_dt = 0;
_log_del_ang.zero();
_log_del_vel.zero();
}
bool SoloGimbal::joints_near_limits()
{
return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135);
}
AccelCalibrator* SoloGimbal::_acal_get_calibrator(uint8_t instance)
{
if(instance==0 && (present() || _calibrator.get_status() == ACCEL_CAL_SUCCESS)) {
return &_calibrator;
} else {
return NULL;
}
}
bool SoloGimbal::_acal_get_ready_to_sample()
{
return _ang_vel_mag_filt < radians(10);
}
bool SoloGimbal::_acal_get_saving()
{
return _gimbalParams.flashing();
}
void SoloGimbal::_acal_save_calibrations()
{
if (_calibrator.get_status() != ACCEL_CAL_SUCCESS) {
return;
}
Vector3f bias;
Vector3f gain;
_calibrator.get_calibration(bias,gain);
_gimbalParams.set_accel_bias(bias);
_gimbalParams.set_accel_gain(gain);
_gimbalParams.flash();
}
void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates)
{
float sin_theta = sinf(_measurement.joint_angles.y);
float cos_theta = cosf(_measurement.joint_angles.y);
float sin_phi = sinf(_measurement.joint_angles.x);
float cos_phi = cosf(_measurement.joint_angles.x);
float sec_phi = 1.0f/cos_phi;
float tan_phi = sin_phi/cos_phi;
joint_rates.x = ang_vel.x*cos_theta+ang_vel.z*sin_theta;
joint_rates.y = ang_vel.x*sin_theta*tan_phi-ang_vel.z*cos_theta*tan_phi+ang_vel.y;
joint_rates.z = sec_phi*(ang_vel.z*cos_theta-ang_vel.x*sin_theta);
}
void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel)
{
float sin_theta = sinf(_measurement.joint_angles.y);
float cos_theta = cosf(_measurement.joint_angles.y);
float sin_phi = sinf(_measurement.joint_angles.x);
float cos_phi = cosf(_measurement.joint_angles.x);
ang_vel.x = cos_theta*joint_rates.x-sin_theta*cos_phi*joint_rates.z;
ang_vel.y = joint_rates.y + sin_phi*joint_rates.z;
ang_vel.z = sin_theta*joint_rates.x+cos_theta*cos_phi*joint_rates.z;
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

160
libraries/AP_Mount/SoloGimbal.h Normal file
View File

@ -0,0 +1,160 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/************************************************************
* SoloGimbal -- library to control a 3 axis rate gimbal. *
* *
* Author: Arthur Benemann, Paul Riseborough; *
* *
************************************************************/
#ifndef __SOLOGIMBAL_H__
#define __SOLOGIMBAL_H__
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include "AP_Mount.h"
#include "SoloGimbalEKF.h"
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_AccelCal/AP_AccelCal.h>
#include "SoloGimbal_Parameters.h"
enum gimbal_state_t {
GIMBAL_STATE_NOT_PRESENT = 0,
GIMBAL_STATE_PRESENT_INITIALIZING,
GIMBAL_STATE_PRESENT_ALIGNING,
GIMBAL_STATE_PRESENT_RUNNING
};
enum gimbal_mode_t {
GIMBAL_MODE_IDLE = 0,
GIMBAL_MODE_POS_HOLD,
GIMBAL_MODE_POS_HOLD_FF,
GIMBAL_MODE_STABILIZE
};
class SoloGimbal : AP_AccelCal_Client
{
public:
//Constructor
SoloGimbal(const AP_AHRS_NavEKF &ahrs) :
_ekf(ahrs),
_ahrs(ahrs),
_state(GIMBAL_STATE_NOT_PRESENT),
_yaw_rate_ff_ef_filt(0.0f),
_vehicle_yaw_rate_ef_filt(0.0f),
_lockedToBody(false),
_vehicle_delta_angles(),
_vehicle_to_gimbal_quat(),
_vehicle_to_gimbal_quat_filt(),
_filtered_joint_angles(),
_max_torque(5000.0f),
_log_dt(0),
_log_del_ang(),
_log_del_vel()
{
AP_AccelCal::register_client(this);
}
void update_target(Vector3f newTarget);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
void update_fast();
bool present();
bool aligned();
void set_lockedToBody(bool val) { _lockedToBody = val; }
void write_logs(DataFlash_Class* dataflash);
float get_log_dt() { return _log_dt; }
void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); }
void fetch_params() { _gimbalParams.fetch_params(); }
void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg) {
_gimbalParams.handle_param_value(dataflash, msg);
}
private:
// private methods
void update_estimators();
void send_controls(mavlink_channel_t chan);
void extract_feedback(const mavlink_gimbal_report_t& report_msg);
void update_joint_angle_est();
Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_tilt(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst);
Vector3f get_ang_vel_dem_gyro_bias();
Vector3f get_ang_vel_dem_body_lock();
void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates);
void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel);
void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng);
void _acal_save_calibrations();
bool _acal_get_ready_to_sample();
bool _acal_get_saving();
AccelCalibrator* _acal_get_calibrator(uint8_t instance);
gimbal_mode_t get_mode();
bool joints_near_limits();
// private member variables
gimbal_state_t _state;
struct {
float delta_time;
Vector3f delta_angles;
Vector3f delta_velocity;
Vector3f joint_angles;
} _measurement;
float _yaw_rate_ff_ef_filt;
float _vehicle_yaw_rate_ef_filt;
static const uint8_t _compid = MAV_COMP_ID_GIMBAL;
// joint angle filter states
Vector3f _vehicle_delta_angles;
Quaternion _vehicle_to_gimbal_quat;
Quaternion _vehicle_to_gimbal_quat_filt;
Vector3f _filtered_joint_angles;
uint32_t _last_report_msg_ms;
float _max_torque;
float _ang_vel_mag_filt;
mavlink_channel_t _chan;
Vector3f _ang_vel_dem_rads; // rad/s
Vector3f _att_target_euler_rad; // desired earth-frame roll, tilt and pan angles in radians
bool _lockedToBody;
SoloGimbalEKF _ekf; // state of small EKF for gimbal
const AP_AHRS_NavEKF &_ahrs; // Main EKF
SoloGimbal_Parameters _gimbalParams;
AccelCalibrator _calibrator;
float _log_dt;
Vector3f _log_del_ang;
Vector3f _log_del_vel;
};
#endif // AP_AHRS_NAVEKF_AVAILABLE
#endif // __AP_MOUNT_H__

196
libraries/AP_NavEKF/AP_SmallEKF.cpp → libraries/AP_Mount/SoloGimbalEKF.cpp
View File

@ -12,8 +12,7 @@
#pragma GCC optimize("O3")
#endif
#include "AP_SmallEKF.h"
#include <AP_AHRS/AP_AHRS.h>
#include "SoloGimbalEKF.h"
#include <AP_Param/AP_Param.h>
#include <AP_Vehicle/AP_Vehicle.h>
@ -23,47 +22,95 @@ extern const AP_HAL::HAL& hal;
// Define tuning parameters
const AP_Param::GroupInfo SmallEKF::var_info[] = {
const AP_Param::GroupInfo SoloGimbalEKF::var_info[] = {
AP_GROUPEND
};
// Hash define constants
#define GYRO_BIAS_LIMIT 0.349066f // maximum allowed gyro bias (rad/sec)
// constructor
SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
SoloGimbalEKF::SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs) :
_ahrs(ahrs),
_main_ekf(ahrs.get_NavEKF_const()),
states(),
state(*reinterpret_cast<struct state_elements *>(&states)),
gSense{},
Cov{},
TiltCorrection(0),
StartTime_ms(0),
FiltInit(false),
lastMagUpdate(0),
dtIMU(0)
state(*reinterpret_cast<struct state_elements *>(&states))
{
AP_Param::setup_object_defaults(this, var_info);
reset();
}
// complete reset
void SoloGimbalEKF::reset()
{
memset(&states,0,sizeof(states));
memset(&gSense,0,sizeof(gSense));
memset(&Cov,0,sizeof(Cov));
TiltCorrection = 0;
StartTime_ms = 0;
FiltInit = false;
lastMagUpdate = 0;
dtIMU = 0;
innovationIncrement = 0;
lastInnovation = 0;
}
// run a 9-state EKF used to calculate orientation
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
{
imuSampleTime_ms = AP_HAL::millis();
dtIMU = delta_time;
// initialise variables and constants
if (!FiltInit) {
StartTime_ms = imuSampleTime_ms;
// Note: the start time is initialised to 0 in the constructor
if (StartTime_ms == 0) {
StartTime_ms = imuSampleTime_ms;
}
// Set data to pre-initialsation defaults
FiltInit = false;
newDataMag = false;
YawAligned = false;
memset(&state, 0, sizeof(state));
state.quat[0] = 1.0f;
bool main_ekf_healthy = false;
nav_filter_status main_ekf_status;
if (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.attitude) {
main_ekf_healthy = true;
}
}
// Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
// Also wait for navigation EKF to be healthy beasue we are using the velocity output data
// This prevents jerky gimbal motion from degrading the EKF initial state estimates
if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
return;
}
Quaternion ned_to_vehicle_quat;
ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
Quaternion vehicle_to_gimbal_quat;
vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);
// calculate initial orientation
state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
const float Sigma_dAngBias = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
FiltInit = true;
hal.console->printf("\nSmallEKF Alignment Started\n");
hal.console->printf("\nSoloGimbalEKF Alignment Started\n");
// Don't run the filter in this timestep becasue we have already used the delta velocity data to set an initial orientation
return;
}
// We are using IMU data and joint angles from the gimbal
@ -85,17 +132,17 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
// predict the covariance
predictCovariance();
// fuse SmallEKF velocity data
fuseVelocity(YawAligned);
// fuse SoloGimbalEKF velocity data
fuseVelocity();
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
// Force it to align if too much time has lapsed
if (((((imuSampleTime_ms - StartTime_ms) > 25000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
//calculate the initial heading using magnetometer, estimated tilt and declination
alignHeading();
YawAligned = true;
hal.console->printf("\nSmallEKF Alignment Completed\n");
hal.console->printf("\nSoloGimbalEKF Alignment Completed\n");
}
// Fuse magnetometer data if we have new measurements and an aligned heading
@ -109,7 +156,7 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
}
// state prediction
void SmallEKF::predictStates()
void SoloGimbalEKF::predictStates()
{
static Vector3f gimDelAngCorrected;
static Vector3f gimDelAngPrev;
@ -138,14 +185,21 @@ void SmallEKF::predictStates()
// sum delta velocities to get velocity
state.velocity += delVelNav;
state.delAngBias.x = constrain_float(state.delAngBias.x, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
state.delAngBias.y = constrain_float(state.delAngBias.y, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
state.delAngBias.z = constrain_float(state.delAngBias.z, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
}
// covariance prediction using optimised algebraic toolbox expressions
// equivalent to P = F*P*transpose(P) + G*imu_errors*transpose(G) +
// gyro_bias_state_noise
void SmallEKF::predictCovariance()
void SoloGimbalEKF::predictCovariance()
{
float delAngBiasVariance = sq(dtIMU*dtIMU*5E-4f);
float delAngBiasVariance = sq(dtIMU*5E-6f);
if (YawAligned && !hal.util->get_soft_armed()) {
delAngBiasVariance *= 4.0f;
}
float daxNoise = sq(dtIMU*0.0087f);
float dayNoise = sq(dtIMU*0.0087f);
@ -540,9 +594,13 @@ void SmallEKF::predictCovariance()
}
// Fuse the SmallEKF velocity estimates - this enables alevel reference to be maintained during constant turns
void SmallEKF::fuseVelocity(bool yawInit)
// Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
void SoloGimbalEKF::fuseVelocity()
{
if (!_ahrs.have_inertial_nav()) {
return;
}
float R_OBS = 0.25f;
float innovation[3];
float varInnov[3];
@ -553,11 +611,18 @@ void SmallEKF::fuseVelocity(bool yawInit)
for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
stateIndex = 3 + obsIndex;
// Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
// Calculate the velocity measurement innovation using the SoloGimbalEKF estimate as the observation
// if heading isn't aligned, use zero velocity (static assumption)
if (yawInit) {
Vector3f measVelNED;
_main_ekf.getVelNED(measVelNED);
if (YawAligned) {
Vector3f measVelNED = Vector3f(0,0,0);
nav_filter_status main_ekf_status;
if (_ahrs.get_filter_status(main_ekf_status)) {
if (main_ekf_status.flags.horiz_vel) {
_ahrs.get_velocity_NED(measVelNED);
}
}
innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
} else {
innovation[obsIndex] = state.velocity[obsIndex];
@ -599,10 +664,10 @@ void SmallEKF::fuseVelocity(bool yawInit)
}
// check for new magnetometer data and update store measurements if available
void SmallEKF::readMagData()
void SoloGimbalEKF::readMagData()
{
if (_ahrs.get_compass() &&
_ahrs.get_compass()->use_for_yaw() &&
if (_ahrs.get_compass() &&
_ahrs.get_compass()->use_for_yaw() &&
_ahrs.get_compass()->last_update_usec() != lastMagUpdate) {
// store time of last measurement update
lastMagUpdate = _ahrs.get_compass()->last_update_usec();
@ -619,7 +684,7 @@ void SmallEKF::readMagData()
}
// Fuse compass measurements from autopilot
void SmallEKF::fuseCompass()
void SoloGimbalEKF::fuseCompass()
{
float q0 = state.quat[0];
float q1 = state.quat[1];
@ -766,11 +831,10 @@ void SmallEKF::fuseCompass()
}
// Perform an initial heading alignment using the magnetic field and assumed declination
void SmallEKF::alignHeading()
void SoloGimbalEKF::alignHeading()
{
// calculate the correction rotation vector in NED frame
Vector3f deltaRotNED;
deltaRotNED.z = -calcMagHeadingInnov();
Vector3f deltaRotNED = Vector3f(0,0,-calcMagHeadingInnov());
// rotate into sensor frame
Vector3f angleCorrection = Tsn.transposed()*deltaRotNED;
@ -785,7 +849,7 @@ void SmallEKF::alignHeading()
// Calculate magnetic heading innovation
float SmallEKF::calcMagHeadingInnov()
float SoloGimbalEKF::calcMagHeadingInnov()
{
// Define rotation from magnetometer to sensor using a 312 rotation sequence
Matrix3f Tms;
@ -800,18 +864,19 @@ float SmallEKF::calcMagHeadingInnov()
Tms[2][2] = cosTheta*cosPhi;
// get earth magnetic field estimate from main ekf if available to take advantage of main ekf magnetic field learning
Vector3f body_magfield, earth_magfield;
Vector3f earth_magfield = Vector3f(0,0,0);
_ahrs.get_mag_field_NED(earth_magfield);
float declination;
if (_main_ekf.healthy()) {
_main_ekf.getMagNED(earth_magfield);
_main_ekf.getMagXYZ(body_magfield);
if (!earth_magfield.is_zero()) {
declination = atan2f(earth_magfield.y,earth_magfield.x);
} else {
body_magfield.zero();
earth_magfield.zero();
declination = _ahrs.get_compass()->get_declination();
}
Vector3f body_magfield = Vector3f(0,0,0);
_ahrs.get_mag_field_correction(body_magfield);
// Define rotation from magnetometer to NED axes
Matrix3f Tmn = Tsn*Tms;
@ -822,17 +887,27 @@ float SmallEKF::calcMagHeadingInnov()
float innovation = atan2f(magMeasNED.y,magMeasNED.x) - declination;
// wrap the innovation so it sits on the range from +-pi
if (innovation > 3.1415927f) {
innovation = innovation - 6.2831853f;
} else if (innovation < -3.1415927f) {
innovation = innovation + 6.2831853f;
if (innovation > M_PI_F) {
innovation = innovation - 2*M_PI_F;
} else if (innovation < -M_PI_F) {
innovation = innovation + 2*M_PI_F;
}
return innovation;
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
if (innovation - lastInnovation > M_PI_F) {
// Angle has wrapped in the positive direction to subtract an additional 2*Pi
innovationIncrement -= 2*M_PI_F;
} else if (innovation -innovationIncrement < -M_PI_F) {
// Angle has wrapped in the negative direction so add an additional 2*Pi
innovationIncrement += 2*M_PI_F;
}
lastInnovation = innovation;
return innovation + innovationIncrement;
}
// Force symmmetry and non-negative diagonals on state covarinace matrix
void SmallEKF::fixCovariance()
void SoloGimbalEKF::fixCovariance()
{
// force symmetry
for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
@ -851,7 +926,7 @@ void SmallEKF::fixCovariance()
}
// return data for debugging EKF
void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
void SoloGimbalEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
{
tilt = TiltCorrection;
velocity = state.velocity;
@ -864,7 +939,7 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector
}
// get gyro bias data
void SmallEKF::getGyroBias(Vector3f &gyroBias) const
void SoloGimbalEKF::getGyroBias(Vector3f &gyroBias) const
{
if (dtIMU < 1.0e-6f) {
gyroBias.zero();
@ -873,17 +948,26 @@ void SmallEKF::getGyroBias(Vector3f &gyroBias) const
}
}
void SoloGimbalEKF::setGyroBias(const Vector3f &gyroBias)
{
if (dtIMU < 1.0e-6f) {
return;
}
state.delAngBias = gyroBias * dtIMU;
}
// get quaternion data
void SmallEKF::getQuat(Quaternion &quat) const
void SoloGimbalEKF::getQuat(Quaternion &quat) const
{
quat = state.quat;
}
// get filter status - true is aligned
bool SmallEKF::getStatus() const
bool SoloGimbalEKF::getStatus() const
{
float run_time = AP_HAL::millis() - StartTime_ms;
return YawAligned && (run_time > 30000);
return YawAligned && (run_time > 15000);
}
#endif // HAL_CPU_CLASS

26
libraries/AP_NavEKF/AP_SmallEKF.h → libraries/AP_Mount/SoloGimbalEKF.h
View File

@ -18,8 +18,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef AP_SmallEKF
#define AP_SmallEKF
#ifndef _SOLO_GIMBAL_EKF_
#define _SOLO_GIMBAL_EKF_
#include <AP_Math/AP_Math.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
@ -27,13 +27,13 @@
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Param/AP_Param.h>
#include "AP_Nav_Common.h"
#include <AP_NavEKF/AP_Nav_Common.h>
#include <AP_AHRS/AP_AHRS.h>
#include "AP_NavEKF.h"
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_Math/vectorN.h>
class SmallEKF
class SoloGimbalEKF
{
public:
typedef float ftype;
@ -78,17 +78,22 @@ public:
#endif
// Constructor
SmallEKF(const AP_AHRS_NavEKF &ahrs);
SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs);
// Run the EKF main loop once every time we receive sensor data
void RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles);
void reset();
// get some debug information
void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const;
// get gyro bias data
void getGyroBias(Vector3f &gyroBias) const;
// set gyro bias
void setGyroBias(const Vector3f &gyroBias);
// get quaternion data
void getQuat(Quaternion &quat) const;
@ -99,7 +104,6 @@ public:
private:
const AP_AHRS_NavEKF &_ahrs;
const NavEKF &_main_ekf;
// the states are available in two forms, either as a Vector13 or
// broken down as individual elements. Both are equivalent (same
@ -140,6 +144,10 @@ private:
uint32_t imuSampleTime_ms;
float dtIMU;
// States used for unwrapping of compass yaw error
float innovationIncrement;
float lastInnovation;
// state prediction
void predictStates();
@ -147,7 +155,7 @@ private:
void predictCovariance();
// EKF velocity fusion
void fuseVelocity(bool yawInit);
void fuseVelocity();
// read magnetometer data
void readMagData();
@ -165,4 +173,4 @@ private:
void fixCovariance();
};
#endif // AP_SmallEKF
#endif // _SOLO_GIMBAL_EKF_

278
libraries/AP_Mount/SoloGimbal_Parameters.cpp Normal file
View File

@ -0,0 +1,278 @@
#include "SoloGimbal_Parameters.h"
#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal;
const uint32_t SoloGimbal_Parameters::_retry_period = 3000;
const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5;
SoloGimbal_Parameters::SoloGimbal_Parameters()
{
reset();
}
void SoloGimbal_Parameters::reset()
{
memset(_params,0,sizeof(_params));
_last_request_ms = 0;
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param)
{
switch(param) {
case GMB_PARAM_GMB_OFF_ACC_X:
return "GMB_OFF_ACC_X";
case GMB_PARAM_GMB_OFF_ACC_Y:
return "GMB_OFF_ACC_Y";
case GMB_PARAM_GMB_OFF_ACC_Z:
return "GMB_OFF_ACC_Z";
case GMB_PARAM_GMB_GN_ACC_X:
return "GMB_GN_ACC_X";
case GMB_PARAM_GMB_GN_ACC_Y:
return "GMB_GN_ACC_Y";
case GMB_PARAM_GMB_GN_ACC_Z:
return "GMB_GN_ACC_Z";
case GMB_PARAM_GMB_OFF_GYRO_X:
return "GMB_OFF_GYRO_X";
case GMB_PARAM_GMB_OFF_GYRO_Y:
return "GMB_OFF_GYRO_Y";
case GMB_PARAM_GMB_OFF_GYRO_Z:
return "GMB_OFF_GYRO_Z";
case GMB_PARAM_GMB_OFF_JNT_X:
return "GMB_OFF_JNT_X";
case GMB_PARAM_GMB_OFF_JNT_Y:
return "GMB_OFF_JNT_Y";
case GMB_PARAM_GMB_OFF_JNT_Z:
return "GMB_OFF_JNT_Z";
case GMB_PARAM_GMB_K_RATE:
return "GMB_K_RATE";
case GMB_PARAM_GMB_POS_HOLD:
return "GMB_POS_HOLD";
case GMB_PARAM_GMB_MAX_TORQUE:
return "GMB_MAX_TORQUE";
case GMB_PARAM_GMB_SND_TORQUE:
return "GMB_SND_TORQUE";
case GMB_PARAM_GMB_SYSID:
return "GMB_SYSID";
case GMB_PARAM_GMB_FLASH:
return "GMB_FLASH";
default:
return "";
};
}
void SoloGimbal_Parameters::fetch_params()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) {
_params[i].state = GMB_PARAMSTATE_FETCH_AGAIN;
}
}
}
bool SoloGimbal_Parameters::initialized()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) {
return false;
}
}
return true;
}
bool SoloGimbal_Parameters::received_all()
{
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) {
return false;
}
}
return true;
}
void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) {
if (!_params[param].seen) {
value = def_val;
} else {
value = _params[param].value;
}
}
void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) {
if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH && _params[param].value == value) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT) {
return;
}
_params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
_params[param].value = value;
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name(param), _params[param].value, MAV_PARAM_TYPE_REAL32);
_last_request_ms = AP_HAL::millis();
}
void SoloGimbal_Parameters::update()
{
uint32_t tnow_ms = AP_HAL::millis();
// retry initial param retrieval
if(!received_all()){
if (tnow_ms-_last_request_ms > _retry_period) {
_last_request_ms = tnow_ms;
mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
}
// retry param_set
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET && tnow_ms - _last_request_ms > _retry_period) {
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
if (!_params[i].seen) {
_params[i].fetch_attempts++;
}
}
}
// check for nonexistant parameters
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
_params[i].state = GMB_PARAMSTATE_NONEXISTANT;
hal.console->printf("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
}
}
if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
bool done = true;
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
done = false;
break;
}
}
if (done) {
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
set_param(GMB_PARAM_GMB_FLASH,69.0f);
}
}
}
void SoloGimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg)
{
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
if (dataflash != NULL) {
dataflash->Log_Write_Parameter(packet.param_id, packet.param_value);
}
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
_params[i].seen = true;
switch(_params[i].state) {
case GMB_PARAMSTATE_NONEXISTANT:
case GMB_PARAMSTATE_NOT_YET_READ:
case GMB_PARAMSTATE_FETCH_AGAIN:
_params[i].value = packet.param_value;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
break;
case GMB_PARAMSTATE_CONSISTENT:
_params[i].value = packet.param_value;
break;
case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
if (i == GMB_PARAM_GMB_FLASH) {
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && packet.param_value == 1) {
_flashing_step = GMB_PARAM_NOT_FLASHING;
}
_params[i].value = 0;
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
} else if (packet.param_value == _params[i].value) {
_params[i].state = GMB_PARAMSTATE_CONSISTENT;
}
break;
}
break;
}
}
}
Vector3f SoloGimbal_Parameters::get_accel_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z);
return ret;
}
Vector3f SoloGimbal_Parameters::get_accel_gain()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f);
get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f);
get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f);
return ret;
}
void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias)
{
set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x);
set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y);
set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z);
}
void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain)
{
set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x);
set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y);
set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z);
}
Vector3f SoloGimbal_Parameters::get_gyro_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z);
return ret;
}
void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias)
{
set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x);
set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y);
set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z);
}
Vector3f SoloGimbal_Parameters::get_joint_bias()
{
Vector3f ret;
get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x);
get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y);
get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z);
return ret;
}
float SoloGimbal_Parameters::get_K_rate()
{
float ret;
get_param(GMB_PARAM_GMB_K_RATE,ret);
return ret;
}
void SoloGimbal_Parameters::flash()
{
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET;
}
bool SoloGimbal_Parameters::flashing()
{
return _flashing_step != GMB_PARAM_NOT_FLASHING;
}

94
libraries/AP_Mount/SoloGimbal_Parameters.h Normal file
View File

@ -0,0 +1,94 @@
#ifndef __SOLOGIMBAL_PARAMETERS__
#define __SOLOGIMBAL_PARAMETERS__
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <DataFlash/DataFlash.h>
enum gmb_param_state_t {
GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized
GMB_PARAMSTATE_FETCH_AGAIN=1, // parameter is being fetched
GMB_PARAMSTATE_ATTEMPTING_TO_SET=2, // parameter is being set
GMB_PARAMSTATE_CONSISTENT=3, // parameter is consistent
GMB_PARAMSTATE_NONEXISTANT=4 // parameter does not seem to exist
};
enum gmb_param_t {
GMB_PARAM_GMB_OFF_ACC_X=0,
GMB_PARAM_GMB_OFF_ACC_Y,
GMB_PARAM_GMB_OFF_ACC_Z,
GMB_PARAM_GMB_GN_ACC_X,
GMB_PARAM_GMB_GN_ACC_Y,
GMB_PARAM_GMB_GN_ACC_Z,
GMB_PARAM_GMB_OFF_GYRO_X,
GMB_PARAM_GMB_OFF_GYRO_Y,
GMB_PARAM_GMB_OFF_GYRO_Z,
GMB_PARAM_GMB_OFF_JNT_X,
GMB_PARAM_GMB_OFF_JNT_Y,
GMB_PARAM_GMB_OFF_JNT_Z,
GMB_PARAM_GMB_K_RATE,
GMB_PARAM_GMB_POS_HOLD,
GMB_PARAM_GMB_MAX_TORQUE,
GMB_PARAM_GMB_SND_TORQUE,
GMB_PARAM_GMB_SYSID,
GMB_PARAM_GMB_FLASH,
MAVLINK_GIMBAL_NUM_TRACKED_PARAMS
};
enum gmb_flashing_step_t {
GMB_PARAM_NOT_FLASHING=0,
GMB_PARAM_FLASHING_WAITING_FOR_SET,
GMB_PARAM_FLASHING_WAITING_FOR_ACK
};
class SoloGimbal_Parameters
{
public:
SoloGimbal_Parameters();
void reset();
bool initialized();
bool received_all();
void fetch_params();
void get_param(gmb_param_t param, float& value, float def_val = 0.0f);
void set_param(gmb_param_t param, float value);
void update();
void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg);
Vector3f get_accel_bias();
Vector3f get_accel_gain();
void set_accel_bias(const Vector3f& bias);
void set_accel_gain(const Vector3f& gain);
Vector3f get_gyro_bias();
void set_gyro_bias(const Vector3f& bias);
Vector3f get_joint_bias();
float get_K_rate();
void flash();
bool flashing();
void set_channel(mavlink_channel_t chan) { _chan = chan; }
private:
static const char* get_param_name(gmb_param_t param);
static const uint32_t _retry_period;
static const uint8_t _max_fetch_attempts;
struct {
float value;
gmb_param_state_t state;
uint8_t fetch_attempts;
bool seen;
} _params[MAVLINK_GIMBAL_NUM_TRACKED_PARAMS];
uint32_t _last_request_ms;
gmb_flashing_step_t _flashing_step;
mavlink_channel_t _chan;
};
#endif // __SOLOGIMBAL_PARAMETERS__

5
libraries/AP_Mount/examples/trivial_AP_Mount/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

5
libraries/AP_Notify/examples/AP_Notify_test/wscript
View File

@ -1,10 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
import ardupilotwaf
def build(bld):
ardupilotwaf.example(
bld,
bld.ap_example(
use='ap',
)

Some files were not shown because too many files have changed in this diff Show More