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

Copter: integrate AP_VisualOdom

Browse Source
This commit is contained in:
Randy Mackay 2017-03-01 20:18:11 +09:00
parent d5dd7e719a
commit 713c08672f
9 changed files with 96 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/APM_Config.h
View File

@ -18,6 +18,7 @@
//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
//#define VISUAL_ODOMETRY_ENABLED DISABLED // disable visual odometry to save 2K of flash space
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
//#define ADSB_ENABLED DISABLED // disable ADSB support
//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor

1
ArduCopter/ArduCopter.cpp
View File

@ -96,6 +96,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(read_rangefinder, 20, 100),
SCHED_TASK(update_proximity, 100, 50),
SCHED_TASK(update_beacon, 400, 50),
SCHED_TASK(update_visual_odom, 400, 50),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
SCHED_TASK(update_throttle_hover,100, 90),

8
ArduCopter/Copter.h
View File

@ -91,6 +91,7 @@
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
#include <AP_Button/AP_Button.h>
#include <AP_Arming/AP_Arming.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
// Configuration
#include "defines.h"
@ -588,6 +589,11 @@ private:
// last valid RC input time
uint32_t last_radio_update_ms;
#if VISUAL_ODOMETRY_ENABLED == ENABLED
// last visual odometry update time
uint32_t visual_odom_last_update_ms;
#endif
// Top-level logic
// setup the var_info table
AP_Param param_loader;
@ -692,6 +698,8 @@ private:
void stats_update();
void init_beacon();
void update_beacon();
void init_visual_odom();
void update_visual_odom();
void send_pid_tuning(mavlink_channel_t chan);
void gcs_send_message(enum ap_message id);
void gcs_send_mission_item_reached_message(uint16_t mission_index);

6
ArduCopter/GCS_Mavlink.cpp
View File

@ -2020,6 +2020,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
#endif
break;
case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
#if VISUAL_ODOMETRY_ENABLED == ENABLED
copter.g2.visual_odom.handle_msg(msg);
#endif
break;
default:
handle_common_message(msg);
break;

6
ArduCopter/Parameters.cpp
View File

@ -998,6 +998,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/RC_Channel/RC_Channels.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels),
#if VISUAL_ODOMETRY_ENABLED == ENABLED
// @Group: VISO
// @Path: ../libraries/AP_VisualOdom/AP_VisualOdom.cpp
AP_SUBGROUPINFO(visual_odom, "VISO", 18, ParametersG2, AP_VisualOdom),
#endif
AP_GROUPEND
};

5
ArduCopter/Parameters.h
View File

@ -536,6 +536,11 @@ public:
// beacon (non-GPS positioning) library
AP_Beacon beacon;
#if VISUAL_ODOMETRY_ENABLED == ENABLED
// Visual Odometry camera
AP_VisualOdom visual_odom;
#endif
#if PROXIMITY_ENABLED == ENABLED
// proximity (aka object avoidance) library
AP_Proximity proximity;

5
ArduCopter/config.h
View File

@ -215,10 +215,13 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// OPTICAL_FLOW
// OPTICAL_FLOW & VISUAL ODOMETRY
#ifndef OPTFLOW
# define OPTFLOW ENABLED
#endif
#ifndef VISUAL_ODOMETRY_ENABLED
# define VISUAL_ODOMETRY_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Auto Tuning

43
ArduCopter/sensors.cpp
View File

@ -297,6 +297,11 @@ void Copter::update_sensor_status_flags(void)
if (precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
if (g2.visual_odom.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
if (ap.rc_receiver_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
@ -382,7 +387,12 @@ void Copter::update_sensor_status_flags(void)
}
#endif
#if PRECISION_LANDING == ENABLED
if (!precland.healthy()) {
if (precland.enabled() && !precland.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
if (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
@ -461,3 +471,34 @@ void Copter::update_beacon()
{
g2.beacon.update();
}
// init visual odometry sensor
void Copter::init_visual_odom()
{
#if VISUAL_ODOMETRY_ENABLED == ENABLED
g2.visual_odom.init();
#endif
}
// update visual odometry sensor
void Copter::update_visual_odom()
{
#if VISUAL_ODOMETRY_ENABLED == ENABLED
// check for updates
if (g2.visual_odom.enabled() && (g2.visual_odom.get_last_update_ms() != visual_odom_last_update_ms)) {
visual_odom_last_update_ms = g2.visual_odom.get_last_update_ms();
float time_delta_sec = g2.visual_odom.get_time_delta_usec() / 1000000.0f;
ahrs.writeBodyFrameOdom(g2.visual_odom.get_confidence(),
g2.visual_odom.get_position_delta(),
g2.visual_odom.get_angle_delta(),
time_delta_sec,
visual_odom_last_update_ms,
g2.visual_odom.get_pos_offset());
// log sensor data
DataFlash.Log_Write_VisualOdom(time_delta_sec,
g2.visual_odom.get_angle_delta(),
g2.visual_odom.get_position_delta(),
g2.visual_odom.get_confidence());
}
#endif
}

25
ArduCopter/system.cpp
View File

@ -279,6 +279,9 @@ void Copter::init_ardupilot()
// init beacons used for non-gps position estimation
init_beacon();
// init visual odometry
init_visual_odom();
// initialise AP_RPM library
rpm_sensor.init();
@ -386,11 +389,27 @@ bool Copter::ekf_position_ok()
// optflow_position_ok - returns true if optical flow based position estimate is ok
bool Copter::optflow_position_ok()
{
#if OPTFLOW != ENABLED
#if OPTFLOW != ENABLED && VISUAL_ODOMETRY_ENABLED != ENABLED
return false;
#else
// return immediately if optflow is not enabled or EKF not used
if (!optflow.enabled() || !ahrs.have_inertial_nav()) {
// return immediately if EKF not used
if (!ahrs.have_inertial_nav()) {
return false;
}
// return immediately if neither optflow nor visual odometry is enabled
bool enabled = false;
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
enabled = true;
}
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
if (g2.visual_odom.enabled()) {
enabled = true;
}
#endif
if (!enabled) {
return false;
}