Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: added initial ekf estimation of gimbal bias

Browse Source 
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2015-01-30 17:39:31 +11:00
parent ee9c778834
commit da27a8696d
4 changed files with 61 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Mount/AP_Mount.cpp
View File

@ -385,7 +385,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
AP_GROUPEND
};
AP_Mount::AP_Mount(const AP_AHRS &ahrs, const struct Location &current_loc) :
AP_Mount::AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location &current_loc) :
_ahrs(ahrs),
_current_loc(current_loc),
_num_instances(0),

14
libraries/AP_Mount/AP_Mount.h
View File

@ -39,6 +39,16 @@ class AP_Mount_Servo;
class AP_Mount_MAVLink;
class AP_Mount_Alexmos;
/*
This is a workaround to allow the MAVLink backend access to the
SmallEKF. It would be nice to find a neater solution to this
*/
#if AP_AHRS_NAVEKF_AVAILABLE
#define AP_AHRS_MOUNT AP_AHRS_NavEKF
#else
#define AP_AHRS_MOUNT AP_AHRS
#endif
class AP_Mount
{
// declare backends as friends
@ -58,7 +68,7 @@ public:
};
// Constructor
AP_Mount(const AP_AHRS &ahrs, const struct Location &current_loc);
AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location &current_loc);
// init - detect and initialise all mounts
void init(const AP_SerialManager& serial_manager);
@ -115,7 +125,7 @@ public:
protected:
// private members
const AP_AHRS &_ahrs;
const AP_AHRS_MOUNT &_ahrs;
const struct Location &_current_loc; // reference to the vehicle's current location
// frontend parameters

39
libraries/AP_Mount/AP_Mount_MAVLink.cpp
View File

@ -1,8 +1,23 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Mount_MAVLink.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include <GCS_MAVLink.h>
#define MOUNT_DEBUG 0
#if MOUNT_DEBUG
#include <stdio.h>
#endif
AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance),
_initialised(false),
_chan(MAVLINK_COMM_0),
_last_mode(MAV_MOUNT_MODE_RETRACT),
_ekf(frontend._ahrs)
{}
// init - performs any required initialisation for this instance
void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager)
{
@ -150,6 +165,17 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
{
// just save it for future processing and reporting to GCS for now
mavlink_msg_gimbal_report_decode(msg, &_gimbal_report);
Vector3f delta_angles(_gimbal_report.delta_angle_x,
_gimbal_report.delta_angle_y,
_gimbal_report.delta_angle_z);
Vector3f delta_velocity(_gimbal_report.delta_velocity_x,
_gimbal_report.delta_velocity_y,
_gimbal_report.delta_velocity_z);
Vector3f joint_angles(_gimbal_report.joint_roll,
_gimbal_report.joint_pitch,
_gimbal_report.joint_yaw);
_ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
}
/*
@ -159,7 +185,7 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
{
mavlink_msg_gimbal_report_send(chan,
0, 0, // send as broadcast
_gimbal_report.counter,
_gimbal_report.delta_time,
_gimbal_report.delta_angle_x,
_gimbal_report.delta_angle_y,
_gimbal_report.delta_angle_z,
@ -169,4 +195,15 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
_gimbal_report.joint_roll,
_gimbal_report.joint_pitch,
_gimbal_report.joint_yaw);
float tilt;
Vector3f velocity, euler, gyroBias;
_ekf.getDebug(tilt, velocity, euler, gyroBias);
#if MOUNT_DEBUG
::printf("tilt=%.2f euler(%.2f, %.2f, %.2f) bias=(%.2f, %.2f %.2f)\n",
tilt,
degrees(euler.x), degrees(euler.y), degrees(euler.z),
degrees(gyroBias.x), degrees(gyroBias.y), degrees(gyroBias.z));
#endif
}
#endif // AP_AHRS_NAVEKF_AVAILABLE

17
libraries/AP_Mount/AP_Mount_MAVLink.h
View File

@ -7,13 +7,17 @@
#ifndef __AP_MOUNT_MAVLINK_H__
#define __AP_MOUNT_MAVLINK_H__
#include <AP_HAL.h>
#include <AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <GCS_MAVLink.h>
#include <RC_Channel.h>
#include <AP_Mount_Backend.h>
#include <AP_SmallEKF.h>
#define AP_MOUNT_MAVLINK_COMPID 10 // Use hard-coded component id to communicate with gimbal, sysid is taken from globally defined mavlink_system.sysid
@ -22,12 +26,7 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
public:
// Constructor
AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance),
_initialised(false),
_chan(MAVLINK_COMM_0),
_last_mode(MAV_MOUNT_MODE_RETRACT)
{}
AP_Mount_MAVLink(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);
@ -61,8 +60,12 @@ private:
enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
Vector3f _last_angle_target; // last angle target sent to mount
// state of small EKF for gimbal
SmallEKF _ekf;
// keep last gimbal report
mavlink_gimbal_report_t _gimbal_report;
};
#endif // AP_AHRS_NAVEKF_AVAILABLE
#endif // __AP_MOUNT_MAVLINK_H__