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

Sub: Changes to match recent Copter updates.

This commit is contained in:
Rustom Jehangir 2016-06-07 21:49:10 -07:00 committed by Andrew Tridgell
parent 7e33957af2
commit 9198b8cb29
12 changed files with 63 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduSub/GCS_Mavlink.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include <GCS_MAVLink/GCS.h>
class GCS_MAVLINK_Sub : public GCS_MAVLINK
{
public:
void data_stream_send(void) override;
protected:
uint32_t telem_delay() const override;
private:
void handleMessage(mavlink_message_t * msg) override;
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
bool try_send_message(enum ap_message id) override;
};

2
ArduSub/Log.cpp
View File

@ -215,7 +215,7 @@ void Sub::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
// Write a Current data packet
void Sub::Log_Write_Current()
{
DataFlash.Log_Write_Current(battery, (int16_t)(motors.get_throttle()));
DataFlash.Log_Write_Current(battery);
// also write power status
DataFlash.Log_Write_Power();

13
ArduSub/Parameters.cpp
View File

@ -146,7 +146,7 @@ const AP_Param::Info Sub::var_info[] = {
// @Increment: .1
// @Values: 0:Disabled,1:Shallow,3:Steep
// @User: Standard
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE),
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE_DEFAULT),
// @Param: RTL_SPEED
// @DisplayName: RTL speed
@ -1128,7 +1128,10 @@ void Sub::convert_pid_parameters(void)
{ Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" },
{ Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }
};
AP_Param::ConversionInfo filt_conversion_info[] = {
AP_Param::ConversionInfo angle_and_filt_conversion_info[] = {
{ Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" },
{ Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" },
{ Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" },
{ Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" },
{ Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" },
{ Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }
@ -1153,9 +1156,9 @@ void Sub::convert_pid_parameters(void)
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&imax_conversion_info[i], 1.0f/4500.0f);
}
// convert filter without scaling
table_size = ARRAY_SIZE(filt_conversion_info);
// convert angle controller gain and filter without scaling
table_size = ARRAY_SIZE(angle_and_filt_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&filt_conversion_info[i], 1.0f);
AP_Param::convert_old_parameter(&angle_and_filt_conversion_info[i], 1.0f);
}
}

3
ArduSub/Parameters.h
View File

@ -53,7 +53,8 @@ public:
k_param_software_type,
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2,
k_param_NavEKF2_old, // deprecated
k_param_NavEKF2,
// simulation
k_param_sitl = 10,

9
ArduSub/Sub.h
View File

@ -42,7 +42,6 @@
// Application dependencies
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
#include <AP_GPS/AP_GPS.h> // ArduPilot GPS library
#include <DataFlash/DataFlash.h> // ArduPilot Mega Flash Memory Library
@ -96,6 +95,8 @@
#include "defines.h"
#include "config.h"
#include "GCS_Mavlink.h"
// libraries which are dependent on #defines in defines.h and/or config.h
#if SPRAYER == ENABLED
#include <AC_Sprayer/AC_Sprayer.h> // crop sprayer library
@ -120,7 +121,7 @@
class Sub : public AP_HAL::HAL::Callbacks {
public:
friend class GCS_MAVLINK;
friend class GCS_MAVLINK_Sub;
friend class Parameters;
Sub(void);
@ -214,7 +215,7 @@ private:
AP_SerialManager serial_manager;
static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
GCS_MAVLINK_Sub gcs[MAVLINK_COMM_NUM_BUFFERS];
// User variables
#ifdef USERHOOK_VARIABLES
@ -640,7 +641,6 @@ private:
void send_rpm(mavlink_channel_t chan);
void rpm_update();
void send_pid_tuning(mavlink_channel_t chan);
bool telemetry_delayed(mavlink_channel_t chan);
void gcs_send_message(enum ap_message id);
void gcs_send_mission_item_reached_message(uint16_t mission_index);
void gcs_data_stream_send(void);
@ -918,7 +918,6 @@ private:
uint16_t perf_info_get_num_long_running();
uint32_t perf_info_get_num_dropped();
Vector3f pv_location_to_vector(const Location& loc);
Vector3f pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec);
float pv_alt_above_origin(float alt_above_home_cm);
float pv_alt_above_home(float alt_above_origin_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);

7
ArduSub/commands.cpp
View File

@ -137,6 +137,13 @@ void Sub::set_system_time_from_GPS()
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
uint64_t gps_timestamp = gps.time_epoch_usec();
hal.util->set_system_clock(gps_timestamp);
// update signing timestamp
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
ap.system_time_set = true;
Log_Write_Event(DATA_SYSTEM_TIME_SET);
}

6
ArduSub/config.h
View File

@ -221,7 +221,7 @@
#ifndef COMPASS_OFFSETS_MAX
# define COMPASS_OFFSETS_MAX 600 // PX4 onboard compass has high offsets
#endif
#else // SITL, FLYMAPLE, etc
#else // SITL, etc
#ifndef COMPASS_OFFSETS_MAX
# define COMPASS_OFFSETS_MAX 500
#endif
@ -417,8 +417,8 @@
# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb
#endif
#ifndef RTL_CONE_SLOPE
# define RTL_CONE_SLOPE 3.0f // slope of RTL cone (height / distance). 0 = No cone
#ifndef RTL_CONE_SLOPE_DEFAULT
# define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone
#endif
#ifndef RTL_MIN_CONE_SLOPE

30
ArduSub/control_flip.cpp
View File

@ -20,8 +20,8 @@
* Flip_Recover (while copter is between -90deg and original target angle) : use earth frame angle controller to return vehicle to original attitude
*/
#define FLIP_THR_INC 200 // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 240 // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_THR_INC 0.20f // throttle increase during Flip_Start stage (under 45deg lean angle)
#define FLIP_THR_DEC 0.24f // throttle decrease during Flip_Roll stage (between 45deg ~ -90deg roll)
#define FLIP_ROTATION_RATE 40000 // rotation rate request in centi-degrees / sec (i.e. 400 deg/sec)
#define FLIP_TIMEOUT_MS 2500 // timeout after 2.5sec. Vehicle will switch back to original flight mode
#define FLIP_RECOVERY_ANGLE 500 // consider successful recovery when roll is back within 5 degrees of original
@ -34,7 +34,6 @@
FlipState flip_state; // current state of flip
control_mode_t flip_orig_control_mode; // flight mode when flip was initated
mode_reason_t flip_orig_control_mode_reason;
uint32_t flip_start_time; // time since flip began
int8_t flip_roll_dir; // roll direction (-1 = roll left, 1 = roll right)
int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = pitch back)
@ -64,7 +63,6 @@ bool Sub::flip_init(bool ignore_checks)
// capture original flight mode so that we can return to it after completion
flip_orig_control_mode = control_mode;
flip_orig_control_mode_reason = control_mode_reason;
// initialise state
flip_state = Flip_Start;
@ -145,9 +143,7 @@ void Sub::flip_run()
// between 45deg ~ -90deg request 400deg/sec roll
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// beyond -90deg move on to recovery
if ((flip_angle < 4500) && (flip_angle > -9000)) {
@ -157,11 +153,9 @@ void Sub::flip_run()
case Flip_Pitch_A:
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) > 9000) && (flip_angle > 4500)) {
@ -173,9 +167,7 @@ void Sub::flip_run()
// between 45deg ~ -90deg request 400deg/sec pitch
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
// decrease throttle
if (throttle_out >= g.throttle_min) {
throttle_out = MAX(throttle_out - FLIP_THR_DEC, g.throttle_min);
}
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
// check roll for inversion
if ((labs(ahrs.roll_sensor) < 9000) && (flip_angle > -4500)) {
@ -201,7 +193,7 @@ void Sub::flip_run()
// check for successful recovery
if (fabsf(recovery_angle) <= FLIP_RECOVERY_ANGLE) {
// restore original flight mode
if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
@ -212,7 +204,7 @@ void Sub::flip_run()
case Flip_Abandon:
// restore original flight mode
if (!set_mode(flip_orig_control_mode, flip_orig_control_mode_reason)) {
if (!set_mode(flip_orig_control_mode, MODE_REASON_FLIP_COMPLETE)) {
// this should never happen but just in case
set_mode(STABILIZE, MODE_REASON_UNKNOWN);
}
@ -222,9 +214,5 @@ void Sub::flip_run()
}
// output pilot's throttle without angle boost
if (is_zero(throttle_out)) {
attitude_control.set_throttle_out_unstabilized(0,false,g.throttle_filt);
} else {
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
}
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
}

3
ArduSub/defines.h
View File

@ -128,7 +128,8 @@ enum mode_reason_t {
MODE_REASON_THROTTLE_LAND_ESCAPE,
MODE_REASON_FENCE_BREACH,
MODE_REASON_TERRAIN_FAILSAFE,
MODE_REASON_BRAKE_TIMEOUT
MODE_REASON_BRAKE_TIMEOUT,
MODE_REASON_FLIP_COMPLETE
};
// Tuning enumeration

3
ArduSub/motors.cpp
View File

@ -194,9 +194,6 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
sprayer.test_pump(false);
#endif
// short delay to allow reading of rc inputs
delay(30);
// enable output to motors
enable_motor_output();

20
ArduSub/position_vector.cpp
View File

@ -17,26 +17,6 @@ Vector3f Sub::pv_location_to_vector(const Location& loc)
return Vector3f((loc.lat-origin.lat) * LATLON_TO_CM, (loc.lng-origin.lng) * LATLON_TO_CM * scaleLongDown, alt_above_origin);
}
// pv_location_to_vector_with_default - convert lat/lon coordinates to a position vector,
// defaults to the default_posvec's lat/lon and alt if the provided lat/lon or alt are zero
Vector3f Sub::pv_location_to_vector_with_default(const Location& loc, const Vector3f& default_posvec)
{
Vector3f pos = pv_location_to_vector(loc);
// set target altitude to default if not provided
if (loc.alt == 0) {
pos.z = default_posvec.z;
}
// set target position to default if not provided
if (loc.lat == 0 && loc.lng == 0) {
pos.x = default_posvec.x;
pos.y = default_posvec.y;
}
return pos;
}
// pv_alt_above_origin - convert altitude above home to altitude above EKF origin
float Sub::pv_alt_above_origin(float alt_above_home_cm)
{

15
ArduSub/system.cpp
View File

@ -138,17 +138,10 @@ void Sub::init_ardupilot()
ap.usb_connected = true;
check_usb_mux();
// init the GCS connected to the console
gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);
// init telemetry port
gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// setup serial port for telem2
gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
// setup serial port for fourth telemetry port (not used by default)
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
// setup telem slots with serial ports
for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky