Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Update Sub to match relevant Copter and Library developments

This commit is contained in:
Jacob Walser 2016-11-17 00:12:06 -05:00 committed by Andrew Tridgell
parent 4704faf60d
commit 456f5e2e93
16 changed files with 117 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/APM_Config.h
View File

@ -21,15 +21,15 @@
//#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define CAMERA DISABLED // disable camera trigger to save 1k of flash
//#define RANGEFINDER_ENABLED DISABLED // disable rangefinder to save 1k of flash
//#define PROXIMITY_ENABLED DISABLED // disable proximity sensors
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
//#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
#define AC_TERRAIN DISABLED // disable terrain library
#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define EPM_ENABLED DISABLED // disable epm cargo gripper to save 500bytes of flash
#define GRIPPER_ENABLED DISABLED // disable gripper to save 500bytes of flash
//#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 FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
#define PRECISION_LANDING DISABLED // enable precision landing using companion computer or IRLock sensor
#define TRANSECT_ENABLED DISABLED

7
ArduSub/ArduSub.cpp
View File

@ -129,12 +129,9 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(rpm_update, 10, 200),
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100),
#if FRSKY_TELEM_ENABLED == ENABLED
SCHED_TASK(frsky_telemetry_send, 5, 75),
#endif
SCHED_TASK(terrain_update, 10, 100),
#if EPM_ENABLED == ENABLED
SCHED_TASK(epm_update, 10, 75),
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK(gripper_update, 10, 75),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),

20
ArduSub/GCS_Mavlink.cpp
View File

@ -338,7 +338,7 @@ void NOINLINE Sub::send_hwstatus(mavlink_channel_t chan)
mavlink_msg_hwstatus_send(
chan,
hal.analogin->board_voltage()*1000,
hal.i2c->lockup_count());
0);
}
void NOINLINE Sub::send_servo_out(mavlink_channel_t chan)
@ -392,7 +392,15 @@ void NOINLINE Sub::send_radio_out(mavlink_channel_t chan)
hal.rcout->read(4),
hal.rcout->read(5),
hal.rcout->read(6),
hal.rcout->read(7));
hal.rcout->read(7),
hal.rcout->read(8),
hal.rcout->read(9),
hal.rcout->read(10),
hal.rcout->read(11),
hal.rcout->read(12),
hal.rcout->read(13),
hal.rcout->read(14),
hal.rcout->read(15));
}
void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
@ -1587,20 +1595,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
result = sub.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);
break;
#if EPM_ENABLED == ENABLED
#if GRIPPER_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER:
// param1 : gripper number (ignored)
// param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
if(!sub.epm.enabled()) {
if(!sub.g2.gripper.enabled()) {
result = MAV_RESULT_FAILED;
} else {
result = MAV_RESULT_ACCEPTED;
switch ((uint8_t)packet.param2) {
case GRIPPER_ACTION_RELEASE:
sub.epm.release();
sub.g2.gripper.release();
break;
case GRIPPER_ACTION_GRAB:
sub.epm.grab();
sub.g2.gripper.grab();
break;
default:
result = MAV_RESULT_FAILED;

2
ArduSub/Log.cpp
View File

@ -362,7 +362,7 @@ void Sub::Log_Write_Performance()
num_loops : perf_info_get_num_loops(),
max_time : perf_info_get_max_time(),
pm_test : pmTest1,
i2c_lockup_count : hal.i2c->lockup_count(),
i2c_lockup_count : 0,
ins_error_count : ins.error_count(),
log_dropped : DataFlash.num_dropped() - perf_info_get_num_dropped(),
};

39
ArduSub/Parameters.cpp
View File

@ -835,12 +835,6 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/AP_Relay/AP_Relay.cpp
GOBJECT(relay, "RELAY_", AP_Relay),
#if EPM_ENABLED == ENABLED
// @Group: EPM_
// @Path: ../libraries/AP_EPM/AP_EPM.cpp
GOBJECT(epm, "EPM_", AP_EPM),
#endif
#if PARACHUTE == ENABLED
// @Group: CHUTE_
// @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
@ -1072,16 +1066,37 @@ const AP_Param::Info Sub::var_info[] = {
*/
const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: TKOFF_NAV_ALT
// @DisplayName: Takeoff navigation altitude
// @Description: This is the altitude in meters above the takeoff point that attitude changes for navigation can begin
// @Range: 0 5
// @User: Standard
AP_GROUPINFO("WP_TKOFF_NAV_ALT", 1, ParametersG2, takeoff_nav_alt, 0),
// @Param: WP_NAVALT_MIN
// @DisplayName: Minimum navigation altitude
// @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing.
// @Range: 0 5
// @User: Standard
AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0),
#if PROXIMITY_ENABLED == ENABLED
// @Group: PRX
// @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
AP_SUBGROUPINFO(proximity, "PRX", 2, ParametersG2, AP_Proximity),
#endif
#if GRIPPER_ENABLED == ENABLED
// @Group: GRIP_
// @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
AP_SUBGROUPINFO(gripper, "GRIP_", 3, ParametersG2, AP_Gripper),
#endif
AP_GROUPEND
};
/*
constructor for g2 object
*/
ParametersG2::ParametersG2(void)
: proximity(sub.serial_manager)
{
AP_Param::setup_object_defaults(this, var_info);
}
/*
This is a conversion table from old parameter values to new
parameter names. The startup code looks for saved values of the old

14
ArduSub/Parameters.h
View File

@ -613,13 +613,23 @@ public:
*/
class ParametersG2 {
public:
ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); }
ParametersG2(void);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
// altitude at which nav control can start in takeoff
AP_Float takeoff_nav_alt;
AP_Float wp_navalt_min;
#if GRIPPER_ENABLED
AP_Gripper gripper;
#endif
#if PROXIMITY_ENABLED == ENABLED
// proximity (aka object avoidance) library
AP_Proximity proximity;
#endif
};
extern const AP_Param::Info var_info[];

5
ArduSub/Sub.cpp
View File

@ -48,9 +48,6 @@ Sub::Sub(void) :
desired_climb_rate(0),
loiter_time_max(0),
loiter_time(0),
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry(ahrs, battery),
#endif
climb_rate(0),
target_rangefinder_alt(0.0f),
baro_alt(0),
@ -70,7 +67,7 @@ Sub::Sub(void) :
pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
g.p_pos_xy, g.pi_vel_xy),
avoid(ahrs, inertial_nav, fence),
avoid(ahrs, inertial_nav, fence, g2.proximity),
wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
circle_nav(inertial_nav, ahrs, pos_control),
pmTest1(0),

22
ArduSub/Sub.h
View File

@ -64,6 +64,7 @@
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <AP_Proximity/AP_Proximity.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
#include <Filter/Filter.h> // Filter library
@ -85,7 +86,6 @@
#include <AP_Notify/AP_Notify.h> // Notify library
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
#include <AP_BoardConfig/AP_BoardConfig.h> // board configuration library
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
#include <AP_Terrain/AP_Terrain.h>
#include <AP_ADSB/AP_ADSB.h>
@ -102,8 +102,8 @@
#if SPRAYER == ENABLED
#include <AC_Sprayer/AC_Sprayer.h> // crop sprayer library
#endif
#if EPM_ENABLED == ENABLED
#include <AP_EPM/AP_EPM.h> // EPM cargo gripper stuff
#if GRIPPER_ENABLED == ENABLED
#include <AP_Gripper/AP_Gripper.h> // gripper stuff
#endif
#if PARACHUTE == ENABLED
#include <AP_Parachute/AP_Parachute.h> // Parachute release library
@ -124,6 +124,7 @@ class Sub : public AP_HAL::HAL::Callbacks {
public:
friend class GCS_MAVLINK_Sub;
friend class Parameters;
friend class ParametersG2;
Sub(void);
@ -413,11 +414,6 @@ private:
// Battery Sensors
AP_BattMonitor battery;
// FrSky telemetry support
#if FRSKY_TELEM_ENABLED == ENABLED
AP_Frsky_Telem frsky_telemetry;
#endif
// Altitude
// The cm/s we are moving up or down based on filtered data - Positive = UP
int16_t climb_rate;
@ -529,11 +525,6 @@ private:
AC_Sprayer sprayer;
#endif
// EPM Cargo Griper
#if EPM_ENABLED == ENABLED
AP_EPM epm;
#endif
// Parachute release
#if PARACHUTE == ENABLED
AP_Parachute parachute;
@ -949,7 +940,7 @@ private:
void update_precland();
void read_battery(void);
void read_receiver_rssi(void);
void epm_update();
void gripper_update();
void terrain_update();
void terrain_logging();
bool terrain_use();
@ -987,7 +978,6 @@ private:
bool optflow_position_ok();
void update_auto_armed();
void check_usb_mux(void);
void frsky_telemetry_send(void);
bool should_log(uint32_t mask);
bool current_mode_has_user_takeoff(bool must_navigate);
bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
@ -1028,7 +1018,7 @@ private:
#if PARACHUTE == ENABLED
void do_parachute(const AP_Mission::Mission_Command& cmd);
#endif
#if EPM_ENABLED == ENABLED
#if GRIPPER_ENABLED == ENABLED
void do_gripper(const AP_Mission::Mission_Command& cmd);
#endif
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);

36
ArduSub/commands_logic.cpp
View File

@ -134,8 +134,8 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
break;
#endif
#if EPM_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control EPM gripper
#if GRIPPER_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER: // Mission command to control gripper
do_gripper(cmd);
break;
#endif
@ -231,11 +231,27 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_CONDITION_YAW:
return verify_yaw();
case MAV_CMD_DO_PARACHUTE:
// assume parachute was released successfully
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_DO_PARACHUTE: // assume parachute was released successfully
case MAV_CMD_DO_GRIPPER:
case MAV_CMD_DO_GUIDED_LIMITS:
return true;
default:
// error message
gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
// return true if we do not recognize the command so that we move on to the next command
return true;
}
@ -554,19 +570,19 @@ void Sub::do_parachute(const AP_Mission::Mission_Command& cmd)
}
#endif
#if EPM_ENABLED == ENABLED
// do_gripper - control EPM gripper
#if GRIPPER_ENABLED == ENABLED
// do_gripper - control gripper
void Sub::do_gripper(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.gripper.action) {
case GRIPPER_ACTION_RELEASE:
epm.release();
Log_Write_Event(DATA_EPM_RELEASE);
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case GRIPPER_ACTION_GRAB:
epm.grab();
Log_Write_Event(DATA_EPM_GRAB);
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
default:
// do nothing

7
ArduSub/defines.h
View File

@ -55,7 +55,7 @@ enum aux_sw_func {
AUXSW_AUTO = 16, // change to auto flight mode
AUXSW_AUTOTUNE = 17, // auto tune
AUXSW_LAND = 18, // change to LAND flight mode
AUXSW_EPM = 19, // Operate the EPM cargo gripper low=off, middle=neutral, high=on
AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on
AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable
AUXSW_PARACHUTE_RELEASE = 22, // Parachute release
AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch
@ -297,6 +297,7 @@ enum ThrowModeState {
#define LOG_HELI_MSG 0x20
#define LOG_PRECLAND_MSG 0x21
#define LOG_GUIDEDTARGET_MSG 0x22
#define LOG_PROXIMITY_MSG 0x24
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
@ -352,8 +353,8 @@ enum ThrowModeState {
#define DATA_ACRO_TRAINER_DISABLED 43
#define DATA_ACRO_TRAINER_LEVELING 44
#define DATA_ACRO_TRAINER_LIMITED 45
#define DATA_EPM_GRAB 46
#define DATA_EPM_RELEASE 47
#define DATA_GRIPPER_GRAB 46
#define DATA_GRIPPER_RELEASE 47
#define DATA_PARACHUTE_DISABLED 49
#define DATA_PARACHUTE_ENABLED 50
#define DATA_PARACHUTE_RELEASED 51

3
ArduSub/make.inc
View File

@ -44,6 +44,7 @@ LIBRARIES += AC_Circle
LIBRARIES += AP_Declination
LIBRARIES += AC_Fence
LIBRARIES += AC_Avoidance
LIBRARIES += AP_Proximity
LIBRARIES += AP_Scheduler
LIBRARIES += AP_RCMapper
LIBRARIES += AP_Notify
@ -51,7 +52,6 @@ LIBRARIES += AP_BattMonitor
LIBRARIES += AP_BoardConfig
LIBRARIES += AP_Frsky_Telem
LIBRARIES += AC_Sprayer
LIBRARIES += AP_EPM
LIBRARIES += AP_Parachute
LIBRARIES += AP_LandingGear
LIBRARIES += AP_Terrain
@ -62,3 +62,4 @@ LIBRARIES += AC_InputManager
LIBRARIES += AP_ADSB
LIBRARIES += AP_JSButton
LIBRARIES += AP_LeakDetector
LIBRARIES += AP_Gripper

11
ArduSub/sensors.cpp
View File

@ -140,7 +140,8 @@ void Sub::update_optical_flow(void)
uint8_t flowQuality = optflow.quality();
Vector2f flowRate = optflow.flowRate();
Vector2f bodyRate = optflow.bodyRate();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
const Vector3f &posOffset = optflow.get_pos_offset();
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset);
if (g.log_bitmask & MASK_LOG_OPTFLOW) {
Log_Write_Optflow();
}
@ -206,10 +207,10 @@ void Sub::accel_cal_update()
}
}
#if EPM_ENABLED == ENABLED
// epm update - moves epm pwm output back to neutral after grab or release is completed
void Sub::epm_update()
#if GRIPPER_ENABLED == ENABLED
// gripper update
void Sub::gripper_update()
{
epm.update();
g2.gripper.update();
}
#endif

14
ArduSub/switches.cpp
View File

@ -233,7 +233,7 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
case AUXSW_RESETTOARMEDYAW:
case AUXSW_SUPERSIMPLE_MODE:
case AUXSW_ACRO_TRAINER:
case AUXSW_EPM:
case AUXSW_GRIPPER:
case AUXSW_SPRAYER:
case AUXSW_PARACHUTE_ENABLE:
case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
@ -400,16 +400,16 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
}
break;
#if EPM_ENABLED == ENABLED
case AUXSW_EPM:
#if GRIPPER_ENABLED == ENABLED
case AUXSW_GRIPPER:
switch(ch_flag) {
case AUX_SWITCH_LOW:
epm.release();
Log_Write_Event(DATA_EPM_RELEASE);
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case AUX_SWITCH_HIGH:
epm.grab();
Log_Write_Event(DATA_EPM_GRAB);
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
}
break;

20
ArduSub/system.cpp
View File

@ -111,9 +111,9 @@ void Sub::init_ardupilot()
// initialise serial port
serial_manager.init();
// init EPM cargo gripper
#if EPM_ENABLED == ENABLED
epm.init();
// init cargo gripper
#if GRIPPER_ENABLED == ENABLED
g2.gripper.init();
#endif
// initialise notify system
@ -143,11 +143,6 @@ void Sub::init_ardupilot()
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky
frsky_telemetry.init(serial_manager);
#endif
// identify ourselves correctly with the ground station
mavlink_system.sysid = g.sysid_this_mav;
@ -420,15 +415,6 @@ void Sub::check_usb_mux(void)
ap.usb_connected = usb_check;
}
// frsky_telemetry_send - sends telemetry data using frsky telemetry
// should be called at 5Hz by scheduler
#if FRSKY_TELEM_ENABLED == ENABLED
void Sub::frsky_telemetry_send(void)
{
frsky_telemetry.send_frames((uint8_t)control_mode);
}
#endif
/*
should we log a message type now?
*/

4
ArduSub/takeoff.cpp
View File

@ -114,7 +114,7 @@ void Sub::auto_takeoff_set_start_alt(void)
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// we are not flying, add the takeoff_nav_alt
auto_takeoff_no_nav_alt_cm += g2.takeoff_nav_alt * 100;
auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100;
}
}
@ -127,7 +127,7 @@ void Sub::auto_takeoff_attitude_run(float target_yaw_rate)
{
float nav_roll, nav_pitch;
if (g2.takeoff_nav_alt > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
if (g2.wp_navalt_min > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
// we haven't reached the takeoff navigation altitude yet
nav_roll = 0;
nav_pitch = 0;

4
ArduSub/wscript
View File

@ -17,8 +17,6 @@ def build(bld):
'AC_Sprayer',
'AC_WPNav',
'AP_Camera',
'AP_EPM',
'AP_Frsky_Telem',
'AP_IRLock',
'AP_InertialNav',
'AP_JSButton',
@ -33,6 +31,8 @@ def build(bld):
'AP_RSSI',
'AP_Relay',
'AP_ServoRelayEvents',
'AP_Proximity',
'AP_Gripper',
],
use='mavlink',
)