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

Sub: Update to library changes

Browse Source
This commit is contained in:
Jacob Walser 2017-01-31 12:06:55 -05:00 committed by Andrew Tridgell
parent ac1e1b9460
commit 029cf3b388
8 changed files with 109 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -498,7 +498,7 @@ void Sub::one_hz_loop()
}
// update assigned functions and enable auxiliary servos
RC_Channel_aux::enable_aux_servos();
SRV_Channels::enable_aux_servos();
check_usb_mux();

2
ArduSub/GCS_Mavlink.cpp
View File

@ -1524,7 +1524,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// Send an invalid signal to the motors to prevent spinning due to neutral (1500) pwm pulse being cut short
// For that matter, send an invalid signal to all channels to prevent undesired/unexpected behavior
hal.rcout->cork();
for(int i=0; i<RC_MAX_CHANNELS; i++ ) {
for(int i=0; i<NUM_RC_CHANNELS; i++ ) {
// Set to 1 because 0 is interpreted as flag to ignore update
hal.rcout->write(i, 1);
}

4
ArduSub/Log.cpp
View File

@ -377,9 +377,9 @@ void Sub::Log_Write_Attitude()
DataFlash.Log_Write_Attitude(ahrs, targets);
#if OPTFLOW == ENABLED
DataFlash.Log_Write_EKF2(ahrs,optflow.enabled());
DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
#else
DataFlash.Log_Write_EKF2(ahrs,false);
DataFlash.Log_Write_EKF(ahrs,false);
#endif
DataFlash.Log_Write_AHRS2(ahrs);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

86
ArduSub/Parameters.cpp
View File

@ -409,56 +409,6 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Advanced
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 0),
// RC channel
//-----------
// @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_1, "RC1_", RC_Channel),
// @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_2, "RC2_", RC_Channel),
// @Group: RC3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_3, "RC3_", RC_Channel),
// @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_4, "RC4_", RC_Channel),
// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),
// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),
// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),
// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),
// @Group: RC10_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_10, "RC10_", RC_Channel_aux),
// @Group: RC11_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_11, "RC11_", RC_Channel_aux),
// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),
// @Group: RC13_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_13, "RC13_", RC_Channel_aux),
// @Group: RC14_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),
// @Param: JS_GAIN_DEFAULT
// @DisplayName: Default gain at boot
// @Description: Default gain at boot, must be in range [JS_GAIN_MIN , JS_GAIN_MAX]
@ -521,7 +471,7 @@ const AP_Param::Info Sub::var_info[] = {
// @User: Standard
// @RebootRequired: True
// @Values: 0:BlueROV1, 1:Vectored, 2:Vectored_6DOF, 3:Vectored_6DOF_90, 4:SimpleROV-3, 5:SimpleROV-4, 6:SimpleROV-5, 7:Custom
GSCALAR(frame_configuration, "FRAME_CONFIG", AS_MOTORS_VECTORED_FRAME),
GSCALAR(frame_configuration, "FRAME_CONFIG", AP_Motors6DOF::SUB_FRAME_VECTORED),
// @Group: BTN0_
// @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
@ -858,14 +808,14 @@ const AP_Param::Info Sub::var_info[] = {
GOBJECT(rcmap, "RCMAP_", RCMapper),
#endif
// @Group: EKF_
// @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
GOBJECTN(EKF, NavEKF, "EKF_", NavEKF),
// @Group: EK2_
// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2),
// @Group: EK3_
// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3),
// @Group: MIS_
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
GOBJECT(mission, "MIS_", AP_Mission),
@ -960,6 +910,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(gripper, "GRIP_", 3, ParametersG2, AP_Gripper),
#endif
// @Group: SERVO
// @Path: ../libraries/SRV_Channel/SRV_Channel.cpp
AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),
// @Group: RC
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels),
AP_GROUPEND
};
@ -1001,4 +959,22 @@ void Sub::load_parameters(void)
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
convert_old_parameters();
}
void Sub::convert_old_parameters(void) {
const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old,
Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old,
Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old,
Parameters::k_param_rc_7_old, Parameters::k_param_rc_8_old,
Parameters::k_param_rc_9_old, Parameters::k_param_rc_10_old,
Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
const uint16_t old_aux_chan_mask = 0x3FF0;
// note that we don't pass in rcmap as we don't want output channel functions changed based on rcmap
if (SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, nullptr)) {
// the rest needs to be done after motors allocation
// upgrading_frame_params = true;
}
}

145
ArduSub/Parameters.h
View File

@ -86,7 +86,7 @@ public:
// Navigation libraries
k_param_ahrs = 50, // AHRS
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation // remove
k_param_NavEKF2, // EKF2
k_param_attitude_control, // Attitude Control
k_param_pos_control, // Position Control
@ -97,6 +97,7 @@ public:
k_param_rally, // Disabled
k_param_circle_nav, // Disabled
k_param_avoid, // Relies on proximity and fence
k_param_NavEKF3,
// Other external hardware interfaces
@ -106,21 +107,21 @@ public:
k_param_camera_mount, // Camera gimbal
// RC_Channel settings
k_param_rc_1 = 75,
k_param_rc_2,
k_param_rc_3,
k_param_rc_4,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_rc_9,
k_param_rc_10,
k_param_rc_11,
k_param_rc_12,
k_param_rc_13,
k_param_rc_14,
// RC_Channel settings (deprecated)
k_param_rc_1_old = 75,
k_param_rc_2_old,
k_param_rc_3_old,
k_param_rc_4_old,
k_param_rc_5_old,
k_param_rc_6_old,
k_param_rc_7_old,
k_param_rc_8_old,
k_param_rc_9_old,
k_param_rc_10_old,
k_param_rc_11_old,
k_param_rc_12_old,
k_param_rc_13_old,
k_param_rc_14_old,
// Joystick gain parameters
k_param_gain_default,
@ -317,72 +318,56 @@ public:
AP_Int8 fs_ekf_action;
AP_Int8 fs_crash_check;
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Int16 gcs_pid_mask;
AP_Int8 terrain_follow;
AP_Int8 terrain_follow;
// RC channels
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
RC_Channel rc_4;
RC_Channel_aux rc_5;
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
RC_Channel_aux rc_9;
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
RC_Channel_aux rc_12;
RC_Channel_aux rc_13;
RC_Channel_aux rc_14;
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
AP_Float gain_default;
AP_Float maxGain;
AP_Float minGain;
AP_Int8 numGainSettings;
AP_Float throttle_gain;
AP_Int16 cam_tilt_center;
AP_Float gain_default;
AP_Float maxGain;
AP_Float minGain;
AP_Int8 numGainSettings;
AP_Float throttle_gain;
AP_Int16 cam_tilt_center;
AP_Int16 cam_tilt_step;
AP_Int16 lights_step;
AP_Int16 cam_tilt_step;
AP_Int16 lights_step;
// Joystick button parameters
JSButton jbtn_0;
JSButton jbtn_1;
JSButton jbtn_2;
JSButton jbtn_3;
JSButton jbtn_4;
JSButton jbtn_5;
JSButton jbtn_6;
JSButton jbtn_7;
JSButton jbtn_8;
JSButton jbtn_9;
JSButton jbtn_10;
JSButton jbtn_11;
JSButton jbtn_12;
JSButton jbtn_13;
JSButton jbtn_14;
JSButton jbtn_15;
JSButton jbtn_0;
JSButton jbtn_1;
JSButton jbtn_2;
JSButton jbtn_3;
JSButton jbtn_4;
JSButton jbtn_5;
JSButton jbtn_6;
JSButton jbtn_7;
JSButton jbtn_8;
JSButton jbtn_9;
JSButton jbtn_10;
JSButton jbtn_11;
JSButton jbtn_12;
JSButton jbtn_13;
JSButton jbtn_14;
JSButton jbtn_15;
// Acro parameters
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_expo;
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_expo;
// PI/D controllers
AC_PI_2D pi_vel_xy;
AC_PI_2D pi_vel_xy;
AC_P p_vel_z;
AC_PID pid_accel_z;
AC_P p_vel_z;
AC_PID pid_accel_z;
AC_P p_pos_xy;
AC_P p_alt_hold;
AC_P p_pos_xy;
AC_P p_alt_hold;
@ -406,21 +391,6 @@ public:
// above.
Parameters() :
rc_1 (CH_1),
rc_2 (CH_2),
rc_3 (CH_3),
rc_4 (CH_4),
rc_5 (CH_5),
rc_6 (CH_6),
rc_7 (CH_7),
rc_8 (CH_8),
rc_9 (CH_9),
rc_10 (CH_10),
rc_11 (CH_11),
rc_12 (CH_12),
rc_13 (CH_13),
rc_14 (CH_14),
// PID controller initial P initial I initial D initial imax initial filt hz pid rate
//---------------------------------------------------------------------------------------------------------------------------------
pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME),
@ -466,6 +436,11 @@ public:
AP_Proximity proximity;
#endif
// RC input channels
RC_Channels rc_channels;
// control over servo output ranges
SRV_Channels servo_channels;
};
extern const AP_Param::Info var_info[];

8
ArduSub/Sub.h
View File

@ -52,8 +52,8 @@
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
#include <AP_InertialSensor/AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
#include <AP_AHRS/AP_AHRS.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AP_Rally/AP_Rally.h> // Rally point library
#include <AC_PID/AC_PID.h> // PID library
@ -196,9 +196,9 @@ private:
#endif
// Inertial Navigation EKF
NavEKF EKF{&ahrs, barometer, rangefinder};
NavEKF2 EKF2{&ahrs, barometer, rangefinder};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF, EKF2, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
NavEKF3 EKF3{&ahrs, barometer, rangefinder};
AP_AHRS_NavEKF ahrs{ins, barometer, gps, rangefinder, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL sitl;
@ -879,6 +879,8 @@ private:
bool surface_init(bool ignore_flags);
void surface_run();
void convert_old_parameters(void);
public:
void mavlink_delay_cb();
void failsafe_check();

16
ArduSub/joystick.cpp
View File

@ -160,12 +160,12 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) {
uint8_t i;
// Find the first aux channel configured as mount tilt, if any
if(RC_Channel_aux::find_channel(RC_Channel_aux::k_mount_tilt, i)) {
if(SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
// Get the channel output limits
RC_Channel *ch = RC_Channel::rc_channel(i);
uint16_t min = ch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN);
uint16_t max = ch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
SRV_Channel *ch = SRV_Channels::srv_channel(i);
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt_goal = constrain_int16(cam_tilt_goal-g.cam_tilt_step,min,max);
}
@ -175,12 +175,12 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) {
uint8_t i;
// Find the first aux channel configured as mount tilt, if any
if(RC_Channel_aux::find_channel(RC_Channel_aux::k_mount_tilt, i)) {
if(SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
// Get the channel output limits
RC_Channel *ch = RC_Channel::rc_channel(i);
uint16_t min = ch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN);
uint16_t max = ch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
SRV_Channel *ch = SRV_Channels::srv_channel(i);
uint16_t min = ch->get_output_min();
uint16_t max = ch->get_output_max();
cam_tilt_goal = constrain_int16(cam_tilt_goal+g.cam_tilt_step,min,max);
}

2
ArduSub/make.inc
View File

@ -14,8 +14,8 @@ LIBRARIES += AP_Math
LIBRARIES += AP_InertialSensor
LIBRARIES += AP_AccelCal
LIBRARIES += AP_AHRS
LIBRARIES += AP_NavEKF
LIBRARIES += AP_NavEKF2
LIBRARIES += AP_NavEKF3
LIBRARIES += AP_Mission
LIBRARIES += AP_Rally
LIBRARIES += AC_PID