From 9198b8cb29970d66071f8bb2cdee0bb918781460 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Tue, 7 Jun 2016 21:49:10 -0700 Subject: [PATCH] Sub: Changes to match recent Copter updates. --- ArduSub/GCS_Mavlink.h | 23 +++++++++++++++++++++++ ArduSub/Log.cpp | 2 +- ArduSub/Parameters.cpp | 13 ++++++++----- ArduSub/Parameters.h | 3 ++- ArduSub/Sub.h | 9 ++++----- ArduSub/commands.cpp | 7 +++++++ ArduSub/config.h | 6 +++--- ArduSub/control_flip.cpp | 30 +++++++++--------------------- ArduSub/defines.h | 3 ++- ArduSub/motors.cpp | 3 --- ArduSub/position_vector.cpp | 20 -------------------- ArduSub/system.cpp | 15 ++++----------- 12 files changed, 63 insertions(+), 71 deletions(-) create mode 100644 ArduSub/GCS_Mavlink.h diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h new file mode 100644 index 0000000000..d473f4366b --- /dev/null +++ b/ArduSub/GCS_Mavlink.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +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; + +}; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index fd8e755d6f..89213a0b69 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -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(); diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 257aebb6d6..32ab76361c 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -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 -#include // MAVLink GCS definitions #include // Serial manager library #include // ArduPilot GPS library #include // 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 // 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); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index 0125ec7d96..8e3bcc50a3 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -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); } diff --git a/ArduSub/config.h b/ArduSub/config.h index bfd85e5cac..ccb73c7cbd 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -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 diff --git a/ArduSub/control_flip.cpp b/ArduSub/control_flip.cpp index fa40cc35f9..d16d505093 100644 --- a/ArduSub/control_flip.cpp +++ b/ArduSub/control_flip.cpp @@ -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); } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index be83d61c88..bf3d60d417 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -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 diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 4dbb0d382f..1fb0f611be 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -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(); diff --git a/ArduSub/position_vector.cpp b/ArduSub/position_vector.cpp index cbf109cf21..305c223b4e 100644 --- a/ArduSub/position_vector.cpp +++ b/ArduSub/position_vector.cpp @@ -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) { diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 285de64035..66a2928553 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -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