From ff8008d81a040c61c3dd941b30924bcfd0d75ce1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Jan 2016 21:03:22 +1100 Subject: [PATCH 001/109] DataFlash: remove unused callbacks, unimplemented functions Closes #3269 --- libraries/DataFlash/DFMessageWriter.cpp | 4 ---- libraries/DataFlash/DataFlash.cpp | 12 ------------ libraries/DataFlash/DataFlash.h | 11 +---------- libraries/DataFlash/DataFlash_Backend.h | 6 ------ 4 files changed, 1 insertion(+), 32 deletions(-) diff --git a/libraries/DataFlash/DFMessageWriter.cpp b/libraries/DataFlash/DFMessageWriter.cpp index 2da44baf3a..307457c6bb 100644 --- a/libraries/DataFlash/DFMessageWriter.cpp +++ b/libraries/DataFlash/DFMessageWriter.cpp @@ -37,9 +37,6 @@ void DFMessageWriter_DFLogStart::process() if (!_dataflash_backend->Log_Write_Format(_dataflash_backend->structure(next_format_to_send))) { return; // call me again! } - // provide hook to avoid corrupting the APM1/APM2 - // dataflash by writing too fast: - _dataflash_backend->WroteStartupFormat(); next_format_to_send++; } _fmt_done = true; @@ -52,7 +49,6 @@ void DFMessageWriter_DFLogStart::process() return; } ap = AP_Param::next_scalar(&token, &type); - _dataflash_backend->WroteStartupParam(); } stage = ls_blockwriter_stage_sysinfo; diff --git a/libraries/DataFlash/DataFlash.cpp b/libraries/DataFlash/DataFlash.cpp index 3022ff022e..6c19adc198 100644 --- a/libraries/DataFlash/DataFlash.cpp +++ b/libraries/DataFlash/DataFlash.cpp @@ -111,13 +111,6 @@ uint16_t DataFlash_Class::get_num_logs(void) { } return backends[0]->get_num_logs(); } -void DataFlash_Class::Log_Fill_Format(const struct LogStructure *s, struct log_Format &pkt) { - if (_next_backend == 0) { - // how were we called?! - return; - } - backends[0]->Log_Fill_Format(s, pkt); -} void DataFlash_Class::LogReadProcess(uint16_t log_num, uint16_t start_page, uint16_t end_page, @@ -189,11 +182,6 @@ void DataFlash_Class::Log_Write_EntireMission(const AP_Mission &mission) FOR_EACH_BACKEND(Log_Write_EntireMission(mission)); } -void DataFlash_Class::Log_Write_Format(const struct LogStructure *s) -{ - FOR_EACH_BACKEND(Log_Write_Format(s)); -} - void DataFlash_Class::Log_Write_Message(const char *message) { FOR_EACH_BACKEND(Log_Write_Message(message)); diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 77c078e8c4..c22376fcda 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -88,7 +88,7 @@ public: void StartNewLog(void); void EnableWrites(bool enable); - void Log_Write_Format(const struct LogStructure *structure); + void Log_Write_Parameter(const char *name, float value); void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt); void Log_Write_RFND(const RangeFinder &rangefinder); @@ -149,11 +149,6 @@ public: void periodic_tasks(); // may want to split this into GCS/non-GCS duties - // this is out here for the trickle-startup-messages logging. - // Think before calling. - bool Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token, - enum ap_var_type type); - vehicle_startup_message_Log_Writer _vehicle_messages; // parameter support @@ -166,10 +161,6 @@ public: const struct LogStructure *structure(uint16_t num) const; protected: - void Log_Fill_Format(const struct LogStructure *structure, struct log_Format &pkt); - - void WroteStartupFormat(); - void WroteStartupParam(); const struct LogStructure *_structures; uint8_t _num_types; diff --git a/libraries/DataFlash/DataFlash_Backend.h b/libraries/DataFlash/DataFlash_Backend.h index 27313cbf11..b89a7b1751 100644 --- a/libraries/DataFlash/DataFlash_Backend.h +++ b/libraries/DataFlash/DataFlash_Backend.h @@ -55,9 +55,6 @@ public: void EnableWrites(bool enable) { _writes_enabled = enable; } bool logging_started(void) const { return log_write_started; } - // initialisation this really shouldn't take structure and - // num_types, however the CLI LogReadProcess function requires it. - // That function needs to be split. virtual void Init() { _writes_enabled = true; } @@ -86,9 +83,6 @@ public: uint8_t num_types() const; const struct LogStructure *structure(uint8_t structure) const; - virtual void WroteStartupFormat() { } - virtual void WroteStartupParam() { } - void Log_Write_EntireMission(const AP_Mission &mission); bool Log_Write_Format(const struct LogStructure *structure); bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd); From 93f07076796578a04474c078e5eca80fc893cf01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 14:41:07 +1100 Subject: [PATCH 002/109] AP_Param: added count_parameters() API this auto-clears when an ENABLE parameter changes --- libraries/AP_Param/AP_Param.cpp | 27 +++++++++++++++++++++++++++ libraries/AP_Param/AP_Param.h | 4 ++++ 2 files changed, 31 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 39276b1bb1..b8e733bfb4 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -67,6 +67,9 @@ extern const AP_HAL::HAL &hal; // number of rows in the _var_info[] table uint16_t AP_Param::_num_vars; +// cached parameter count +uint16_t AP_Param::_parameter_count; + // storage and naming information about all types that can be saved const AP_Param::Info *AP_Param::_var_info; @@ -823,6 +826,11 @@ bool AP_Param::save(bool force_save) ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float)); } + if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) { + // clear cached parameter count + _parameter_count = 0; + } + char name[AP_MAX_NAME_SIZE+1]; copy_name_info(info, ginfo, ginfo0, idx, name, sizeof(name), true); @@ -1656,3 +1664,22 @@ void AP_Param::send_parameter(char *name, enum ap_var_type param_header_type) co name_axis = 'Z'; GCS_MAVLINK::send_parameter_value_all(name, AP_PARAM_FLOAT, v->z); } + +/* + return count of all scalar parameters + */ +uint16_t AP_Param::count_parameters(void) +{ + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { + AP_Param *vp; + AP_Param::ParamToken token; + + vp = AP_Param::first(&token, NULL); + do { + _parameter_count++; + } while (NULL != (vp = AP_Param::next_scalar(&token, NULL))); + } + return _parameter_count; +} + diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 2b9d89a526..a32d593acc 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -313,6 +313,9 @@ public: // return true if the parameter is configured bool configured(void) { return configured_in_defaults_file() || configured_in_storage(); } + // count of parameters in tree + static uint16_t count_parameters(void); + private: /// EEPROM header /// @@ -439,6 +442,7 @@ private: static StorageAccess _storage; static uint16_t _num_vars; + static uint16_t _parameter_count; static const struct Info * _var_info; /* From 68a46bc1fff9c8ffb84e3786779d62f24b4d503f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 14:41:32 +1100 Subject: [PATCH 003/109] GCS_MAVLink: use AP_Param::count_parameters() --- libraries/GCS_MAVLink/GCS.h | 6 ------ libraries/GCS_MAVLink/GCS_Common.cpp | 23 +++-------------------- 2 files changed, 3 insertions(+), 26 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 6bac32d597..43c83678e6 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -212,12 +212,6 @@ private: /// /// @return The number of reportable parameters. /// - static uint16_t _count_parameters(); ///< count reportable - // parameters - - static uint16_t _parameter_count; ///< cache of reportable - // parameters - mavlink_channel_t chan; uint16_t packet_drops; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9f5ecff955..a2b60f0274 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -27,7 +27,6 @@ extern const AP_HAL::HAL& hal; uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms; uint8_t GCS_MAVLINK::mavlink_active = 0; -uint16_t GCS_MAVLINK::_parameter_count; GCS_MAVLINK::GCS_MAVLINK() : waypoint_receive_timeout(5000) @@ -103,22 +102,6 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager init(uart, mav_chan); } -uint16_t -GCS_MAVLINK::_count_parameters() -{ - // if we haven't cached the parameter count yet... - if (0 == _parameter_count) { - AP_Param *vp; - AP_Param::ParamToken token; - - vp = AP_Param::first(&token, NULL); - do { - _parameter_count++; - } while (NULL != (vp = AP_Param::next_scalar(&token, NULL))); - } - return _parameter_count; -} - /** * @brief Send the next pending parameter, called from deferred message * handling code @@ -520,7 +503,7 @@ void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg) // Start sending parameters - next call to ::update will kick the first one out _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); _queued_parameter_index = 0; - _queued_parameter_count = _count_parameters(); + _queued_parameter_count = AP_Param::count_parameters(); } void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) @@ -555,7 +538,7 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg) param_name, value, mav_var_type(p_type), - _count_parameters(), + AP_Param::count_parameters(), packet.param_index); } @@ -1184,7 +1167,7 @@ void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type p param_name, param_value, mav_var_type(param_type), - _count_parameters(), + AP_Param::count_parameters(), -1); } } From 6c064ae8bd3be1496f4ff9d260b8a69362f520a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 19:05:42 +1100 Subject: [PATCH 004/109] HAL_SITL: flow control is enabled on SITL faster parameter download --- libraries/AP_HAL_SITL/UARTDriver.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 4c9aaf53d0..ebe9e9d785 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -56,6 +56,8 @@ public: // file descriptor, exposed so SITL_State::loop_hook() can use it int _fd; + enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; } + private: uint8_t _portNumber; bool _connected = false; // true if a client has connected From 9941c91d36380f3a171aa0fe3a733dade9c41de2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 19:11:14 +1100 Subject: [PATCH 005/109] RC_Channel: prevent a warning message for assigned RC channels --- libraries/RC_Channel/RC_Channel_aux.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index 775bbe472d..89f7554f3c 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -391,6 +391,9 @@ bool RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::Aux_servo_function_ for (uint8_t i=0; i_ch_out == channel) { if (_aux_channels[i]->function != k_none) { + if (_aux_channels[i]->function == function) { + return true; + } hal.console->printf("Channel %u already assigned %u\n", (unsigned)channel, (unsigned)_aux_channels[i]->function); From 519afc7a0638522a42006c155b426d648aa7b5da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 19:15:43 +1100 Subject: [PATCH 006/109] HAL_PX4: debug code for FRAM corruption --- libraries/AP_HAL_PX4/Storage.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp index 42aef07553..81d8ecaf82 100644 --- a/libraries/AP_HAL_PX4/Storage.cpp +++ b/libraries/AP_HAL_PX4/Storage.cpp @@ -23,6 +23,7 @@ using namespace PX4; #define STORAGE_DIR "/fs/microsd/APM" #define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg" #define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak" +//#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav" #define MTD_PARAMS_FILE "/fs/mtd" #define MTD_SIGNATURE 0x14012014 #define MTD_SIGNATURE_OFFSET (8192-4) @@ -164,6 +165,15 @@ void PX4Storage::_storage_open(void) } } close(fd); + +#ifdef SAVE_STORAGE_FILE + fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (fd != -1) { + write(fd, _buffer, sizeof(_buffer)); + close(fd); + ::printf("Saved storage file %s\n", SAVE_STORAGE_FILE); + } +#endif _initialised = true; } From 0e8dbe92f0b4cd5570addf8ee5e6841f5236e466 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Dec 2015 21:33:38 +1100 Subject: [PATCH 007/109] GCS_MAVLink: fixed string overrun found by asan --- libraries/GCS_MAVLink/GCS_Common.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a2b60f0274..0cfade2121 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -589,7 +589,9 @@ GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str) comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) { // send immediately - mavlink_msg_statustext_send(chan, severity, str); + char msg[50] {}; + strncpy(msg, str, sizeof(msg)); + mavlink_msg_statustext_send(chan, severity, msg); } else { // send via the deferred queuing system mavlink_statustext_t *s = &pending_status; From a3d781bf3fc98154e357a1435f2222834761b685 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Dec 2015 21:00:12 +1100 Subject: [PATCH 008/109] GCS_MAVLink: fixed a valgrind error --- libraries/GCS_MAVLink/GCS_Common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0cfade2121..b6c3c40c2a 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1142,7 +1142,7 @@ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, .. if ((1U<= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) { - char msg2[50]; + char msg2[50] {}; va_list arg_list; va_start(arg_list, fmt); hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list); From e80fb8b3fa301fb3857d3306099da7a763be99ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 6 Jan 2016 19:58:15 +1100 Subject: [PATCH 009/109] AP_NavEKF2: Improve non-GPS in-flight attitude accuracy The non-GPS mode was not being activated for small height gains - eg indoor flight. The incorrect innovation consistency check was being applied to the synthetic velocity observations. --- libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 3b8fc1434f..aa6af599f4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED() // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity // otherwise we scale it using manoeuvre acceleration // Use different errors if flying without GPS using synthetic position and velocity data - if (PV_AidingMode == AID_NONE && inFlight) { + if (PV_AidingMode == AID_NONE && motorsArmed) { // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use) // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres R_OBS[0] = sq(10.0f); @@ -257,6 +257,8 @@ void NavEKF2_core::FuseVelPosNED() // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres R_OBS[3] = sq(25.0f); R_OBS[4] = R_OBS[3]; + R_OBS[5] = posDownObsNoise; + for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; } else { if (gpsSpdAccuracy > 0.0f) { // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter @@ -270,14 +272,14 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[1] = R_OBS[0]; R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; + R_OBS[5] = posDownObsNoise; + // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity + // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance + // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early + for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); + for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; } - R_OBS[5] = posDownObsNoise; - // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity - // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance - // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early - for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); - for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting From 2777ff63ba683a3ece154787258f28397eeeea5f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Jan 2016 21:59:56 +1100 Subject: [PATCH 010/109] SITL: heli-sim: prevent ground rotation and reduced affect of ground impact --- libraries/SITL/SIM_Helicopter.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index 35e853dcaa..e5a0cb3c41 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -54,7 +54,7 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) : */ void Helicopter::update(const struct sitl_input &input) { - float rsc = (input.servos[7]-1000) / 1000.0f; + float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1); // ignition only for gas helis bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true; @@ -152,17 +152,20 @@ void Helicopter::update(const struct sitl_input &input) accel_body = Vector3f(lateral_x_thrust, lateral_y_thrust, -thrust / mass); accel_body += dcm * air_resistance; + + bool was_on_ground = on_ground(position); update_dynamics(rot_accel); // constrain height to the ground - if (on_ground(position)) { + if (on_ground(position) && !was_on_ground) { // zero roll/pitch, but keep yaw float r, p, y; dcm.to_euler(&r, &p, &y); dcm.from_euler(0, 0, y); position.z = -(ground_level + frame_height - home.alt*0.01f); + velocity_ef.zero(); } // update lat/lon/altitude From 77af00c5e1edbaad6d00f73a752f30d285ead6cf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Jan 2016 17:48:55 +1100 Subject: [PATCH 011/109] AP_Motors: allow arbitrary mapping of channels on multirotors using RCn_FUNCTION with motor1, motor2 etc --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 11 +++++++++-- libraries/AP_Motors/AP_Motors_Class.cpp | 4 ++++ libraries/AP_Motors/AP_Motors_Class.h | 4 ++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index aa2748082f..b03cb512ff 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -415,8 +415,15 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc // set order that motor appears in test _test_order[motor_num] = testing_order; - // disable this channel from being used by RC_Channel_aux - RC_Channel_aux::disable_aux_channel(motor_num); + uint8_t chan; + if (RC_Channel_aux::find_channel((RC_Channel_aux::Aux_servo_function_t)(RC_Channel_aux::k_motor1+motor_num), + chan)) { + _motor_map[motor_num] = chan; + _motor_map_mask |= 1U<write(chan, pwm); } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index fd723f4a49..ad1094c0ac 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -160,5 +160,9 @@ protected: float _batt_voltage; // latest battery voltage reading float _batt_current; // latest battery current reading float _air_density_ratio; // air density / sea level density - decreases in altitude + + // mapping to output channels + uint8_t _motor_map[AP_MOTORS_MAX_NUM_MOTORS]; + uint16_t _motor_map_mask; }; #endif // __AP_MOTORS_CLASS_H__ From dfccf8f7139dc8d332822eb9f7bf006836dd15de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Jan 2016 21:24:06 +1100 Subject: [PATCH 012/109] AP_Motors: also wrap set_freq and enable_ch for motor mapping --- libraries/AP_Motors/AP_MotorsCoax.cpp | 12 ++++---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 14 +++++----- libraries/AP_Motors/AP_MotorsMatrix.cpp | 4 +-- libraries/AP_Motors/AP_MotorsSingle.cpp | 14 +++++----- libraries/AP_Motors/AP_MotorsTri.cpp | 10 +++---- libraries/AP_Motors/AP_Motors_Class.cpp | 29 ++++++++++++++++++++ libraries/AP_Motors/AP_Motors_Class.h | 2 ++ 7 files changed, 58 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index f2595a9279..4207bd26d5 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -88,23 +88,23 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) uint32_t mask2 = 1U << AP_MOTORS_MOT_3 | 1U << AP_MOTORS_MOT_4 ; - hal.rcout->set_freq(mask2, _speed_hz); + rc_set_freq(mask2, _speed_hz); // set update rate for the two servos uint32_t mask = 1U << AP_MOTORS_MOT_1 | 1U << AP_MOTORS_MOT_2 ; - hal.rcout->set_freq(mask, _servo_speed); + rc_set_freq(mask, _servo_speed); } // enable - starts allowing signals to be sent to motors void AP_MotorsCoax::enable() { // enable output channels - hal.rcout->enable_ch(AP_MOTORS_MOT_1); - hal.rcout->enable_ch(AP_MOTORS_MOT_2); - hal.rcout->enable_ch(AP_MOTORS_MOT_3); - hal.rcout->enable_ch(AP_MOTORS_MOT_4); + rc_enable_ch(AP_MOTORS_MOT_1); + rc_enable_ch(AP_MOTORS_MOT_2); + rc_enable_ch(AP_MOTORS_MOT_3); + rc_enable_ch(AP_MOTORS_MOT_4); } // output_min - sends minimum values out to the motor and trim values to the servos diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index ab0bfed3c1..27451eb9bd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -131,19 +131,19 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) 1U << AP_MOTORS_MOT_2 | 1U << AP_MOTORS_MOT_3 | 1U << AP_MOTORS_MOT_4; - hal.rcout->set_freq(mask, _speed_hz); + rc_set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors and servos void AP_MotorsHeli_Single::enable() { // enable output channels - hal.rcout->enable_ch(AP_MOTORS_MOT_1); // swash servo 1 - hal.rcout->enable_ch(AP_MOTORS_MOT_2); // swash servo 2 - hal.rcout->enable_ch(AP_MOTORS_MOT_3); // swash servo 3 - hal.rcout->enable_ch(AP_MOTORS_MOT_4); // yaw - hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor - hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc + rc_enable_ch(AP_MOTORS_MOT_1); // swash servo 1 + rc_enable_ch(AP_MOTORS_MOT_2); // swash servo 2 + rc_enable_ch(AP_MOTORS_MOT_3); // swash servo 3 + rc_enable_ch(AP_MOTORS_MOT_4); // yaw + rc_enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor + rc_enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc } // init_outputs - initialise Servo/PWM ranges and endpoints diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index b03cb512ff..dd15bc97dc 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -49,7 +49,7 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) mask |= 1U << i; } } - hal.rcout->set_freq( mask, _speed_hz ); + rc_set_freq( mask, _speed_hz ); } // set frame orientation (normally + or X) @@ -78,7 +78,7 @@ void AP_MotorsMatrix::enable() // enable output channels for( i=0; ienable_ch(i); + rc_enable_ch(i); } } } diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 8705088ba9..a1a21b2f62 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -96,20 +96,20 @@ void AP_MotorsSingle::set_update_rate( uint16_t speed_hz ) 1U << AP_MOTORS_MOT_2 | 1U << AP_MOTORS_MOT_3 | 1U << AP_MOTORS_MOT_4 ; - hal.rcout->set_freq(mask, _servo_speed); + rc_set_freq(mask, _servo_speed); uint32_t mask2 = 1U << AP_MOTORS_MOT_7; - hal.rcout->set_freq(mask2, _speed_hz); + rc_set_freq(mask2, _speed_hz); } // enable - starts allowing signals to be sent to motors void AP_MotorsSingle::enable() { // enable output channels - hal.rcout->enable_ch(AP_MOTORS_MOT_1); - hal.rcout->enable_ch(AP_MOTORS_MOT_2); - hal.rcout->enable_ch(AP_MOTORS_MOT_3); - hal.rcout->enable_ch(AP_MOTORS_MOT_4); - hal.rcout->enable_ch(AP_MOTORS_MOT_7); + rc_enable_ch(AP_MOTORS_MOT_1); + rc_enable_ch(AP_MOTORS_MOT_2); + rc_enable_ch(AP_MOTORS_MOT_3); + rc_enable_ch(AP_MOTORS_MOT_4); + rc_enable_ch(AP_MOTORS_MOT_7); } // output_min - sends minimum values out to the motor and trim values to the servos diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 8edec89246..ef169a5d46 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -97,17 +97,17 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz ) 1U << AP_MOTORS_MOT_1 | 1U << AP_MOTORS_MOT_2 | 1U << AP_MOTORS_MOT_4; - hal.rcout->set_freq(mask, _speed_hz); + rc_set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors void AP_MotorsTri::enable() { // enable output channels - hal.rcout->enable_ch(AP_MOTORS_MOT_1); - hal.rcout->enable_ch(AP_MOTORS_MOT_2); - hal.rcout->enable_ch(AP_MOTORS_MOT_4); - hal.rcout->enable_ch(AP_MOTORS_CH_TRI_YAW); + rc_enable_ch(AP_MOTORS_MOT_1); + rc_enable_ch(AP_MOTORS_MOT_2); + rc_enable_ch(AP_MOTORS_MOT_4); + rc_enable_ch(AP_MOTORS_CH_TRI_YAW); } // output_min - sends minimum values out to the motors diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index d2fbd87ac3..e65e186ef5 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -76,3 +76,32 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) } hal.rcout->write(chan, pwm); } + +/* + set frequency of a set of channels + */ +void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) +{ + uint32_t mask2 = 0; + for (uint8_t i=0; i<32; i++) { + uint32_t bit = 1UL<set_freq(mask2, freq_hz); +} + +void AP_Motors::rc_enable_ch(uint8_t chan) +{ + if (_motor_map_mask & (1U<enable_ch(chan); +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index ad1094c0ac..fff392a597 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -130,6 +130,8 @@ protected: virtual void output_armed_zero_throttle() { output_min(); } virtual void output_disarmed()=0; virtual void rc_write(uint8_t chan, uint16_t pwm); + virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); + virtual void rc_enable_ch(uint8_t chan); // update the throttle input filter virtual void update_throttle_filter() = 0; From 7c9ee9363b4819eba4203f983a59483d8ea793c2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Jan 2016 07:35:24 +1100 Subject: [PATCH 013/109] AP_Motors: added rc_map_mask() function --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsSingle.cpp | 2 +- libraries/AP_Motors/AP_MotorsTri.cpp | 2 +- libraries/AP_Motors/AP_Motors_Class.cpp | 28 +++++++++++++------- libraries/AP_Motors/AP_Motors_Class.h | 1 + 6 files changed, 23 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 27451eb9bd..864e9194eb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -316,7 +316,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() uint16_t AP_MotorsHeli_Single::get_motor_mask() { // heli uses channels 1,2,3,4,7 and 8 - return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC); + return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC); } // update_motor_controls - sends commands to motor controllers diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index dd15bc97dc..f8ef191dc4 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -114,7 +114,7 @@ uint16_t AP_MotorsMatrix::get_motor_mask() mask |= 1U << i; } } - return mask; + return rc_map_mask(mask); } void AP_MotorsMatrix::output_armed_not_stabilizing() diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index a1a21b2f62..02009d9e35 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -130,7 +130,7 @@ void AP_MotorsSingle::output_min() uint16_t AP_MotorsSingle::get_motor_mask() { // single copter uses channels 1,2,3,4 and 7 - return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6); + return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6); } void AP_MotorsSingle::output_armed_not_stabilizing() diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index ef169a5d46..729ab0e5e6 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -130,7 +130,7 @@ void AP_MotorsTri::output_min() uint16_t AP_MotorsTri::get_motor_mask() { // tri copter uses channels 1,2,4 and 7 - return (1U << AP_MOTORS_MOT_1) | + return rc_map_mask(1U << AP_MOTORS_MOT_1) | (1U << AP_MOTORS_MOT_2) | (1U << AP_MOTORS_MOT_4) | (1U << AP_MOTORS_CH_TRI_YAW); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index e65e186ef5..0769347b72 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -81,6 +81,23 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) set frequency of a set of channels */ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) +{ + hal.rcout->set_freq(rc_map_mask(mask), freq_hz); +} + +void AP_Motors::rc_enable_ch(uint8_t chan) +{ + if (_motor_map_mask & (1U<enable_ch(chan); +} + +/* + map an internal motor mask to real motor mask + */ +uint32_t AP_Motors::rc_map_mask(uint32_t mask) const { uint32_t mask2 = 0; for (uint8_t i=0; i<32; i++) { @@ -94,14 +111,5 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) } } } - hal.rcout->set_freq(mask2, freq_hz); -} - -void AP_Motors::rc_enable_ch(uint8_t chan) -{ - if (_motor_map_mask & (1U<enable_ch(chan); + return mask2; } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index fff392a597..6b2664e270 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -132,6 +132,7 @@ protected: virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); virtual void rc_enable_ch(uint8_t chan); + virtual uint32_t rc_map_mask(uint32_t mask) const; // update the throttle input filter virtual void update_throttle_filter() = 0; From a07a8aece1955bd4ce48617ab31c79e89b3e1aac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 21:58:26 +1100 Subject: [PATCH 014/109] RC_Channel: added pwm_to_angle_dz_trim() --- libraries/RC_Channel/RC_Channel.cpp | 16 +++++++++++++--- libraries/RC_Channel/RC_Channel.h | 2 ++ 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 68d3cf19ab..689033b711 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -312,10 +312,10 @@ RC_Channel::update_min_max() the current radio_in value using the specified dead_zone */ int16_t -RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) +RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim) { - int16_t radio_trim_high = radio_trim + dead_zone; - int16_t radio_trim_low = radio_trim - dead_zone; + int16_t radio_trim_high = _trim + dead_zone; + int16_t radio_trim_low = _trim - dead_zone; // prevent div by 0 if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0) @@ -330,6 +330,16 @@ RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) return 0; } +/* + return an "angle in centidegrees" (normally -4500 to 4500) from + the current radio_in value using the specified dead_zone + */ +int16_t +RC_Channel::pwm_to_angle_dz(uint16_t dead_zone) +{ + return pwm_to_angle_dz_trim(dead_zone, radio_trim); +} + /* return an "angle in centidegrees" (normally -4500 to 4500) from the current radio_in value diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index af5856882e..ff010dbae6 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -62,6 +62,7 @@ public: void set_reverse(bool reverse); bool get_reverse(void) const; void set_default_dead_zone(int16_t dzone); + uint16_t get_dead_zone(void) const { return _dead_zone; } // get the channel number uint8_t get_ch_out(void) const { return _ch_out; }; @@ -105,6 +106,7 @@ public: // includes offset from PWM //int16_t get_radio_out(void); + int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim); int16_t pwm_to_angle_dz(uint16_t dead_zone); int16_t pwm_to_angle(); From 9188670e03539fcded193894a70c34971fd053a2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Dec 2015 14:07:06 +1100 Subject: [PATCH 015/109] AP_TECS: added get_height_rate_demand() call --- libraries/AP_TECS/AP_TECS.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index be7603bc39..5607bdcaa8 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -87,6 +87,11 @@ public: return _land_sink; } + // return height rate demand, in m/s + float get_height_rate_demand(void) const { + return _hgt_rate_dem; + } + // this supports the TECS_* user settable parameters static const struct AP_Param::GroupInfo var_info[]; From a6c39dee84fc7ac0ba88fc002a9fdf4d961ef794 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 08:33:33 +1100 Subject: [PATCH 016/109] AP_NavEKF: don't allocate EKF1 unless it will leave 4k free memory --- libraries/AP_NavEKF/AP_NavEKF.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a2c631a94e..0a3c97ebdf 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -421,6 +421,11 @@ bool NavEKF::InitialiseFilterDynamic(void) return false; } if (core == nullptr) { + if (hal.util->available_memory() < 4096 + sizeof(*core)) { + _enable.set(0); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF: not enough memory"); + return false; + } core = new NavEKF_core(*this, _ahrs, _baro, _rng); if (core == nullptr) { _enable.set(0); From f8b52c6a672268c709fdf8b73b4a8d243512d804 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 08:34:32 +1100 Subject: [PATCH 017/109] AP_AHRS: try to start EKF2 slightly before EKF2 this gives priority to EKF2 on memory --- libraries/AP_AHRS/AP_AHRS.h | 5 +++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e23ce96830..0d5ad7dcc6 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -412,6 +412,11 @@ public: // time that the AHRS has been up virtual uint32_t uptime_ms(void) const = 0; + // get the selected ekf type, for allocation decisions + int8_t get_ekf_type(void) const { + return _ekf_type; + } + protected: AHRS_VehicleClass _vehicle_class; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 06abbeed65..bd3c5e6088 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -109,7 +109,8 @@ void AP_AHRS_NavEKF::update_EKF1(void) if (start_time_ms == 0) { start_time_ms = AP_HAL::millis(); } - if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { + // slight extra delay on EKF1 to prioritise EKF2 for memory + if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100) { ekf1_started = EKF1.InitialiseFilterDynamic(); } } From 5f4bd10477eb0392db42fc6fe16ee88285802f93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:09:09 +1100 Subject: [PATCH 018/109] AP_HAL: added get_custom_defaults_file() to Util --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/Util.h | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 7a3988743f..8064742bc6 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -417,4 +417,8 @@ #define HAL_OS_POSIX_IO 0 #endif +#ifndef HAL_PARAM_DEFAULTS_PATH +#define HAL_PARAM_DEFAULTS_PATH NULL +#endif + #endif // __AP_HAL_BOARDS_H__ diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index fd1292e615..30c35a6d54 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -23,6 +23,11 @@ public: virtual const char* get_custom_log_directory() { return NULL; } virtual const char* get_custom_terrain_directory() const { return NULL; } + // get path to custom defaults file for AP_Param + virtual const char* get_custom_defaults_file() const { + return HAL_PARAM_DEFAULTS_PATH; + } + // run a debug shall on the given stream if possible. This is used // to support dropping into a debug shell to run firmware upgrade // commands From b4cc3d9668301b02fe27f2f10bb5c22c7ae90085 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:09:40 +1100 Subject: [PATCH 019/109] HAL_SITL: implement get_custom_defaults_file() --- libraries/AP_HAL_SITL/HAL_SITL_Class.cpp | 2 +- libraries/AP_HAL_SITL/SITL_State.h | 3 +++ libraries/AP_HAL_SITL/SITL_cmdline.cpp | 6 ++++++ libraries/AP_HAL_SITL/Util.h | 10 ++++++++++ 4 files changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index 8838a57907..360a00461c 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -43,7 +43,7 @@ static SITLUARTDriver sitlUart2Driver(2, &sitlState); static SITLUARTDriver sitlUart3Driver(3, &sitlState); static SITLUARTDriver sitlUart4Driver(4, &sitlState); -static SITLUtil utilInstance; +static SITLUtil utilInstance(&sitlState); HAL_SITL::HAL_SITL() : AP_HAL::HAL( diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index d79ba5c553..0fbfde1200 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -31,6 +31,7 @@ class HAL_SITL; class HALSITL::SITL_State { friend class HALSITL::SITLScheduler; + friend class HALSITL::SITLUtil; public: void init(int argc, char * const argv[]); @@ -213,6 +214,8 @@ private: // TCP address to connect uartC to const char *_client_address; + + const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index e4ea456846..9d71db17d4 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -55,6 +55,7 @@ void SITL_State::_usage(void) "\t--uartC device set device string for UARTC\n" "\t--uartD device set device string for UARTD\n" "\t--uartE device set device string for UARTE\n" + "\t--defaults path set path to defaults file\n" ); } @@ -120,6 +121,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_UARTD, CMDLINE_UARTE, CMDLINE_ADSB, + CMDLINE_DEFAULTS }; const struct GetOptLong::option options[] = { @@ -142,6 +144,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) {"gimbal", false, 0, CMDLINE_GIMBAL}, {"adsb", false, 0, CMDLINE_ADSB}, {"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR}, + {"defaults", true, 0, CMDLINE_DEFAULTS}, {0, false, 0, 0} }; @@ -197,6 +200,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case CMDLINE_AUTOTESTDIR: autotest_dir = strdup(gopt.optarg); break; + case CMDLINE_DEFAULTS: + defaults_path = strdup(gopt.optarg); + break; case CMDLINE_UARTA: case CMDLINE_UARTB: diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index 22040f2c7c..c8bcbb0b41 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -8,6 +8,9 @@ class HALSITL::SITLUtil : public AP_HAL::Util { public: + SITLUtil(SITL_State *_sitlState) : + sitlState(_sitlState) {} + bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } @@ -22,6 +25,13 @@ public: // create a new semaphore AP_HAL::Semaphore *new_semaphore(void) override { return new HALSITL::Semaphore; } + + // get path to custom defaults file for AP_Param + const char* get_custom_defaults_file() const override { + return sitlState->defaults_path; + } +private: + SITL_State *sitlState; }; #endif // __AP_HAL_SITL_UTIL_H__ From 415d800957d77b28646053c3ba46c13365792167 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:09:54 +1100 Subject: [PATCH 020/109] AP_Param: use get_custom_defaults_file() --- libraries/AP_Param/AP_Param.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index b8e733bfb4..005d95d6ef 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1081,9 +1081,7 @@ bool AP_Param::load_all(void) if the HAL specifies a defaults parameter file then override defaults using that file */ -#ifdef HAL_PARAM_DEFAULTS_PATH - load_defaults_file(HAL_PARAM_DEFAULTS_PATH); -#endif + load_defaults_file(hal.util->get_custom_defaults_file()); while (ofs < _storage.size()) { _storage.read_block(&phdr, ofs, sizeof(phdr)); @@ -1553,6 +1551,9 @@ bool AP_Param::parse_param_line(char *line, char **vname, float &value) */ bool AP_Param::load_defaults_file(const char *filename) { + if (filename == nullptr) { + return false; + } FILE *f = fopen(filename, "r"); if (f == NULL) { return false; From 0d26e3a4454fd354abdbbca62299a107c1f39e59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:10:20 +1100 Subject: [PATCH 021/109] autotest: automatically use sensible defaults for each vehicle type this make first time use of SITL simpler --- Tools/autotest/sim_vehicle.sh | 36 +++++++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh index 4ac1418f81..d01595561d 100755 --- a/Tools/autotest/sim_vehicle.sh +++ b/Tools/autotest/sim_vehicle.sh @@ -27,6 +27,7 @@ MODEL="" BREAKPOINT="" OVERRIDE_BUILD_TARGET="" DELAY_START=0 +DEFAULTS_PATH="" usage() { @@ -232,42 +233,55 @@ EOF } +autotest="../Tools/autotest" +[ -d "$autotest" ] || { + # we are not running from one of the standard vehicle directories. Use + # the location of the sim_vehicle.sh script to find the path + autotest=$(dirname $(readlink -e $0)) +} + # modify build target based on copter frame type case $FRAME in +|quad) BUILD_TARGET="sitl" MODEL="+" + DEFAULTS_PATH="$autotest/copter_params.parm" ;; X) BUILD_TARGET="sitl" EXTRA_PARM="param set FRAME 1;" MODEL="X" + DEFAULTS_PATH="$autotest/copter_params.parm" ;; octa*) BUILD_TARGET="sitl-octa" MODEL="$FRAME" + DEFAULTS_PATH="$autotest/copter_params.parm" ;; heli*) BUILD_TARGET="sitl-heli" MODEL="$FRAME" + DEFAULTS_PATH="$autotest/Helicopter.parm" ;; heli-dual) - BUILD_TARGET="sitl-heli-dual" + BUILD_TARGET="sitl-heli-dual" EXTRA_SIM="$EXTRA_SIM --frame=heli-dual" MODEL="heli-dual" - ;; + ;; heli-compound) - BUILD_TARGET="sitl-heli-compound" + BUILD_TARGET="sitl-heli-compound" EXTRA_SIM="$EXTRA_SIM --frame=heli-compound" MODEL="heli-compound" - ;; + ;; IrisRos) BUILD_TARGET="sitl" + DEFAULTS_PATH="$autotest/copter_params.parm" ;; Gazebo) BUILD_TARGET="sitl" EXTRA_SIM="$EXTRA_SIM --frame=Gazebo" MODEL="$FRAME" + DEFAULTS_PATH="$autotest/copter_params.parm" ;; CRRCSim|last_letter*) BUILD_TARGET="sitl" @@ -277,14 +291,17 @@ case $FRAME in BUILD_TARGET="sitl" MODEL="$FRAME" check_jsbsim_version + DEFAULTS_PATH="$autotest/ArduPlane.parm" ;; quadplane*) BUILD_TARGET="sitl" MODEL="$FRAME" + DEFAULTS_PATH="$autotest/quadplane.parm" ;; *-heli) BUILD_TARGET="sitl-heli" MODEL="$FRAME" + DEFAULTS_PATH="$autotest/Helicopter.parm" ;; *) MODEL="$FRAME" @@ -299,12 +316,6 @@ if [ -n "$OVERRIDE_BUILD_TARGET" ]; then BUILD_TARGET="$OVERRIDE_BUILD_TARGET" fi -autotest="../Tools/autotest" -[ -d "$autotest" ] || { - # we are not running from one of the standard vehicle directories. Use - # the location of the sim_vehicle.sh script to find the path - autotest=$(dirname $(readlink -e $0)) -} VEHICLEDIR="$autotest/../../$VEHICLE" [ -d "$VEHICLEDIR" ] || { VEHICLEDIR=$(dirname $(readlink -e $VEHICLEDIR)) @@ -400,6 +411,11 @@ if [ $USE_MAVLINK_GIMBAL == 1 ]; then cmd="$cmd --gimbal" fi +if [ -n "$DEFAULTS_PATH" ]; then + echo "Using defaults from $DEFAULTS_PATH" + cmd="$cmd --defaults $DEFAULTS_PATH" +fi + if [ $START_HIL == 0 ]; then if [ $USE_VALGRIND == 1 ]; then echo "Using valgrind" From f201552f6ddc8d0ee374dc861d29e852408583c5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:32:22 +1100 Subject: [PATCH 022/109] Plane: always call load_all this ensures defaults file works on first start after erase --- ArduPlane/Parameters.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index ca436338a1..215fa8d5b2 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1282,11 +1282,11 @@ void Plane::load_parameters(void) // save the current format version g.format_version.set_and_save(Parameters::k_format_version); cliSerial->println("done."); - } else { - uint32_t before = micros(); - // Load all auto-loaded EEPROM variables - AP_Param::load_all(); - AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); - cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } + + uint32_t before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); + cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } From 907a680a3b8868884cbcdbe3542b3d5be3d31dce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:32:27 +1100 Subject: [PATCH 023/109] Copter: always call load_all this ensures defaults file works on first start after erase --- ArduCopter/Parameters.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 99c0504019..075d486bf9 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1145,11 +1145,11 @@ void Copter::load_parameters(void) // save the current format version g.format_version.set_and_save(Parameters::k_format_version); cliSerial->println("done."); - } else { - uint32_t before = micros(); - // Load all auto-loaded EEPROM variables - AP_Param::load_all(); - AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); - cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } + + uint32_t before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); + cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before)); } From f17ec284f7c9e8d9282fba821f8f7786f0346829 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:32:32 +1100 Subject: [PATCH 024/109] Tracker: always call load_all this ensures defaults file works on first start after erase --- AntennaTracker/Parameters.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index e9339fc0ad..cc8cd3673d 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -302,10 +302,10 @@ void Tracker::load_parameters(void) // save the current format version g.format_version.set_and_save(Parameters::k_format_version); hal.console->println("done."); - } else { - uint32_t before = AP_HAL::micros(); - // Load all auto-loaded EEPROM variables - AP_Param::load_all(); - hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before)); } + + uint32_t before = AP_HAL::micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before)); } From be456fc33dc16e1be7646cb969e0e8b86a58fbd5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:32:36 +1100 Subject: [PATCH 025/109] Rover: always call load_all this ensures defaults file works on first start after erase --- APMrover2/Parameters.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index eae1922b20..0732fce4c8 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -582,13 +582,13 @@ void Rover::load_parameters(void) // save the current format version g.format_version.set_and_save(Parameters::k_format_version); cliSerial->println("done."); - } else { - unsigned long before = micros(); - // Load all auto-loaded EEPROM variables - AP_Param::load_all(); + } - cliSerial->printf("load_all took %luus\n", micros() - before); - } + unsigned long before = micros(); + // Load all auto-loaded EEPROM variables + AP_Param::load_all(); + + cliSerial->printf("load_all took %luus\n", micros() - before); // set a more reasonable default NAVL1_PERIOD for rovers L1_controller.set_default_period(8); From 70a45680ede61ba3c52d42bd5bb01c9580b3a208 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 6 Jan 2016 12:13:07 -0800 Subject: [PATCH 026/109] AP_TECS: fixed param desc for TECS_LAND_THR --- libraries/AP_TECS/AP_TECS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 97d80904ae..320c941817 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -122,7 +122,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: LAND_THR // @DisplayName: Cruise throttle during landing approach (percentage) - // @Description: Use this parameter instead of LAND_ASPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If it is negative if TECS_LAND_ASPD is in use then this value is not used during landing. + // @Description: Use this parameter instead of LAND_ARSPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If this value is negative then it is disabled and TECS_LAND_ARSPD is used instead. // @Range: -1 100 // @Increment: 0.1 // @User: User From e7d7ab5ef6791eb2757528e797861efa898e524a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 09:41:45 +1100 Subject: [PATCH 027/109] autotest: no need to forceload parms now --- Tools/autotest/sim_vehicle.sh | 3 --- 1 file changed, 3 deletions(-) diff --git a/Tools/autotest/sim_vehicle.sh b/Tools/autotest/sim_vehicle.sh index d01595561d..89839adffa 100755 --- a/Tools/autotest/sim_vehicle.sh +++ b/Tools/autotest/sim_vehicle.sh @@ -453,9 +453,6 @@ options="$options --out 10.0.2.2:14550" fi options="$options --out 127.0.0.1:14550 --out 127.0.0.1:14551" extra_cmd1="" -if [ $WIPE_EEPROM == 1 ]; then - extra_cmd="param forceload $autotest/$PARMS; $EXTRA_PARM; param fetch" -fi if [ $START_ANTENNA_TRACKER == 1 ]; then options="$options --load-module=tracker" extra_cmd="$extra_cmd module load map; tracker set port $TRACKER_UARTA; tracker start;" From 5692ed629a9e903742c2bea026d0a3da820e26cb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 11:34:04 +1100 Subject: [PATCH 028/109] PX4Firmware: submodule update (smbus battery update) --- modules/PX4Firmware | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/PX4Firmware b/modules/PX4Firmware index 8048e54259..c1323c55fa 160000 --- a/modules/PX4Firmware +++ b/modules/PX4Firmware @@ -1 +1 @@ -Subproject commit 8048e542599a89acd72fff2fce831a8012201a96 +Subproject commit c1323c55fa8e2657144a1f2202d7c0492b8acde3 From 67c3cf3d9b3ba8e3426fdb01daae77c520e42e9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 13:48:59 +1100 Subject: [PATCH 029/109] autotest: fixed incorrect default parms for plane --- Tools/autotest/ArduPlane.parm | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/autotest/ArduPlane.parm b/Tools/autotest/ArduPlane.parm index eeb43b1e96..7a580ede53 100644 --- a/Tools/autotest/ArduPlane.parm +++ b/Tools/autotest/ArduPlane.parm @@ -1,5 +1,4 @@ EK2_ENABLE 1 -AHRS_EKF_USE 1 ALT_CTRL_ALG 2 BATT_MONITOR 4 LOG_BITMASK 65535 @@ -66,7 +65,6 @@ NAVL1_PERIOD 15 ACRO_LOCKING 1 ELEVON_OUTPUT 0 VTAIL_OUTPUT 0 -SKIP_GYRO_CAL 1 # we need small INS_ACC offsets so INS is recognised as being calibrated INS_ACCOFFS_X 0.001 INS_ACCOFFS_Y 0.001 @@ -80,3 +78,4 @@ INS_ACC2OFFS_Z 0.001 INS_ACC2SCAL_X 1.001 INS_ACC2SCAL_Y 1.001 INS_ACC2SCAL_Z 1.001 +INS_GYR_CAL 0 From f6b909d0a5feeb991a8b9c9451788c90b7012e2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 11:57:00 +0900 Subject: [PATCH 030/109] AHRS_NavEKF: resolve compiler warning re signed vs unsigned comparison --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index bd3c5e6088..bbbf6a9b64 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -110,7 +110,7 @@ void AP_AHRS_NavEKF::update_EKF1(void) start_time_ms = AP_HAL::millis(); } // slight extra delay on EKF1 to prioritise EKF2 for memory - if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100) { + if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U) { ekf1_started = EKF1.InitialiseFilterDynamic(); } } From 6c458b07cbdde4b5591ad749fdcce7d926669073 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 12:25:26 +0900 Subject: [PATCH 031/109] MotorsHeli_RSC: resolve compiler warning re init order --- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 9808a1c642..f4f7c876d0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -27,10 +27,10 @@ class AP_MotorsHeli_RSC { public: AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn, uint8_t default_channel, - uint16_t loop_rate) : + uint16_t loop_rate) : + _loop_rate(loop_rate), _aux_fn(aux_fn), - _default_channel(default_channel), - _loop_rate(loop_rate) + _default_channel(default_channel) {}; // init_servo - servo initialization on start-up From 5ddd33227774a505d965a43fc425dc859167ef5c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 12:11:27 +0900 Subject: [PATCH 032/109] AC_AttControl: add ANGLE_BOOST param This allows disabling angle boost --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 7 +++++++ libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 +++ libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 4 ++++ 3 files changed, 14 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 430c22a15b..095c7faee1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -59,6 +59,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // IDs 8,9,10,11 RESERVED (in use on Solo) + // @Param: ANGLE_BOOST + // @DisplayName: Angle Boost + // @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1), + AP_GROUPEND }; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 773204767f..956fcd4547 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -293,6 +293,9 @@ protected: // Enable/Disable body frame rate feed forward AP_Int8 _rate_bf_ff_enabled; + // Enable/Disable angle boost + AP_Int8 _angle_boost_enabled; + // Intersampling period in seconds float _dt; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 993df984a9..073d79f41b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -16,6 +16,10 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const // throttle value should be 0 ~ 1000 float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in) { + if (!_angle_boost_enabled) { + return throttle_in; + _angle_boost = 0; + } // inverted_factor is 1 for tilt angles below 60 degrees // reduces as a function of angle beyond 60 degrees // becomes zero at 90 degrees From 15e673f5ae4b4caa31ac5572f6e829aeaeffdc2a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 12:55:44 +0900 Subject: [PATCH 033/109] Copter: fix precland parameter description prefix Thanks to Thomas Stone for finding this No functional change --- ArduCopter/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 075d486bf9..ed609c2cea 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1056,7 +1056,7 @@ const AP_Param::Info Copter::var_info[] = { #endif #if PRECISION_LANDING == ENABLED - // @Group: PRECLAND_ + // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), #endif From bfa69ed5439a2fcd718de7f0f015772d7a20a6e8 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Mon, 4 Jan 2016 18:56:03 -0500 Subject: [PATCH 034/109] Copter: 3.3.3-rc1 release notes --- ArduCopter/ReleaseNotes.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index a6af61b105..b6283991e4 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,9 @@ APM:Copter Release Notes: ------------------------------------------------------------------ +Copter 3.3.3-rc1 4-Jan-2016 +Changes from 3.3.2 +1) Restrict mode changes in helicopter when rotor is not at speed +------------------------------------------------------------------ Copter 3.3.2 01-Dec-2015 / 3.3.2-rc2 18-Nov-2015 Changes from 3.3.2-rc1 1) Bug fix for desired climb rate initialisation that could lead to drop when entering AltHold, Loiter, PosHold From 85b4288990d98c86c619d38088a74cf442179ca7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 14:29:08 +0900 Subject: [PATCH 035/109] Copter: update 3.3.3-rc1 release notes --- ArduCopter/ReleaseNotes.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index b6283991e4..0b34b946b7 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -3,6 +3,8 @@ APM:Copter Release Notes: Copter 3.3.3-rc1 4-Jan-2016 Changes from 3.3.2 1) Restrict mode changes in helicopter when rotor is not at speed +2) add ATC_ANGLE_BOOST param to allow disabling angle boost for all flight modes +3) add LightWare range finder support ------------------------------------------------------------------ Copter 3.3.2 01-Dec-2015 / 3.3.2-rc2 18-Nov-2015 Changes from 3.3.2-rc1 From 8fdbb36827ca489df157984c4a8b444a7300a6e4 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 6 Jan 2016 17:11:12 -0800 Subject: [PATCH 036/109] AP_BattMonitor: add is_powering_off --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 4 ++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 4 ++++ libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp | 1 + 3 files changed, 9 insertions(+) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 35647883e8..b2d5c508ba 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -194,6 +194,10 @@ bool AP_BattMonitor::healthy(uint8_t instance) const { return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).healthy; } +bool AP_BattMonitor::is_powering_off(uint8_t instance) const { + return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).is_powering_off; +} + /// has_current - returns true if battery monitor instance provides current info bool AP_BattMonitor::has_current(uint8_t instance) const { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index ab9721b873..04a817f4cb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -48,6 +48,7 @@ public: struct BattMonitor_State { uint8_t instance; // the instance number of this monitor bool healthy; // battery monitor is communicating correctly + bool is_powering_off; // true if the battery is about to power off float voltage; // voltage in volts float current_amps; // current in amperes float current_total_mah; // total current draw since start-up @@ -70,6 +71,9 @@ public: bool healthy(uint8_t instance) const; bool healthy() const { return healthy(AP_BATT_PRIMARY_INSTANCE); } + bool is_powering_off(uint8_t instance) const; + bool is_powering_off() const { return is_powering_off(AP_BATT_PRIMARY_INSTANCE); } + /// has_current - returns true if battery monitor instance provides current info bool has_current(uint8_t instance) const; bool has_current() const { return has_current(AP_BATT_PRIMARY_INSTANCE); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp index f47745b657..6a79a0ee90 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp @@ -69,6 +69,7 @@ void AP_BattMonitor_SMBus_PX4::read() _state.last_time_micros = AP_HAL::micros(); _state.current_total_mah = batt_status.discharged_mah; _state.healthy = true; + _state.is_powering_off = batt_status.is_powering_off; // read capacity if ((_batt_fd >= 0) && !_capacity_updated) { From 3e4b931d18f94a56c5370735a9fbb3f7bb81d039 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jan 2016 12:50:02 -0800 Subject: [PATCH 037/109] CodeStyle: update xmlpretty.py --- Tools/CodeStyle/xmlpretty.py | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/Tools/CodeStyle/xmlpretty.py b/Tools/CodeStyle/xmlpretty.py index d315e57bc8..47e482db37 100755 --- a/Tools/CodeStyle/xmlpretty.py +++ b/Tools/CodeStyle/xmlpretty.py @@ -1,9 +1,20 @@ #!/usr/bin/python import xml.dom.minidom as minidom -from sys import exit, argv, stderr +from sys import exit, argv, stderr, stdout import re +import argparse -dom = minidom.parse(argv[1]) +parser = argparse.ArgumentParser(description="Format XML") +parser.add_argument('infile', nargs=1) +parser.add_argument('outfile', nargs='?') + +args = parser.parse_args() + +f = open(args.infile[0],'r') +text = f.read() +f.close() + +dom = minidom.parseString(text) def foreach_tree(doc, root, func, level=0): func(doc, root, level) @@ -57,5 +68,14 @@ foreach_tree(dom, dom.documentElement, strip_text_whitespace) foreach_tree(dom, dom.documentElement, auto_indent) foreach_tree(dom, dom.documentElement, auto_space) -print "" -print dom.documentElement.toxml() +if args.outfile is not None: + f = open(args.outfile, 'w') + f.truncate() +else: + f = stdout + +f.write("\n") +f.write(dom.documentElement.toxml()) +f.write("\n") + +f.close() From f299fa7b3dc969069df41b5c227389cad3a2cc25 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jan 2016 22:10:06 -0800 Subject: [PATCH 038/109] CodeStyle: xmlpretty.py considers nodes with only text children to be one-liners --- Tools/CodeStyle/xmlpretty.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Tools/CodeStyle/xmlpretty.py b/Tools/CodeStyle/xmlpretty.py index 47e482db37..4d1c9858fc 100755 --- a/Tools/CodeStyle/xmlpretty.py +++ b/Tools/CodeStyle/xmlpretty.py @@ -16,6 +16,13 @@ f.close() dom = minidom.parseString(text) +def contains_only_text(node): + childNodes = node.childNodes[:] + for child in childNodes: + if child.nodeType != child.TEXT_NODE: + return False + return True + def foreach_tree(doc, root, func, level=0): func(doc, root, level) @@ -47,7 +54,7 @@ def strip_text_completely(doc, node, level): node.unlink() def auto_indent(doc, node, level): - if level > 0 and node.parentNode.nodeName not in ("description", "field", "param", "include"): + if level > 0 and not contains_only_text(node.parentNode): node.parentNode.insertBefore(doc.createTextNode("\n%s" % (" "*4*level)), node) if node.nextSibling is None: node.parentNode.appendChild(doc.createTextNode("\n%s" % (" "*4*(level-1)))) From dcd16696a2539b4c472d2328d04fb4f365e95086 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jan 2016 22:52:20 -0800 Subject: [PATCH 039/109] Copter: refactor RTL to compute full path on initialization --- ArduCopter/Copter.cpp | 1 - ArduCopter/Copter.h | 13 +++++- ArduCopter/control_rtl.cpp | 96 ++++++++++++++++++++++---------------- 3 files changed, 66 insertions(+), 44 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 97eafc6137..41a6142486 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -49,7 +49,6 @@ Copter::Copter(void) : guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), - rtl_alt(0.0f), circle_pilot_yaw_override(false), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 90ccce8691..8490242fe6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -340,7 +340,15 @@ private: // RTL RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) bool rtl_state_complete; // set to true if the current state is completed - float rtl_alt; // altitude the vehicle is returning at + + struct { + // NEU w/ origin-relative altitude + Vector3f origin_point; + Vector3f climb_target; + Vector3f return_target; + Vector3f descent_target; + bool land; + } rtl_path; // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw @@ -802,7 +810,8 @@ private: void rtl_descent_run(); void rtl_land_start(); void rtl_land_run(); - float get_RTL_alt(); + void rtl_build_path(); + float rtl_compute_return_alt_above_origin(float rtl_return_dist); bool sport_init(bool ignore_checks); void sport_run(); bool stabilize_init(bool ignore_checks); diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index cf491f3e9d..8b7ccc2787 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -13,6 +13,7 @@ bool Copter::rtl_init(bool ignore_checks) { if (position_ok() || ignore_checks) { + rtl_build_path(); rtl_climb_start(); return true; }else{ @@ -34,10 +35,10 @@ void Copter::rtl_run() rtl_loiterathome_start(); break; case RTL_LoiterAtHome: - if (g.rtl_alt_final > 0 && !failsafe.radio) { - rtl_descent_start(); - }else{ + if (rtl_path.land || failsafe.radio) { rtl_land_start(); + }else{ + rtl_descent_start(); } break; case RTL_FinalDescent: @@ -79,7 +80,6 @@ void Copter::rtl_climb_start() { rtl_state = RTL_InitialClimb; rtl_state_complete = false; - rtl_alt = get_RTL_alt(); // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); @@ -89,22 +89,8 @@ void Copter::rtl_climb_start() wp_nav.set_speed_xy(g.rtl_speed_cms); } - // get horizontal stopping point - Vector3f destination; - wp_nav.get_wp_stopping_point_xy(destination); - -#if AC_RALLY == ENABLED - // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); - rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home - destination.z = pv_alt_above_origin(rally_point.alt); -#else - destination.z = pv_alt_above_origin(rtl_alt); -#endif - // set the destination - wp_nav.set_wp_destination(destination); + wp_nav.set_wp_destination(rtl_path.climb_target); wp_nav.set_fast_waypoint(true); // hold current yaw during initial climb @@ -117,19 +103,7 @@ void Copter::rtl_return_start() rtl_state = RTL_ReturnHome; rtl_state_complete = false; - // set target to above home/rally point -#if AC_RALLY == ENABLED - // rally_point will be the nearest rally point or home. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt); - rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home - Vector3f destination = pv_location_to_vector(rally_point); -#else - Vector3f destination = pv_location_to_vector(ahrs.get_home()); - destination.z = pv_alt_above_origin(rtl_alt); -#endif - - wp_nav.set_wp_destination(destination); + wp_nav.set_wp_destination(rtl_path.return_target); // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); @@ -313,14 +287,14 @@ void Copter::rtl_descent_run() wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller - pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt); + pos_control.set_alt_target_with_slew(rtl_path.descent_target.z, G_Dt); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude - rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f; + rtl_state_complete = fabsf(rtl_path.descent_target.z - inertial_nav.get_altitude()) < 20.0f; } // rtl_loiterathome_start - initialise controllers to loiter over home @@ -415,13 +389,46 @@ void Copter::rtl_land_run() rtl_state_complete = ap.land_complete; } -// get_RTL_alt - return altitude which vehicle should return home at -// altitude is in cm above home -float Copter::get_RTL_alt() +void Copter::rtl_build_path() +{ + // origin point is our stopping point + pos_control.get_stopping_point_xy(rtl_path.origin_point); + pos_control.get_stopping_point_z(rtl_path.origin_point); + + // set return target to nearest rally point or home position +#if AC_RALLY == ENABLED + Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0); + rtl_path.return_target = pv_location_to_vector(rally_point); +#else + rtl_path.return_target = pv_location_to_vector(ahrs.get_home()); +#endif + + Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point; + + float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y); + + // compute return altitude + rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist); + + // climb target is above our origin point at the return altitude + rtl_path.climb_target.x = rtl_path.origin_point.x; + rtl_path.climb_target.y = rtl_path.origin_point.y; + rtl_path.climb_target.z = rtl_path.return_target.z; + + // descent target is below return target at rtl_alt_final + rtl_path.descent_target.x = rtl_path.return_target.x; + rtl_path.descent_target.y = rtl_path.return_target.y; + rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final); + + // set land flag + rtl_path.land = g.rtl_alt_final <= 0; +} + +// return altitude in cm above origin at which vehicle should return home +float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist) { // maximum of current altitude + climb_min and rtl altitude - float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude); - ret = MAX(ret, RTL_ALT_MIN); + float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled @@ -430,6 +437,13 @@ float Copter::get_RTL_alt() } #endif - return ret; -} +#if AC_RALLY == ENABLED + // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes + Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt); + rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home + rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home + ret = rally_point.alt; +#endif + return pv_alt_above_origin(ret); +} From 3800c66f071cef8b7595fb776af9e6bb07b8a03a Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jan 2016 23:00:29 -0800 Subject: [PATCH 040/109] Copter: add RTL_CONE_SLOPE --- ArduCopter/Parameters.cpp | 8 ++++++++ ArduCopter/Parameters.h | 4 +++- ArduCopter/config.h | 12 ++++++++++++ ArduCopter/control_rtl.cpp | 4 ++++ 4 files changed, 27 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ed609c2cea..7d2f609b2c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -127,6 +127,14 @@ const AP_Param::Info Copter::var_info[] = { // @Increment: 1 // @User: Standard GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), + + // @Param: RTL_CONE_SLOPE + // @DisplayName: RTL cone slope + // @Description: Defines a cone above home which determines maximum climb + // @Range: 0.5 10.0 + // @Increment: .1 + // @User: Standard + GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE), // @Param: RTL_SPEED // @DisplayName: RTL speed diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c647d4482b..cf299cff23 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -217,7 +217,8 @@ public: // 135 : reserved for Solo until features merged with master // k_param_rtl_speed_cms = 135, - k_param_fs_batt_curr_rtl, // 136 + k_param_fs_batt_curr_rtl, + k_param_rtl_cone_slope, // 137 // // 140: Sensor parameters @@ -380,6 +381,7 @@ public: AP_Int16 rtl_altitude; AP_Int16 rtl_speed_cms; + AP_Float rtl_cone_slope; AP_Float sonar_gain; AP_Int8 failsafe_battery_enabled; // battery failsafe enabled diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9d76f8c36d..d340e314e0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -484,10 +484,22 @@ # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude #endif +#ifndef RTL_CONE_SLOPE + # define RTL_CONE_SLOPE 3.0f // slope of RTL cone. 0 = No cone +#endif + #ifndef RTL_ALT_MIN # define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) #endif +#ifndef RTL_ABS_MIN_CLIMB + # define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb +#endif + +#ifndef RTL_MIN_CONE_SLOPE + # define RTL_MIN_CONE_SLOPE 0.5f // absolute minimum initial climb +#endif + #ifndef RTL_CLIMB_MIN_DEFAULT # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL #endif diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 8b7ccc2787..ddc238e51d 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -430,6 +430,10 @@ float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist) // maximum of current altitude + climb_min and rtl altitude float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); + if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes + ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB))); + } + #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { From 2927d6b2dfb27f0b8de91f65e66a425d2af4bfd2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 17:21:31 +0900 Subject: [PATCH 041/109] Copter: add RTL_CONE_SLOPE param description values --- ArduCopter/Parameters.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 7d2f609b2c..d74fb2d391 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -133,6 +133,7 @@ const AP_Param::Info Copter::var_info[] = { // @Description: Defines a cone above home which determines maximum climb // @Range: 0.5 10.0 // @Increment: .1 + // @Values: 0:Disabled,1:Shallow,3:Steep // @User: Standard GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE), From e16cccf21846d65595ef345c91bb31404e442070 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 17:29:54 +0900 Subject: [PATCH 042/109] Copter: RTL config formatting fix No functional change --- ArduCopter/config.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d340e314e0..a1a65fef86 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -484,24 +484,24 @@ # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude #endif -#ifndef RTL_CONE_SLOPE - # define RTL_CONE_SLOPE 3.0f // slope of RTL cone. 0 = No cone -#endif - #ifndef RTL_ALT_MIN # define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) #endif +#ifndef RTL_CLIMB_MIN_DEFAULT + # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL +#endif + #ifndef RTL_ABS_MIN_CLIMB - # define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb + # 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 #endif #ifndef RTL_MIN_CONE_SLOPE - # define RTL_MIN_CONE_SLOPE 0.5f // absolute minimum initial climb -#endif - -#ifndef RTL_CLIMB_MIN_DEFAULT - # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL + # define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone #endif #ifndef RTL_LOITER_TIME From c829ec0c2c4f9c6e65791783006b679cd20aceb0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Jan 2016 17:35:18 +0900 Subject: [PATCH 043/109] AC_PrecLand: add missing parameter description Thanks to Thomas Stone for finding this --- libraries/AC_PrecLand/AC_PrecLand.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 90fb0cb845..9d61fe76b2 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -8,6 +8,7 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AC_PrecLand::var_info[] = { + // @Param: ENABLED // @DisplayName: Precision Land enabled/disabled and behaviour // @Description: Precision Land enabled/disabled and behaviour // @Values: 0:Disabled, 1:Enabled Always Land, 2:Enabled Strict From 321803919caee413ac0ba07204ac4bc89a5464f3 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Thu, 7 Jan 2016 10:20:22 -0200 Subject: [PATCH 044/109] Revert "AP_HAL_Linux: support PWM input for BH hat" This reverts commit 8cca0beba9e5ab83a376fee2e57e30911e33cfcf. The PWM input using the RPI DMA is causing trouble with RPI boards using PPMSUM. Let's revert it until the solution is found. We still leave the ifdef in RCInput for BH hat. --- libraries/AP_HAL_Linux/RCInput.cpp | 36 +---- libraries/AP_HAL_Linux/RCInput.h | 7 +- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 180 ++++++++++--------------- libraries/AP_HAL_Linux/RCInput_RPI.h | 35 ++--- 4 files changed, 89 insertions(+), 169 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 2bba7ad948..b29d0cad77 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -42,11 +42,6 @@ uint8_t RCInput::num_channels() return _num_channels; } -void RCInput::set_num_channels(uint8_t num) -{ - _num_channels = num; -} - uint16_t RCInput::read(uint8_t ch) { new_rc_input = false; @@ -317,18 +312,10 @@ reset: memset(&dsm_state, 0, sizeof(dsm_state)); } -void RCInput::_process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1) -{ - if (channel < _num_channels) { - _pwm_values[channel] = width_s1; // range: 700 ~ 2300 - new_rc_input = true; - } -} - /* process a RC input pulse of the given width */ -void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel) +void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) { #if 0 // useful for debugging @@ -340,23 +327,14 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t c fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1); } #endif + // treat as PPM-sum + _process_ppmsum_pulse(width_s0 + width_s1); - if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) { + // treat as SBUS + _process_sbus_pulse(width_s0, width_s1); - // treat as PPM-sum - _process_ppmsum_pulse(width_s0 + width_s1); - - // treat as SBUS - _process_sbus_pulse(width_s0, width_s1); - - // treat as DSM - _process_dsm_pulse(width_s0, width_s1); - - } else { - - // treat as PWM - _process_pwm_pulse(channel, width_s0, width_s1); - } + // treat as DSM + _process_dsm_pulse(width_s0, width_s1); } /* diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index b3f4eff47e..2ea32483d3 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -5,7 +5,6 @@ #include "AP_HAL_Linux.h" #define LINUX_RC_INPUT_NUM_CHANNELS 16 -#define LINUX_RC_INPUT_CHANNEL_INVALID (999) class Linux::RCInput : public AP_HAL::RCInput { public: @@ -18,8 +17,6 @@ public: virtual void init(); bool new_input(); uint8_t num_channels(); - void set_num_channels(uint8_t num); - uint16_t read(uint8_t ch); uint8_t read(uint16_t* periods, uint8_t len); @@ -36,8 +33,7 @@ public: protected: - void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1, - uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID); + void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1); void _update_periods(uint16_t *periods, uint8_t len); private: @@ -49,7 +45,6 @@ public: void _process_ppmsum_pulse(uint16_t width); void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1); void _process_dsm_pulse(uint16_t width_s0, uint16_t width_s1); - void _process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1); /* override state */ uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index 4919f02a09..5527d59721 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -30,30 +30,10 @@ #define RCIN_RPI_BUFFER_LENGTH 8 #define RCIN_RPI_SAMPLE_FREQ 500 #define RCIN_RPI_DMA_CHANNEL 0 +#define RCIN_RPI_MAX_COUNTER 1300 +#define PPM_INPUT_RPI RPI_GPIO_4 #define RCIN_RPI_MAX_SIZE_LINE 50 -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -// Same as the circle_buffer size -// See comments in "init_ctrl_data()" to understand values "2" -#define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2) -#define RCIN_RPI_SIG_HIGH 0 -#define RCIN_RPI_SIG_LOW 1 -// Each gpio stands for a rcinput channel, -// the first one in RcChnGpioTbl is channel 1 in receiver -static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { - RPI_GPIO_5, RPI_GPIO_6, RPI_GPIO_12, - RPI_GPIO_13, RPI_GPIO_19, RPI_GPIO_20, - RPI_GPIO_21, RPI_GPIO_26 -}; -#else -#define RCIN_RPI_MAX_COUNTER 1304 -#define RCIN_RPI_SIG_HIGH 1 -#define RCIN_RPI_SIG_LOW 0 -static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { - RPI_GPIO_4 -}; -#endif // CONFIG_HAL_BOARD_SUBTYPE - //Memory Addresses #define RCIN_RPI_RPI1_DMA_BASE 0x20007000 #define RCIN_RPI_RPI1_CLK_BASE 0x20101000 @@ -283,26 +263,26 @@ void RCInput_RPI::init_ctrl_data() phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; //Init dma control blocks. - /*We are transferring 8 bytes of GPIO register. Every 7th iteration we are - sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need - 7 * 8 + 8 = 64 bytes of buffer. Value 7 was selected specially to have a 64-byte "block" - TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64 - "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of - vitual pages for control blocks): for every 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO - sampling, 7 control blocks for setting frequency and 1 control block for sampling timer, so, - we need 7 + 7 + 1 = 15 control blocks. For integer value, we need 15 pages of control blocks. - Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control - blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. - So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO - and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer. + /*We are transferring 1 byte of GPIO register. Every 56th iteration we are + sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need + 56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block" + TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64 + "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of + vitual pages for control blocks): for every 56 iterations (64 bytes of buffer) we need 56 control blocks for GPIO + sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so, + we need 56 + 56 + 1 = 113 control blocks. For integer value, we need 113 pages of control blocks. + Each control block length is 32 bytes. In 113 pages we will have (113 * 4096 / 32) = 113 * 128 control + blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. + So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO + and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer. */ // fprintf(stderr, "ERROR SEARCH1\n"); uint32_t i = 0; - for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168 + for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344 { - //Transfer timer every 7th sample - if(i % 7 == 0) { + //Transfer timer every 56th sample + if(i % 56 == 0) { cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, @@ -316,16 +296,16 @@ void RCInput_RPI::init_ctrl_data() cbp += sizeof(dma_cb_t); } - // Transfer GPIO (8 byte) + // Transfer GPIO (1 byte) cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); - init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, - (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), - 8, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, + (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), + 1, + 0, + (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, cbp + sizeof(dma_cb_t) ) ); - - dest += 8; + + dest += 1; cbp += sizeof(dma_cb_t); // Delay (for setting sampling frequency) @@ -405,9 +385,15 @@ void RCInput_RPI::set_sigaction() //Initial setup of variables RCInput_RPI::RCInput_RPI(): + prev_tick(0), + delta_time(0), curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_pointer(0), - curr_channel(0) + curr_channel(0), + width_s0(0), + curr_signal(0), + last_signal(228), + state(RCIN_RPI_INITIAL_STATE) { #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 int version = 2; @@ -416,9 +402,9 @@ RCInput_RPI::RCInput_RPI(): #endif set_physical_addresses(version); - //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "15" + //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "113" circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version); - con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version); + con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version); } RCInput_RPI::~RCInput_RPI() @@ -442,15 +428,12 @@ void RCInput_RPI::init_registers() void RCInput_RPI::init() { - uint64_t signal_states(0); - + init_registers(); - - //Enable PPM or PWM input - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]); - rc_channels[i].enable_pin->mode(HAL_GPIO_INPUT); - } + + //Enable PPM input + enable_pin = hal.gpio->channel(PPM_INPUT_RPI); + enable_pin->mode(HAL_GPIO_INPUT); //Configuration set_sigaction(); @@ -463,17 +446,11 @@ void RCInput_RPI::init() //Reading first sample curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); + prev_tick = curr_tick; curr_pointer += 8; - signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].prev_tick = curr_tick; - rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH - : RCIN_RPI_SIG_LOW; - rc_channels[i].last_signal = rc_channels[i].curr_signal; - } - curr_pointer += 8; - - set_num_channels(RCIN_RPI_CHN_NUM); + curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; + last_signal = curr_signal; + curr_pointer++; } @@ -482,7 +459,6 @@ void RCInput_RPI::_timer_tick() { int j; void* x; - uint64_t signal_states(0); //Now we are getting address in which DMAC is writing at current moment dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]); @@ -500,7 +476,7 @@ void RCInput_RPI::_timer_tick() } //Processing ready bytes - for (;counter > 0x40;) { + for (;counter > 0x40;counter--) { //Is it timer samle? if (curr_pointer % (64) == 0) { curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); @@ -508,50 +484,36 @@ void RCInput_RPI::_timer_tick() counter-=8; } //Reading required bit - signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH - : RCIN_RPI_SIG_LOW; - - //If the signal changed - if (rc_channels[i].curr_signal != rc_channels[i].last_signal) { - rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick; - rc_channels[i].prev_tick = curr_tick; - switch (rc_channels[i].state) { - case RCIN_RPI_INITIAL_STATE: - rc_channels[i].state = RCIN_RPI_ZERO_STATE; - break; - case RCIN_RPI_ZERO_STATE: - if (rc_channels[i].curr_signal == 0) { - rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time; - rc_channels[i].state = RCIN_RPI_ONE_STATE; - break; - } - else { - break; - } - case RCIN_RPI_ONE_STATE: - if (rc_channels[i].curr_signal == 1) { - rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time; - rc_channels[i].state = RCIN_RPI_ZERO_STATE; -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH - _process_rc_pulse(rc_channels[i].width_s0, - rc_channels[i].width_s1, i); -#else - _process_rc_pulse(rc_channels[i].width_s0, - rc_channels[i].width_s1); -#endif // CONFIG_HAL_BOARD_SUBTYPE - break; - } - else{ - break; - } + curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; + //If the signal changed + if (curr_signal != last_signal) { + delta_time = curr_tick - prev_tick; + prev_tick = curr_tick; + switch (state) { + case RCIN_RPI_INITIAL_STATE: + state = RCIN_RPI_ZERO_STATE; + break; + case RCIN_RPI_ZERO_STATE: + if (curr_signal == 0) { + width_s0 = (uint16_t) delta_time; + state = RCIN_RPI_ONE_STATE; + break; } + else + break; + case RCIN_RPI_ONE_STATE: + if (curr_signal == 1) { + width_s1 = (uint16_t) delta_time; + state = RCIN_RPI_ZERO_STATE; + _process_rc_pulse(width_s0, width_s1); + break; + } + else + break; } - rc_channels[i].last_signal = rc_channels[i].curr_signal; } - curr_pointer += 8; - counter -= 8; + last_signal = curr_signal; + curr_pointer++; if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { curr_pointer = 0; } diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.h b/libraries/AP_HAL_Linux/RCInput_RPI.h index f4c5565eed..4e1e2a2747 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.h +++ b/libraries/AP_HAL_Linux/RCInput_RPI.h @@ -22,11 +22,6 @@ #include #include -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -#define RCIN_RPI_CHN_NUM 8 -#else -#define RCIN_RPI_CHN_NUM 1 -#endif enum state_t{ RCIN_RPI_INITIAL_STATE = -1, @@ -103,33 +98,23 @@ private: Memory_table *con_blocks; uint64_t curr_tick; + uint64_t prev_tick; + uint64_t delta_time; uint32_t curr_tick_inc; uint32_t curr_pointer; uint32_t curr_channel; uint32_t counter; - struct RcChannel { - RcChannel() : - prev_tick(0), delta_time(0), - width_s0(0), width_s1(0), - curr_signal(0), last_signal(0), - enable_pin(0), state(RCIN_RPI_INITIAL_STATE) - {} + uint16_t width_s0; + uint16_t width_s1; - uint64_t prev_tick; - uint64_t delta_time; - - uint16_t width_s0; - uint16_t width_s1; - - uint8_t curr_signal; - uint8_t last_signal; - - state_t state; - - AP_HAL::DigitalSource *enable_pin; - } rc_channels[RCIN_RPI_CHN_NUM]; + uint8_t curr_signal; + uint8_t last_signal; + + state_t state; + + AP_HAL::DigitalSource *enable_pin; void init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb); void* map_peripheral(uint32_t base, uint32_t len); From a803cfd1e881b0509e42ecfaece45f89437590bf Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Thu, 7 Jan 2016 10:24:46 -0200 Subject: [PATCH 045/109] AP_HAL_Linux: RCInput: use GPIO5 for PPMSUM in bhat --- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index 5527d59721..74ddeac4cd 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -31,7 +31,11 @@ #define RCIN_RPI_SAMPLE_FREQ 500 #define RCIN_RPI_DMA_CHANNEL 0 #define RCIN_RPI_MAX_COUNTER 1300 +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH +#define PPM_INPUT_RPI RPI_GPIO_5 +#else #define PPM_INPUT_RPI RPI_GPIO_4 +#endif #define RCIN_RPI_MAX_SIZE_LINE 50 //Memory Addresses From dee20a31f19338daa69ebd8695ff651ca4ba12f4 Mon Sep 17 00:00:00 2001 From: mirkix <“mirkix@gmail.com”> Date: Tue, 22 Dec 2015 22:14:15 +0100 Subject: [PATCH 046/109] AP_Notify: Add gps_num_sats --- libraries/AP_GPS/AP_GPS.cpp | 1 + libraries/AP_Notify/AP_Notify.h | 1 + 2 files changed, 2 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 06c4f1d6af..0ad035ea51 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -428,6 +428,7 @@ AP_GPS::update(void) // update notify with gps status. We always base this on the primary_instance AP_Notify::flags.gps_status = state[primary_instance].status; + AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats; } /* diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 965434f58e..d4f201181e 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -55,6 +55,7 @@ public: struct notify_flags_type { uint32_t initialising : 1; // 1 if initialising and copter should not be moved uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock + uint32_t gps_num_sats : 6; // number of sats uint32_t armed : 1; // 0 = disarmed, 1 = armed uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed uint32_t pre_arm_gps_check : 1; // 0 = failing pre-arm GPS checks, 1 = passed From 24575586243d4e7e2a7b202f143eb2d5e332d211 Mon Sep 17 00:00:00 2001 From: mirkix <“mirkix@gmail.com”> Date: Tue, 22 Dec 2015 22:16:19 +0100 Subject: [PATCH 047/109] AP_Notify: Add display support --- libraries/AP_Notify/AP_Notify.h | 2 + libraries/AP_Notify/Display.cpp | 450 ++++++++++++++++++++ libraries/AP_Notify/Display.h | 42 ++ libraries/AP_Notify/Display_SSD1306_I2C.cpp | 107 +++++ libraries/AP_Notify/Display_SSD1306_I2C.h | 21 + 5 files changed, 622 insertions(+) create mode 100644 libraries/AP_Notify/Display.cpp create mode 100644 libraries/AP_Notify/Display.h create mode 100644 libraries/AP_Notify/Display_SSD1306_I2C.cpp create mode 100644 libraries/AP_Notify/Display_SSD1306_I2C.h diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index d4f201181e..5be90dde0c 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -33,6 +33,8 @@ #include "VRBoard_LED.h" #include "OreoLED_PX4.h" #include "RCOutputRGBLed.h" +#include "Display.h" +#include "Display_SSD1306_I2C.h" #ifndef OREOLED_ENABLED # define OREOLED_ENABLED 0 // set to 1 to enable OreoLEDs diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp new file mode 100644 index 0000000000..ec5bc1e510 --- /dev/null +++ b/libraries/AP_Notify/Display.cpp @@ -0,0 +1,450 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* Notify display driver for 128 x 64 pixel displays */ + +#include "AP_Notify.h" +#include +#include "Display.h" + +static const uint8_t _font[] = { + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x3E, 0x5B, 0x4F, 0x5B, 0x3E, + 0x3E, 0x6B, 0x4F, 0x6B, 0x3E, + 0x1C, 0x3E, 0x7C, 0x3E, 0x1C, + 0x18, 0x3C, 0x7E, 0x3C, 0x18, + 0x1C, 0x57, 0x7D, 0x57, 0x1C, + 0x1C, 0x5E, 0x7F, 0x5E, 0x1C, + 0x00, 0x18, 0x3C, 0x18, 0x00, + 0xFF, 0xE7, 0xC3, 0xE7, 0xFF, + 0x00, 0x18, 0x24, 0x18, 0x00, + 0xFF, 0xE7, 0xDB, 0xE7, 0xFF, + 0x30, 0x48, 0x3A, 0x06, 0x0E, + 0x26, 0x29, 0x79, 0x29, 0x26, + 0x40, 0x7F, 0x05, 0x05, 0x07, + 0x40, 0x7F, 0x05, 0x25, 0x3F, + 0x5A, 0x3C, 0xE7, 0x3C, 0x5A, + 0x7F, 0x3E, 0x1C, 0x1C, 0x08, + 0x08, 0x1C, 0x1C, 0x3E, 0x7F, + 0x14, 0x22, 0x7F, 0x22, 0x14, + 0x5F, 0x5F, 0x00, 0x5F, 0x5F, + 0x06, 0x09, 0x7F, 0x01, 0x7F, + 0x00, 0x66, 0x89, 0x95, 0x6A, + 0x60, 0x60, 0x60, 0x60, 0x60, + 0x94, 0xA2, 0xFF, 0xA2, 0x94, + 0x08, 0x04, 0x7E, 0x04, 0x08, + 0x10, 0x20, 0x7E, 0x20, 0x10, + 0x08, 0x08, 0x2A, 0x1C, 0x08, + 0x08, 0x1C, 0x2A, 0x08, 0x08, + 0x1E, 0x10, 0x10, 0x10, 0x10, + 0x0C, 0x1E, 0x0C, 0x1E, 0x0C, + 0x30, 0x38, 0x3E, 0x38, 0x30, + 0x06, 0x0E, 0x3E, 0x0E, 0x06, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x5F, 0x00, 0x00, + 0x00, 0x07, 0x00, 0x07, 0x00, + 0x14, 0x7F, 0x14, 0x7F, 0x14, + 0x24, 0x2A, 0x7F, 0x2A, 0x12, + 0x23, 0x13, 0x08, 0x64, 0x62, + 0x36, 0x49, 0x56, 0x20, 0x50, + 0x00, 0x08, 0x07, 0x03, 0x00, + 0x00, 0x1C, 0x22, 0x41, 0x00, + 0x00, 0x41, 0x22, 0x1C, 0x00, + 0x2A, 0x1C, 0x7F, 0x1C, 0x2A, + 0x08, 0x08, 0x3E, 0x08, 0x08, + 0x00, 0x80, 0x70, 0x30, 0x00, + 0x08, 0x08, 0x08, 0x08, 0x08, + 0x00, 0x00, 0x60, 0x60, 0x00, + 0x20, 0x10, 0x08, 0x04, 0x02, + 0x3E, 0x51, 0x49, 0x45, 0x3E, + 0x00, 0x42, 0x7F, 0x40, 0x00, + 0x72, 0x49, 0x49, 0x49, 0x46, + 0x21, 0x41, 0x49, 0x4D, 0x33, + 0x18, 0x14, 0x12, 0x7F, 0x10, + 0x27, 0x45, 0x45, 0x45, 0x39, + 0x3C, 0x4A, 0x49, 0x49, 0x31, + 0x41, 0x21, 0x11, 0x09, 0x07, + 0x36, 0x49, 0x49, 0x49, 0x36, + 0x46, 0x49, 0x49, 0x29, 0x1E, + 0x00, 0x00, 0x14, 0x00, 0x00, + 0x00, 0x40, 0x34, 0x00, 0x00, + 0x00, 0x08, 0x14, 0x22, 0x41, + 0x14, 0x14, 0x14, 0x14, 0x14, + 0x00, 0x41, 0x22, 0x14, 0x08, + 0x02, 0x01, 0x59, 0x09, 0x06, + 0x3E, 0x41, 0x5D, 0x59, 0x4E, + 0x7C, 0x12, 0x11, 0x12, 0x7C, + 0x7F, 0x49, 0x49, 0x49, 0x36, + 0x3E, 0x41, 0x41, 0x41, 0x22, + 0x7F, 0x41, 0x41, 0x41, 0x3E, + 0x7F, 0x49, 0x49, 0x49, 0x41, + 0x7F, 0x09, 0x09, 0x09, 0x01, + 0x3E, 0x41, 0x41, 0x51, 0x73, + 0x7F, 0x08, 0x08, 0x08, 0x7F, + 0x00, 0x41, 0x7F, 0x41, 0x00, + 0x20, 0x40, 0x41, 0x3F, 0x01, + 0x7F, 0x08, 0x14, 0x22, 0x41, + 0x7F, 0x40, 0x40, 0x40, 0x40, + 0x7F, 0x02, 0x1C, 0x02, 0x7F, + 0x7F, 0x04, 0x08, 0x10, 0x7F, + 0x3E, 0x41, 0x41, 0x41, 0x3E, + 0x7F, 0x09, 0x09, 0x09, 0x06, + 0x3E, 0x41, 0x51, 0x21, 0x5E, + 0x7F, 0x09, 0x19, 0x29, 0x46, + 0x26, 0x49, 0x49, 0x49, 0x32, + 0x03, 0x01, 0x7F, 0x01, 0x03, + 0x3F, 0x40, 0x40, 0x40, 0x3F, + 0x1F, 0x20, 0x40, 0x20, 0x1F, + 0x3F, 0x40, 0x38, 0x40, 0x3F, + 0x63, 0x14, 0x08, 0x14, 0x63, + 0x03, 0x04, 0x78, 0x04, 0x03, + 0x61, 0x59, 0x49, 0x4D, 0x43, + 0x00, 0x7F, 0x41, 0x41, 0x41, + 0x02, 0x04, 0x08, 0x10, 0x20, + 0x00, 0x41, 0x41, 0x41, 0x7F, + 0x04, 0x02, 0x01, 0x02, 0x04, + 0x40, 0x40, 0x40, 0x40, 0x40, + 0x00, 0x03, 0x07, 0x08, 0x00, + 0x20, 0x54, 0x54, 0x78, 0x40, + 0x7F, 0x28, 0x44, 0x44, 0x38, + 0x38, 0x44, 0x44, 0x44, 0x28, + 0x38, 0x44, 0x44, 0x28, 0x7F, + 0x38, 0x54, 0x54, 0x54, 0x18, + 0x00, 0x08, 0x7E, 0x09, 0x02, + 0x18, 0xA4, 0xA4, 0x9C, 0x78, + 0x7F, 0x08, 0x04, 0x04, 0x78, + 0x00, 0x44, 0x7D, 0x40, 0x00, + 0x20, 0x40, 0x40, 0x3D, 0x00, + 0x7F, 0x10, 0x28, 0x44, 0x00, + 0x00, 0x41, 0x7F, 0x40, 0x00, + 0x7C, 0x04, 0x78, 0x04, 0x78, + 0x7C, 0x08, 0x04, 0x04, 0x78, + 0x38, 0x44, 0x44, 0x44, 0x38, + 0xFC, 0x18, 0x24, 0x24, 0x18, + 0x18, 0x24, 0x24, 0x18, 0xFC, + 0x7C, 0x08, 0x04, 0x04, 0x08, + 0x48, 0x54, 0x54, 0x54, 0x24, + 0x04, 0x04, 0x3F, 0x44, 0x24, + 0x3C, 0x40, 0x40, 0x20, 0x7C, + 0x1C, 0x20, 0x40, 0x20, 0x1C, + 0x3C, 0x40, 0x30, 0x40, 0x3C, + 0x44, 0x28, 0x10, 0x28, 0x44, + 0x4C, 0x90, 0x90, 0x90, 0x7C, + 0x44, 0x64, 0x54, 0x4C, 0x44, + 0x00, 0x08, 0x36, 0x41, 0x00, + 0x00, 0x00, 0x77, 0x00, 0x00, + 0x00, 0x41, 0x36, 0x08, 0x00, + 0x02, 0x01, 0x02, 0x04, 0x02, + 0x3C, 0x26, 0x23, 0x26, 0x3C, + 0x1E, 0xA1, 0xA1, 0x61, 0x12, + 0x3A, 0x40, 0x40, 0x20, 0x7A, + 0x38, 0x54, 0x54, 0x55, 0x59, + 0x21, 0x55, 0x55, 0x79, 0x41, + 0x22, 0x54, 0x54, 0x78, 0x42, + 0x21, 0x55, 0x54, 0x78, 0x40, + 0x20, 0x54, 0x55, 0x79, 0x40, + 0x0C, 0x1E, 0x52, 0x72, 0x12, + 0x39, 0x55, 0x55, 0x55, 0x59, + 0x39, 0x54, 0x54, 0x54, 0x59, + 0x39, 0x55, 0x54, 0x54, 0x58, + 0x00, 0x00, 0x45, 0x7C, 0x41, + 0x00, 0x02, 0x45, 0x7D, 0x42, + 0x00, 0x01, 0x45, 0x7C, 0x40, + 0x7D, 0x12, 0x11, 0x12, 0x7D, + 0xF0, 0x28, 0x25, 0x28, 0xF0, + 0x7C, 0x54, 0x55, 0x45, 0x00, + 0x20, 0x54, 0x54, 0x7C, 0x54, + 0x7C, 0x0A, 0x09, 0x7F, 0x49, + 0x32, 0x49, 0x49, 0x49, 0x32, + 0x3A, 0x44, 0x44, 0x44, 0x3A, + 0x32, 0x4A, 0x48, 0x48, 0x30, + 0x3A, 0x41, 0x41, 0x21, 0x7A, + 0x3A, 0x42, 0x40, 0x20, 0x78, + 0x00, 0x9D, 0xA0, 0xA0, 0x7D, + 0x3D, 0x42, 0x42, 0x42, 0x3D, + 0x3D, 0x40, 0x40, 0x40, 0x3D, + 0x3C, 0x24, 0xFF, 0x24, 0x24, + 0x48, 0x7E, 0x49, 0x43, 0x66, + 0x2B, 0x2F, 0xFC, 0x2F, 0x2B, + 0xFF, 0x09, 0x29, 0xF6, 0x20, + 0xC0, 0x88, 0x7E, 0x09, 0x03, + 0x20, 0x54, 0x54, 0x79, 0x41, + 0x00, 0x00, 0x44, 0x7D, 0x41, + 0x30, 0x48, 0x48, 0x4A, 0x32, + 0x38, 0x40, 0x40, 0x22, 0x7A, + 0x00, 0x7A, 0x0A, 0x0A, 0x72, + 0x7D, 0x0D, 0x19, 0x31, 0x7D, + 0x26, 0x29, 0x29, 0x2F, 0x28, + 0x26, 0x29, 0x29, 0x29, 0x26, + 0x30, 0x48, 0x4D, 0x40, 0x20, + 0x38, 0x08, 0x08, 0x08, 0x08, + 0x08, 0x08, 0x08, 0x08, 0x38, + 0x2F, 0x10, 0xC8, 0xAC, 0xBA, + 0x2F, 0x10, 0x28, 0x34, 0xFA, + 0x00, 0x00, 0x7B, 0x00, 0x00, + 0x08, 0x14, 0x2A, 0x14, 0x22, + 0x22, 0x14, 0x2A, 0x14, 0x08, + 0x55, 0x00, 0x55, 0x00, 0x55, + 0xAA, 0x55, 0xAA, 0x55, 0xAA, + 0xFF, 0x55, 0xFF, 0x55, 0xFF, + 0x00, 0x00, 0x00, 0xFF, 0x00, + 0x10, 0x10, 0x10, 0xFF, 0x00, + 0x14, 0x14, 0x14, 0xFF, 0x00, + 0x10, 0x10, 0xFF, 0x00, 0xFF, + 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x14, 0x14, 0x14, 0xFC, 0x00, + 0x14, 0x14, 0xF7, 0x00, 0xFF, + 0x00, 0x00, 0xFF, 0x00, 0xFF, + 0x14, 0x14, 0xF4, 0x04, 0xFC, + 0x14, 0x14, 0x17, 0x10, 0x1F, + 0x10, 0x10, 0x1F, 0x10, 0x1F, + 0x14, 0x14, 0x14, 0x1F, 0x00, + 0x10, 0x10, 0x10, 0xF0, 0x00, + 0x00, 0x00, 0x00, 0x1F, 0x10, + 0x10, 0x10, 0x10, 0x1F, 0x10, + 0x10, 0x10, 0x10, 0xF0, 0x10, + 0x00, 0x00, 0x00, 0xFF, 0x10, + 0x10, 0x10, 0x10, 0x10, 0x10, + 0x10, 0x10, 0x10, 0xFF, 0x10, + 0x00, 0x00, 0x00, 0xFF, 0x14, + 0x00, 0x00, 0xFF, 0x00, 0xFF, + 0x00, 0x00, 0x1F, 0x10, 0x17, + 0x00, 0x00, 0xFC, 0x04, 0xF4, + 0x14, 0x14, 0x17, 0x10, 0x17, + 0x14, 0x14, 0xF4, 0x04, 0xF4, + 0x00, 0x00, 0xFF, 0x00, 0xF7, + 0x14, 0x14, 0x14, 0x14, 0x14, + 0x14, 0x14, 0xF7, 0x00, 0xF7, + 0x14, 0x14, 0x14, 0x17, 0x14, + 0x10, 0x10, 0x1F, 0x10, 0x1F, + 0x14, 0x14, 0x14, 0xF4, 0x14, + 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x00, 0x00, 0x1F, 0x10, 0x1F, + 0x00, 0x00, 0x00, 0x1F, 0x14, + 0x00, 0x00, 0x00, 0xFC, 0x14, + 0x00, 0x00, 0xF0, 0x10, 0xF0, + 0x10, 0x10, 0xFF, 0x10, 0xFF, + 0x14, 0x14, 0x14, 0xFF, 0x14, + 0x10, 0x10, 0x10, 0x1F, 0x00, + 0x00, 0x00, 0x00, 0xF0, 0x10, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, + 0xFF, 0xFF, 0xFF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xFF, 0xFF, + 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, + 0x38, 0x44, 0x44, 0x38, 0x44, + 0xFC, 0x4A, 0x4A, 0x4A, 0x34, + 0x7E, 0x02, 0x02, 0x06, 0x06, + 0x02, 0x7E, 0x02, 0x7E, 0x02, + 0x63, 0x55, 0x49, 0x41, 0x63, + 0x38, 0x44, 0x44, 0x3C, 0x04, + 0x40, 0x7E, 0x20, 0x1E, 0x20, + 0x06, 0x02, 0x7E, 0x02, 0x02, + 0x99, 0xA5, 0xE7, 0xA5, 0x99, + 0x1C, 0x2A, 0x49, 0x2A, 0x1C, + 0x4C, 0x72, 0x01, 0x72, 0x4C, + 0x30, 0x4A, 0x4D, 0x4D, 0x30, + 0x30, 0x48, 0x78, 0x48, 0x30, + 0xBC, 0x62, 0x5A, 0x46, 0x3D, + 0x3E, 0x49, 0x49, 0x49, 0x00, + 0x7E, 0x01, 0x01, 0x01, 0x7E, + 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, + 0x44, 0x44, 0x5F, 0x44, 0x44, + 0x40, 0x51, 0x4A, 0x44, 0x40, + 0x40, 0x44, 0x4A, 0x51, 0x40, + 0x00, 0x00, 0xFF, 0x01, 0x03, + 0xE0, 0x80, 0xFF, 0x00, 0x00, + 0x08, 0x08, 0x6B, 0x6B, 0x08, + 0x36, 0x12, 0x36, 0x24, 0x36, + 0x06, 0x0F, 0x09, 0x0F, 0x06, + 0x00, 0x00, 0x18, 0x18, 0x00, + 0x00, 0x00, 0x10, 0x10, 0x00, + 0x30, 0x40, 0xFF, 0x01, 0x01, + 0x00, 0x1F, 0x01, 0x01, 0x1E, + 0x00, 0x19, 0x1D, 0x17, 0x12, + 0x00, 0x3C, 0x3C, 0x3C, 0x3C, + 0x00, 0x00, 0x00, 0x00, 0x00 +}; + +bool Display::init(void) +{ + memset(&_flags, 0, sizeof(_flags)); + + _healthy = hw_init(); + + if (!_healthy) { + return false; + } + + // update all on display + update_all(); + hw_update(); + + return true; +} + +void Display::update() +{ + static uint8_t timer = 0; + + // return immediately if not enabled + if (!_healthy) { + return; + } + + // max update frequency 2Hz + if (timer++ < 25) { + return; + } + + timer = 0; + + // check if status has changed + if (_flags.armed != AP_Notify::flags.armed) { + update_arm(); + _flags.armed = AP_Notify::flags.armed; + } + + if (_flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { + update_prearm(); + _flags.pre_arm_check = AP_Notify::flags.pre_arm_check; + } + + if (_flags.gps_status != AP_Notify::flags.gps_status) { + update_gps(); + _flags.gps_status = AP_Notify::flags.gps_status; + } + + if (_flags.gps_num_sats != AP_Notify::flags.gps_num_sats) { + update_gps_sats(); + _flags.gps_num_sats = AP_Notify::flags.gps_num_sats; + } + + if (_flags.ekf_bad != AP_Notify::flags.ekf_bad) { + update_ekf(); + _flags.ekf_bad = AP_Notify::flags.ekf_bad; + } + + // if somethings has changed, update display + if (_need_update) { + hw_update(); + _need_update = false; + } +} + +void Display::update_all() +{ + update_arm(); + update_prearm(); + update_gps(); + update_gps_sats(); + update_ekf(); +} + +void Display::draw_text(uint16_t x, uint16_t y, const char* c) +{ + while (*c != 0) { + draw_char(x, y, *c); + x += 7; + c++; + } +} + +void Display::draw_char(uint16_t x, uint16_t y, const char c) +{ + uint8_t line; + + // draw char to pixel + for (uint8_t i = 0; i < 6; i++) { + if (i == 5) { + line = 0; + } else { + line = _font[(c * 5) + i]; + } + + for (uint8_t j = 0; j < 8; j++) { + if (line & 1) { + set_pixel(x + i, y + j); + } else { + clear_pixel(x + i, y + j); + } + line >>= 1; + } + } + + // update display + _need_update = true; +} + +void Display::update_arm() +{ + if (AP_Notify::flags.armed) { + draw_text(COLUMN(0), ROW(0), ">>>>> ARMED! <<<<<"); + } else { + draw_text(COLUMN(0), ROW(0), " disarmed "); + } +} + +void Display::update_prearm() +{ + if (AP_Notify::flags.pre_arm_check) { + draw_text(COLUMN(0), ROW(2), "Prearm: passed "); + } else { + draw_text(COLUMN(0), ROW(2), "Prearm: failed "); + } +} + +void Display::update_gps() +{ + switch (AP_Notify::flags.gps_status) { + case AP_GPS::NO_GPS: + draw_text(COLUMN(0), ROW(3), "GPS: no GPS"); + break; + case AP_GPS::NO_FIX: + draw_text(COLUMN(0), ROW(3), "GPS: no fix"); + break; + case AP_GPS::GPS_OK_FIX_2D: + draw_text(COLUMN(0), ROW(3), "GPS: 2D "); + break; + case AP_GPS::GPS_OK_FIX_3D: + draw_text(COLUMN(0), ROW(3), "GPS: 3D "); + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + draw_text(COLUMN(0), ROW(3), "GPS: DGPS "); + break; + case AP_GPS::GPS_OK_FIX_3D_RTK: + draw_text(COLUMN(0), ROW(3), "GPS: RTK "); + break; + } +} + +void Display::update_gps_sats() +{ + draw_text(COLUMN(0), ROW(4), "Sats:"); + draw_char(COLUMN(8), ROW(4), (AP_Notify::flags.gps_num_sats / 10) + 48); + draw_char(COLUMN(9), ROW(4), (AP_Notify::flags.gps_num_sats % 10) + 48); +} + +void Display::update_ekf() +{ + if (AP_Notify::flags.ekf_bad) { + draw_text(COLUMN(0), ROW(5), "EKF: fail"); + } else { + draw_text(COLUMN(0), ROW(5), "EKF: ok "); + } +} + diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h new file mode 100644 index 0000000000..028cab107b --- /dev/null +++ b/libraries/AP_Notify/Display.h @@ -0,0 +1,42 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +#include "NotifyDevice.h" + +#define ROW(Y) ((Y * 11) + 2) +#define COLUMN(X) ((X * 7) + 1) + +class Display: public NotifyDevice { +public: + bool init(void); + void update(); + +protected: + virtual bool hw_init() = 0; + virtual bool hw_update() = 0; + virtual bool set_pixel(uint16_t x, uint16_t y) = 0; + virtual bool clear_pixel(uint16_t x, uint16_t y) = 0; + +private: + void draw_char(uint16_t x, uint16_t y, const char c); + void draw_text(uint16_t x, uint16_t y, const char *c); + void update_all(); + void update_arm(); + void update_prearm(); + void update_gps(); + void update_gps_sats(); + void update_ekf(); + + bool _healthy; + bool _need_update; + + struct display_state { + uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock + uint32_t gps_num_sats : 6; // number of sats + uint32_t armed : 1; // 0 = disarmed, 1 = armed + uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed + uint32_t ekf_bad : 1; // 1 if ekf is reporting problems + } _flags; +}; + diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp new file mode 100644 index 0000000000..b6881a7f47 --- /dev/null +++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp @@ -0,0 +1,107 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include + +#include "Display_SSD1306_I2C.h" + +static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +bool Display_SSD1306_I2C::hw_init() +{ + // display init sequence + uint8_t init_seq[] = {0xAE, 0xD5, 0x80, 0xA8, 0x3F, 0xD3, + 0x00, 0x40, 0x8D, 0x14, 0x20, 0x00, + 0xA1, 0xC8, 0xDA, 0x12, 0x81, 0xCF, + 0xD9, 0xF1, 0xDB, 0x40, 0xA4, 0xA6, + 0xAF, 0x21, 0, 127, 0x22, 0, 7 + }; + + // get pointer to i2c bus semaphore + AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); + + // take i2c bus sempahore + if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + + // disable recording of i2c lockup errors + hal.i2c->ignore_errors(true); + + // init display + hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(init_seq), init_seq); + + // re-enable recording of i2c lockup errors + hal.i2c->ignore_errors(false); + + // give back i2c semaphore + i2c_sem->give(); + + // clear display + hw_update(); + + return true; +} + +bool Display_SSD1306_I2C::hw_update() +{ + uint8_t command[] = {0x21, 0, 127, 0x22, 0, 7}; + + // get pointer to i2c bus semaphore + AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); + + // exit immediately if we can't take the semaphore + if (i2c_sem == NULL || !i2c_sem->take(5)) { + return false; + } + + // write buffer to display + for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) { + command[4] = i; + hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(command), command); + hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x40, SSD1306_ROWS, &_displaybuffer[i * SSD1306_ROWS]); + } + + // give back i2c semaphore + i2c_sem->give(); + + return true; +} + +bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y) +{ + // check x, y range + if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) { + return false; + } + // set pixel in buffer + _displaybuffer[x + (y / 8 * SSD1306_ROWS)] |= 1 << (y % 8); + + return true; +} + +bool Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y) +{ + // check x, y range + if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) { + return false; + } + // clear pixel in buffer + _displaybuffer[x + (y / 8 * SSD1306_ROWS)] &= ~(1 << (y % 8)); + + return true; +} diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h new file mode 100644 index 0000000000..431d7f2e57 --- /dev/null +++ b/libraries/AP_Notify/Display_SSD1306_I2C.h @@ -0,0 +1,21 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +#include "Display.h" + +#define SSD1306_I2C_ADDRESS 0x3C // default I2C bus address +#define SSD1306_ROWS 128 // display rows +#define SSD1306_COLUMNS 64 // display columns +#define SSD1306_COLUMNS_PER_PAGE 8 + +class Display_SSD1306_I2C: public Display { +public: + virtual bool hw_init(); + virtual bool hw_update(); + virtual bool set_pixel(uint16_t x, uint16_t y); + virtual bool clear_pixel(uint16_t x, uint16_t y); + +private: + uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE]; +}; From 1744bc085001dee1daf0f278e90669a25301e870 Mon Sep 17 00:00:00 2001 From: mirkix <“mirkix@gmail.com”> Date: Tue, 22 Dec 2015 22:16:59 +0100 Subject: [PATCH 048/109] AP_Notify: Use display with bbbmini --- libraries/AP_Notify/AP_Notify.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index ca68f17ad2..be3c6b8dc7 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -66,8 +66,8 @@ struct AP_Notify::notify_events_type AP_Notify::events; ToshibaLED_I2C toshibaled; NotifyDevice *AP_Notify::_devices[] = {&boardled, &navioled, &toshibaled}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - ToshibaLED_I2C toshibaled; - NotifyDevice *AP_Notify::_devices[] = {&toshibaled}; + Display_SSD1306_I2C display; + NotifyDevice *AP_Notify::_devices[] = {&display}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT ToshibaLED_I2C toshibaled; ToneAlarm_Linux tonealarm; From 2aa195e39b115fecc5867fec11d2e2e55b1c8b1c Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Wed, 6 Jan 2016 11:50:57 -0200 Subject: [PATCH 049/109] waf: update waf submodule to v1.8.18 --- modules/waf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/waf b/modules/waf index d2ade00ef8..878ab1e294 160000 --- a/modules/waf +++ b/modules/waf @@ -1 +1 @@ -Subproject commit d2ade00ef86f5024317ef48ae0e30cff51c8f3dd +Subproject commit 878ab1e29494e28236a260212a5021b1de35d9a3 From b54bb757df0e64a2f5e23c932fc4d2ec45a7d738 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Jan 2016 10:33:45 +1100 Subject: [PATCH 050/109] AP_Param: fixed flymaple build --- libraries/AP_Param/AP_Param.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 005d95d6ef..593dc2c2b7 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1077,11 +1077,13 @@ bool AP_Param::load_all(void) struct Param_header phdr; uint16_t ofs = sizeof(AP_Param::EEPROM_header); +#if HAL_OS_POSIX_IO == 1 /* if the HAL specifies a defaults parameter file then override defaults using that file */ load_defaults_file(hal.util->get_custom_defaults_file()); +#endif while (ofs < _storage.size()) { _storage.read_block(&phdr, ofs, sizeof(phdr)); From a42b286c01903358dce1b5ad4d35506888bf79d2 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Fri, 8 Jan 2016 05:46:39 +0800 Subject: [PATCH 051/109] AP_GPS_SBF: update logging based on vendor recommendation. --- libraries/AP_GPS/AP_GPS_SBF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 8a30759edb..368b6601ed 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -53,7 +53,7 @@ private: "srd, Moderate, UAV\n", "sem, PVT, 5\n", "spm, Rover, StandAlone+SBAS+DGPS+RTK\n", - "sso, Stream2, Dsk1, Rinex+Event+RawData, msec100\n"}; + "sso, Stream2, Dsk1, postprocess+event, msec100\n"}; uint32_t last_hdop = 999; uint32_t crc_error_counter = 0; From 8c7f36563cdc73d2a6129c71d632fc12a5275848 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jan 2016 10:47:20 +0900 Subject: [PATCH 052/109] AC_AttControl: bug fix to angle_boost reporting Thanks to OXINARF for finding this --- libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 073d79f41b..d841f6fb61 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -17,8 +17,8 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in) { if (!_angle_boost_enabled) { - return throttle_in; _angle_boost = 0; + return throttle_in; } // inverted_factor is 1 for tilt angles below 60 degrees // reduces as a function of angle beyond 60 degrees From 60bd7de2d7288db467dccfd94753cff028323560 Mon Sep 17 00:00:00 2001 From: Julien BERAUD Date: Thu, 7 Jan 2016 18:54:13 +0100 Subject: [PATCH 053/109] README: Add Julien BERAUD as bebop maintainer --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 211dd7bbb3..988bee7bb1 100644 --- a/README.md +++ b/README.md @@ -91,6 +91,8 @@ >> - ***Board***: VRBrain >> - [Mike McCauley](#) >> - ***Board***: Flymaple +>> - [Julien BERAUD](#) +>> - ***Board***: Bebop & Bebop 2 >> - [Jonathan Challinger] (https://github.com/3drobotics/ardupilot-solo) >> - ***Vehicle***: 3DRobotics Solo ArduPilot maintainer >> - [Gustavo José de Sousa](https://github.com/guludo) From d916413a15c11a35af4df09051d6d903261eff8d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 5 Jan 2016 16:23:16 -0800 Subject: [PATCH 054/109] Copter: add CAL_ALWAYS_REBOOT option --- ArduCopter/sensors.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index afcbdccc01..232d9fda8a 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -179,6 +179,12 @@ void Copter::compass_cal_update() if (!hal.util->get_soft_armed()) { compass.compass_cal_update(); } +#ifdef CAL_ALWAYS_REBOOT + if (compass.compass_cal_requires_reboot()) { + hal.scheduler->delay(1000); + hal.scheduler->reboot(false); + } +#endif } void Copter::accel_cal_update() @@ -192,6 +198,13 @@ void Copter::accel_cal_update() if(ins.get_new_trim(trim_roll, trim_pitch)) { ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } + +#ifdef CAL_ALWAYS_REBOOT + if (ins.accel_cal_requires_reboot()) { + hal.scheduler->delay(1000); + hal.scheduler->reboot(false); + } +#endif } #if EPM_ENABLED == ENABLED From e216f814c5524f50d8e9dd2e635569be5498ec3f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jan 2016 12:01:51 +0900 Subject: [PATCH 055/109] Copter: add CAL_ALWAYS_REBOOT to APM_Config.h --- ArduCopter/APM_Config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 4b5dddc7fb..b7db6c5ca7 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -40,6 +40,7 @@ //#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor //#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground) +//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) From 3918987507ff28feaf36843807ccb5d3b71405d9 Mon Sep 17 00:00:00 2001 From: Gavin Mogensen Date: Tue, 5 Jan 2016 12:54:05 +1000 Subject: [PATCH 056/109] Copter: aux switches for relays 2,3 and 4 --- ArduCopter/defines.h | 5 ++++- ArduCopter/switches.cpp | 15 +++++++++++++-- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 11083a46c8..c9320fbdec 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -65,7 +65,10 @@ enum aux_sw_func { AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch - AUXSW_BRAKE = 33 // Brake flight mode + AUXSW_BRAKE = 33, // Brake flight mode + AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34) + AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35) + AUXSW_RELAY4 = 36 // Relay4 pin on/off (in Mission planner set CH10_OPT = 36) }; // Frame types diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 152a756836..e7c1b0e985 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -235,7 +235,6 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_MISSION_RESET: case AUXSW_ATTCON_FEEDFWD: case AUXSW_ATTCON_ACCEL_LIM: - case AUXSW_RELAY: case AUXSW_LANDING_GEAR: case AUXSW_MOTOR_ESTOP: case AUXSW_MOTOR_INTERLOCK: @@ -521,7 +520,19 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); break; - case AUXSW_LANDING_GEAR: + case AUXSW_RELAY2: + ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); + break; + + case AUXSW_RELAY3: + ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); + break; + + case AUXSW_RELAY4: + ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); + break; + + case AUXSW_LANDING_GEAR: switch (ch_flag) { case AUX_SWITCH_LOW: landinggear.set_cmd_mode(LandingGear_Deploy); From 6a67ad706ce398aae97bf53a41a9c523bc0e5fef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jan 2016 12:12:29 +0900 Subject: [PATCH 057/109] Copter: add Relays to aux switch parameter descriptions Also fix formatting of new options in switches.cpp No functional change --- ArduCopter/Parameters.cpp | 12 ++++++------ ArduCopter/switches.cpp | 18 +++++++++--------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d74fb2d391..deb827963c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -403,42 +403,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: CH7_OPT // @DisplayName: Channel 7 option // @Description: Select which function if performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), // @Param: CH8_OPT // @DisplayName: Channel 8 option // @Description: Select which function if performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), // @Param: CH9_OPT // @DisplayName: Channel 9 option // @Description: Select which function if performed when CH9 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), // @Param: CH10_OPT // @DisplayName: Channel 10 option // @Description: Select which function if performed when CH10 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), // @Param: CH11_OPT // @DisplayName: Channel 11 option // @Description: Select which function if performed when CH11 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), // @Param: CH12_OPT // @DisplayName: Channel 12 option // @Description: Select which function if performed when CH12 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index e7c1b0e985..6d7a3e00ea 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -521,17 +521,17 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; case AUXSW_RELAY2: - ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); - break; + ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); + break; + + case AUXSW_RELAY3: + ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); + break; - case AUXSW_RELAY3: - ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); - break; - case AUXSW_RELAY4: - ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); - break; - + ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); + break; + case AUXSW_LANDING_GEAR: switch (ch_flag) { case AUX_SWITCH_LOW: From 6cb20b679d9c9d8bc4bbf6d05a16ab5ec3cd4090 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 5 Jan 2016 15:33:49 -0800 Subject: [PATCH 058/109] Plane: add xtrack_error to DF log --- ArduPlane/Log.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 223745280b..c877b2a608 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -280,9 +280,10 @@ struct PACKED log_Nav_Tuning { int16_t airspeed_cm; float altitude; uint32_t groundspeed_cm; + float xtrack_error; }; -// Write a navigation tuning packe +// Write a navigation tuning packet void Plane::Log_Write_Nav_Tuning() { struct log_Nav_Tuning pkt = { @@ -295,7 +296,8 @@ void Plane::Log_Write_Nav_Tuning() altitude_error_cm : (int16_t)altitude_error_cm, airspeed_cm : (int16_t)airspeed.get_airspeed_cm(), altitude : barometer.get_altitude(), - groundspeed_cm : (uint32_t)(gps.ground_speed()*100) + groundspeed_cm : (uint32_t)(gps.ground_speed()*100), + xtrack_error : nav_controller->crosstrack_error() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -483,7 +485,7 @@ static const struct LogStructure log_structure[] = { { LOG_CTUN_MSG, sizeof(log_Control_Tuning), "CTUN", "Qcccchhf", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" }, { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), - "NTUN", "QCfccccfI", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" }, + "NTUN", "QCfccccfIf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM,XT" }, { LOG_SONAR_MSG, sizeof(log_Sonar), "SONR", "QHfffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" }, { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), From 4280dacced741dfacbd88c38d166602e6b89c030 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 5 Jan 2016 16:18:29 -0800 Subject: [PATCH 059/109] Rover: add xtrack_error to DF log --- APMrover2/Log.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index e3357142d5..c1d1fead2f 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -256,9 +256,10 @@ struct PACKED log_Nav_Tuning { uint16_t target_bearing_cd; uint16_t nav_bearing_cd; int8_t throttle; + float xtrack_error; }; -// Write a navigation tuning packet. Total length : 18 bytes +// Write a navigation tuning packet void Rover::Log_Write_Nav_Tuning() { struct log_Nav_Tuning pkt = { @@ -268,7 +269,8 @@ void Rover::Log_Write_Nav_Tuning() wp_distance : wp_distance, target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), - throttle : (int8_t)(100 * channel_throttle->norm_output()) + throttle : (int8_t)(100 * channel_throttle->norm_output()), + xtrack_error : nav_controller->crosstrack_error() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -393,7 +395,7 @@ const LogStructure Rover::log_structure[] = { { LOG_CTUN_MSG, sizeof(log_Control_Tuning), "CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY" }, { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), - "NTUN", "QHfHHb", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr" }, + "NTUN", "QHfHHbf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT" }, { LOG_SONAR_MSG, sizeof(log_Sonar), "SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), From 8c9eafeb3c49a7b2b90c12fffc42bde8b67bb435 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 6 Jan 2016 21:43:23 -0700 Subject: [PATCH 060/109] AP_GPS: Fix param docs that had incorrect spaces The spaces between values aren't stirpped by the autodoc tool that generates the XML files --- libraries/AP_GPS/AP_GPS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0ad035ea51..0d49859a5d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: INJECT_TO // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets // @Description: The GGS can send raw serial packets to inject data to multiple GPSes. - // @Values: 0:send to first GPS, 1:send to 2nd GPS, 127:send to all + // @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL), // @Param: SBP_LOGMASK @@ -101,7 +101,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @DisplayName: GNSS system configuration // @Description: Bitmask for what GNSS system to use (all unchecked or zero to leave GPS as configured) // @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS - // @Bitmask: 0:GPS, 1:SBAS, 2:Galileo, 3:Beidou, 4:IMES, 5:QZSS, 6:GLOSNASS + // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS // @User: Advanced // @RebootRequired: True AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0), From b742ee9cfbfa0eddcaf104e5937265678b63de18 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jan 2016 14:44:00 +0900 Subject: [PATCH 061/109] AP_Motors: fix example sketch --- .../AP_Motors_test/AP_Motors_test.cpp | 51 ++++++++++++------- 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index 2b3d7eb5ba..56c2956e43 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -32,15 +32,22 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +// declare functions +void setup(); +void loop(); +void motor_order_test(); +void stability_test(); +void update_motors(); + RC_Channel rc1(0), rc2(1), rc3(2), rc4(3); // uncomment the row below depending upon what frame you are using -//AP_MotorsTri motors(rc1, rc2, rc3, rc4, 400); -AP_MotorsQuad motors(rc1, rc2, rc3, rc4, 400); -//AP_MotorsHexa motors(rc1, rc2, rc3, rc4, 400); -//AP_MotorsY6 motors(rc1, rc2, rc3, rc4, 400); -//AP_MotorsOcta motors(rc1, rc2, rc3, rc4, 400); -//AP_MotorsOctaQuad motors(rc1, rc2, rc3, rc4, 400); +//AP_MotorsTri motors(400); +AP_MotorsQuad motors(400); +//AP_MotorsHexa motors(400); +//AP_MotorsY6 motors(400); +//AP_MotorsOcta motors(400); +//AP_MotorsOctaQuad motors(400); //AP_MotorsHeli motors(rc1, rc2, rc3, rc4, 400); @@ -51,11 +58,12 @@ void setup() // motor initialisation motors.set_update_rate(490); - // motors.set_frame_orientation(AP_MOTORS_X_FRAME); - motors.set_frame_orientation(AP_MOTORS_PLUS_FRAME); - motors.set_min_throttle(130); + motors.set_frame_orientation(AP_MOTORS_X_FRAME); + motors.Init(); + motors.set_throttle_range(130,1000,2000); motors.set_hover_throttle(500); - motors.Init(); // initialise motors + motors.enable(); + motors.output_min(); // setup radio if (rc3.radio_min == 0) { @@ -72,9 +80,6 @@ void setup() rc3.set_range(130, 1000); rc4.set_angle(4500); - motors.enable(); - motors.output_min(); - hal.scheduler->delay(1000); } @@ -123,7 +128,7 @@ void motor_order_test() // stability_test void stability_test() { - int16_t value, roll_in, pitch_in, yaw_in, throttle_in, throttle_radio_in, avg_out; + int16_t roll_in, pitch_in, yaw_in, throttle_in, avg_out; int16_t testing_array[][4] = { // roll, pitch, yaw, throttle @@ -166,9 +171,11 @@ void stability_test() // arm motors motors.armed(true); + motors.set_interlock(true); + motors.set_stabilizing(true); // run stability test - for (int16_t i=0; i < testing_array_rows; i++) { + for (uint16_t i=0; iread(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output - hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n", + hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t AvgOut:%5d\n", (int)roll_in, (int)pitch_in, (int)yaw_in, @@ -192,7 +198,6 @@ void stability_test() (int)hal.rcout->read(1), (int)hal.rcout->read(2), (int)hal.rcout->read(3), - (int)throttle_radio_in, (int)avg_out); } // set all inputs to motor library to zero and disarm motors @@ -205,4 +210,12 @@ void stability_test() hal.console->println("finished test."); } +void update_motors() +{ + // call update motors 1000 times to get any ramp limiting completed + for (uint16_t i=0; i<1000; i++) { + motors.output(); + } +} + AP_HAL_MAIN(); From 76e3ae190aa25fa24108c7ee4577bad12f0582c4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Dec 2015 19:08:27 +1100 Subject: [PATCH 062/109] GCS_MAVLink: partial merge of common.xml from upstream --- .../message_definitions/common.xml | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index e4601f5cf9..87b757b997 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -1151,6 +1151,11 @@ Speed of the vertical rotation (in degrees per second) + + Request VTOL transition + The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used. + + @@ -1546,6 +1551,36 @@ Payload battery + + Enumeration of VTOL states + + MAV is not configured as VTOL + + + VTOL is in transition from multicopter to fixed-wing + + + VTOL is in transition from fixed-wing to multicopter + + + VTOL is in multicopter state + + + VTOL is in fixed-wing state + + + + Enumeration of landed detector states + + MAV landed state is unknown + + + MAV is landed (on ground) + + + MAV is in air + + Enumeration of the ADSB altimeter types From 7a7bd0607f04bf4c7d6535cebf043cd818d4419d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 19 Nov 2015 17:11:13 +1100 Subject: [PATCH 063/109] SITL: first version of FlightAxis SITL support --- libraries/SITL/SIM_Aircraft.cpp | 4 +- libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SIM_FlightAxis.cpp | 315 ++++++++++++++++++++++++++++++ libraries/SITL/SIM_FlightAxis.h | 161 +++++++++++++++ 4 files changed, 480 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_FlightAxis.cpp create mode 100644 libraries/SITL/SIM_FlightAxis.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c2eac05d63..fb24a9c119 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -133,7 +133,9 @@ void Aircraft::update_position(void) time_now_us += frame_time_us; } last_time_us = time_now_us; - sync_frame_time(); + if (use_time_sync) { + sync_frame_time(); + } } /* diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 8473657d00..9cd5f030f6 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -125,6 +125,7 @@ protected: uint8_t instance; const char *autotest_dir; const char *frame; + bool use_time_sync = true; bool on_ground(const Vector3f &pos) const; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp new file mode 100644 index 0000000000..e42226abd2 --- /dev/null +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -0,0 +1,315 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulator connector for FlightAxis +*/ + +#include "SIM_FlightAxis.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define FLIGHTAXIS_SERVER_IP "192.168.2.48" +#define FLIGHTAXIS_SERVER_PORT 18083 + +namespace SITL { + +// the asprintf() calls are not worth checking for SITL +#pragma GCC diagnostic ignored "-Wunused-result" + +FlightAxis::FlightAxis(const char *home_str, const char *frame_str) : + Aircraft(home_str, frame_str) +{ + use_time_sync = false; + rate_hz = 250 / target_speedup; + heli_demix = strstr(frame_str, "helidemix") != NULL; + rev4_servos = strstr(frame_str, "rev4") != NULL; +} + +/* + extremely primitive SOAP parser that assumes the format used by FlightAxis +*/ +void FlightAxis::parse_reply(const char *reply) +{ + const char *reply0 = reply; + for (uint16_t i=0; i= sizeof(reply)) { + printf("Reply too large %u\n", expected_length); + return nullptr; + } + while (ret < expected_length) { + ssize_t ret2 = sock.recv(&reply[ret], sizeof(reply)-(1+ret), 100); + if (ret2 <= 0) { + return nullptr; + } + ret += ret2; + } + return strdup(reply); +} + + + +void FlightAxis::exchange_data(const struct sitl_input &input) +{ + if (!controller_started) { + printf("Starting controller\n"); + // call a restore first. This allows us to connect after the aircraft is changed in RealFlight + char *reply = soap_request("RestoreOriginalControllerDevice", R"( + + +12 + +)"); + free(reply); + reply = soap_request("InjectUAVControllerInterface", R"( + + +12 + +)"); + free(reply); + activation_frame_counter = frame_counter; + controller_started = true; + } + + float scaled_servos[8]; + for (uint8_t i=0; i<8; i++) { + scaled_servos[i] = (input.servos[i] - 1000) / 1000.0f; + } + + if (rev4_servos) { + // swap first 4 and last 4 servos, for quadplane testing + float saved[4]; + memcpy(saved, &scaled_servos[0], sizeof(saved)); + memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved)); + memcpy(&scaled_servos[4], saved, sizeof(saved)); + } + + if (heli_demix) { + // FlightAxis expects "roll/pitch/collective/yaw" input + float swash1 = scaled_servos[0]; + float swash2 = scaled_servos[1]; + float swash3 = scaled_servos[2]; + + float roll_rate = swash1 - swash2; + float pitch_rate = -((swash1+swash2) / 2.0f - swash3); + + scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1); + scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1); + } + + + char *reply = soap_request("ExchangeData", R"( + + + +255 + +%.4f +%.4f +%.4f +%.4f +%.4f +%.4f +%.4f +%.4f + + + + +)", + scaled_servos[0], + scaled_servos[1], + scaled_servos[2], + scaled_servos[3], + scaled_servos[4], + scaled_servos[5], + scaled_servos[6], + scaled_servos[7]); + + if (reply) { + parse_reply(reply); + free(reply); + } +} + + +/* + update the FlightAxis simulation by one time step + */ +void FlightAxis::update(const struct sitl_input &input) +{ + Vector3f last_velocity_ef = velocity_ef; + + exchange_data(input); + + float dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s; + if (dt_seconds <= 0) { + return; + } + + if (initial_time_s <= 0) { + dt_seconds = 0.001f; + initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds; + } + + Quaternion quat(state.m_orientationQuaternion_W, + state.m_orientationQuaternion_Z, + -state.m_orientationQuaternion_Y, + -state.m_orientationQuaternion_Z); + quat.rotation_matrix(dcm); + dcm.from_euler(radians(state.m_roll_DEG), + radians(state.m_inclination_DEG), + -radians(state.m_azimuth_DEG)); + Quaternion quat2; + quat2.from_rotation_matrix(dcm); + gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)), + radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)), + -radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup; + velocity_ef = Vector3f(state.m_velocityWorldU_MPS, + state.m_velocityWorldV_MPS, + state.m_velocityWorldW_MPS); + position = Vector3f(state.m_aircraftPositionY_MTR, + state.m_aircraftPositionX_MTR, + -state.m_altitudeAGL_MTR); + + // offset based on first position to account for offset in RF world + if (position_offset.is_zero()) { + position_offset = position; + } + position -= position_offset; + + // the accel values given in the state are very strange. Calculate + // it from delta-velocity instead, although this does introduce + // noise + Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds; + accel_ef.z -= GRAVITY_MSS; + accel_body = dcm.transposed() * accel_ef; + accel_body.x = constrain_float(accel_body.x, -16, 16); + accel_body.y = constrain_float(accel_body.y, -16, 16); + accel_body.z = constrain_float(accel_body.z, -16, 16); + + airspeed = state.m_airspeed_MPS; + + battery_voltage = state.m_batteryVoltage_VOLTS; + battery_current = state.m_batteryCurrentDraw_AMPS; + rpm1 = state.m_propRPM; + rpm2 = state.m_heliMainRotorRPM; + + update_position(); + time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6; + + if (frame_counter++ % 1000 == 0) { + if (last_frame_count_s != 0) { + printf("%.2f FPS\n", + 1000 / (state.m_currentPhysicsTime_SEC - last_frame_count_s)); + printf("(%.3f %.3f %.3f %.3f) (%.3f %.3f %.3f %.3f)\n", + quat.q1, quat.q2, quat.q3, quat.q4, + quat2.q1, quat2.q2, quat2.q3, quat2.q4); + } else { + printf("Initial position %f %f %f\n", position.x, position.y, position.z); + } + last_frame_count_s = state.m_currentPhysicsTime_SEC; + } + + last_time_s = state.m_currentPhysicsTime_SEC; +} + +} // namespace SITL diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h new file mode 100644 index 0000000000..635077d5ed --- /dev/null +++ b/libraries/SITL/SIM_FlightAxis.h @@ -0,0 +1,161 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulator connection for ardupilot version of FlightAxis +*/ + +#pragma once + +#include + +#include "SIM_Aircraft.h" + +namespace SITL { + +/* + a FlightAxis simulator + */ +class FlightAxis : public Aircraft { +public: + FlightAxis(const char *home_str, const char *frame_str); + + /* update model by one time step */ + void update(const struct sitl_input &input); + + /* static object creator */ + static Aircraft *create(const char *home_str, const char *frame_str) { + return new FlightAxis(home_str, frame_str); + } + + struct state { + double m_airspeed_MPS; + double m_altitudeASL_MTR; + double m_altitudeAGL_MTR; + double m_groundspeed_MPS; + double m_pitchRate_DEGpSEC; + double m_rollRate_DEGpSEC; + double m_yawRate_DEGpSEC; + double m_azimuth_DEG; + double m_inclination_DEG; + double m_roll_DEG; + double m_aircraftPositionX_MTR; + double m_aircraftPositionY_MTR; + double m_velocityWorldU_MPS; + double m_velocityWorldV_MPS; + double m_velocityWorldW_MPS; + double m_velocityBodyU_MPS; + double m_velocityBodyV_MPS; + double m_velocityBodyW_MPS; + double m_accelerationWorldAX_MPS2; + double m_accelerationWorldAY_MPS2; + double m_accelerationWorldAZ_MPS2; + double m_accelerationBodyAX_MPS2; + double m_accelerationBodyAY_MPS2; + double m_accelerationBodyAZ_MPS2; + double m_windX_MPS; + double m_windY_MPS; + double m_windZ_MPS; + double m_propRPM; + double m_heliMainRotorRPM; + double m_batteryVoltage_VOLTS; + double m_batteryCurrentDraw_AMPS; + double m_batteryRemainingCapacity_MAH; + double m_fuelRemaining_OZ; + double m_isLocked; + double m_hasLostComponents; + double m_anEngineIsRunning; + double m_isTouchingGround; + double m_currentAircraftStatus; + double m_currentPhysicsTime_SEC; + double m_currentPhysicsSpeedMultiplier; + double m_orientationQuaternion_X; + double m_orientationQuaternion_Y; + double m_orientationQuaternion_Z; + double m_orientationQuaternion_W; + double m_flightAxisControllerIsActive; + } state; + + static const uint16_t num_keys = sizeof(state)/sizeof(double); + + struct keytable { + const char *key; + double &ref; + } keytable[num_keys] = { + { "m-airspeed-MPS", state.m_airspeed_MPS }, + { "m-altitudeASL-MTR", state.m_altitudeASL_MTR }, + { "m-altitudeAGL-MTR", state.m_altitudeAGL_MTR }, + { "m-groundspeed-MPS", state.m_groundspeed_MPS }, + { "m-pitchRate-DEGpSEC", state.m_pitchRate_DEGpSEC }, + { "m-rollRate-DEGpSEC", state.m_rollRate_DEGpSEC }, + { "m-yawRate-DEGpSEC", state.m_yawRate_DEGpSEC }, + { "m-azimuth-DEG", state.m_azimuth_DEG }, + { "m-inclination-DEG", state.m_inclination_DEG }, + { "m-roll-DEG", state.m_roll_DEG }, + { "m-aircraftPositionX-MTR", state.m_aircraftPositionX_MTR }, + { "m-aircraftPositionY-MTR", state.m_aircraftPositionY_MTR }, + { "m-velocityWorldU-MPS", state.m_velocityWorldU_MPS }, + { "m-velocityWorldV-MPS", state.m_velocityWorldV_MPS }, + { "m-velocityWorldW-MPS", state.m_velocityWorldW_MPS }, + { "m-velocityBodyU-MPS", state.m_velocityBodyU_MPS }, + { "m-velocityBodyV-MPS", state.m_velocityBodyV_MPS }, + { "m-velocityBodyW-MPS", state.m_velocityBodyW_MPS }, + { "m-accelerationWorldAX-MPS2", state.m_accelerationWorldAX_MPS2 }, + { "m-accelerationWorldAY-MPS2", state.m_accelerationWorldAY_MPS2 }, + { "m-accelerationWorldAZ-MPS2", state.m_accelerationWorldAZ_MPS2 }, + { "m-accelerationBodyAX-MPS2", state.m_accelerationBodyAX_MPS2 }, + { "m-accelerationBodyAY-MPS2", state.m_accelerationBodyAY_MPS2 }, + { "m-accelerationBodyAZ-MPS2", state.m_accelerationBodyAZ_MPS2 }, + { "m-windX-MPS", state.m_windX_MPS }, + { "m-windY-MPS", state.m_windY_MPS }, + { "m-windZ-MPS", state.m_windZ_MPS }, + { "m-propRPM", state.m_propRPM }, + { "m-heliMainRotorRPM", state.m_heliMainRotorRPM }, + { "m-batteryVoltage-VOLTS", state.m_batteryVoltage_VOLTS }, + { "m-batteryCurrentDraw-AMPS", state.m_batteryCurrentDraw_AMPS }, + { "m-batteryRemainingCapacity-MAH", state.m_batteryRemainingCapacity_MAH }, + { "m-fuelRemaining-OZ", state.m_fuelRemaining_OZ }, + { "m-isLocked", state.m_isLocked }, + { "m-hasLostComponents", state.m_hasLostComponents }, + { "m-anEngineIsRunning", state.m_anEngineIsRunning }, + { "m-isTouchingGround", state.m_isTouchingGround }, + { "m-currentAircraftStatus", state.m_currentAircraftStatus }, + { "m-currentPhysicsTime-SEC", state.m_currentPhysicsTime_SEC }, + { "m-currentPhysicsSpeedMultiplier", state.m_currentPhysicsSpeedMultiplier }, + { "m-orientationQuaternion-X", state.m_orientationQuaternion_X }, + { "m-orientationQuaternion-Y", state.m_orientationQuaternion_Y }, + { "m-orientationQuaternion-Z", state.m_orientationQuaternion_Z }, + { "m-orientationQuaternion-W", state.m_orientationQuaternion_W }, + { "m-flightAxisControllerIsActive", state.m_flightAxisControllerIsActive } + }; + +private: + char *soap_request(const char *action, const char *fmt, ...); + void exchange_data(const struct sitl_input &input); + void parse_reply(const char *reply); + + double initial_time_s = 0; + double last_time_s = 0; + bool heli_demix = false; + bool rev4_servos = false; + bool controller_started = false; + uint64_t frame_counter = 0; + uint64_t activation_frame_counter = 0; + double last_frame_count_s = 0; + Vector3f position_offset; +}; + + +} // namespace SITL From 22e25cc792b6acb7466f40a302f29539581441d7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 19 Nov 2015 17:11:25 +1100 Subject: [PATCH 064/109] HAL_SITL: added Flightaxis support --- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 9d71db17d4..82c6544b1a 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -24,6 +24,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -76,6 +77,7 @@ static const struct { { "rover", SimRover::create }, { "crrcsim", CRRCSim::create }, { "jsbsim", JSBSim::create }, + { "flightaxis", FlightAxis::create }, { "gazebo", Gazebo::create }, { "last_letter", last_letter::create }, { "tracker", Tracker::create }, From b187a0c6eba7b9bd32050ca52ca313ca833634c6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Nov 2015 10:18:09 +1100 Subject: [PATCH 065/109] AP_Motors: added quadplane frame type this puts the motors on outputs 5 to 8, to leave the first 4 for the plane --- libraries/AP_Motors/AP_MotorsQuad.cpp | 6 ++++++ libraries/AP_Motors/AP_Motors_Class.h | 1 + 2 files changed, 7 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp index cd92f2997a..3b0e8ab0e2 100644 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ b/libraries/AP_Motors/AP_MotorsQuad.cpp @@ -88,6 +88,12 @@ void AP_MotorsQuad::setup_motors() add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4); add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + } else if ( _flags.frame_orientation == AP_MOTORS_QUADPLANE ) { + // quadplane frame set-up, X arrangement on motors 5 to 8 + add_motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); + add_motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); + add_motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); }else{ // X frame set-up add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 6b2664e270..060b3e56db 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -32,6 +32,7 @@ #define AP_MOTORS_NEW_X_FRAME 11 #define AP_MOTORS_NEW_V_FRAME 12 #define AP_MOTORS_NEW_H_FRAME 13 // same as X frame but motors spin in opposite direction +#define AP_MOTORS_QUADPLANE 14 // motors on 5..8 // motor update rate #define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors From 6468fc6d932000c0f1cd676103b3feba28dcdf79 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Nov 2015 19:24:04 +1100 Subject: [PATCH 066/109] Plane: Initial implementation of quadplane adds "HOVER" mode --- ArduPlane/ArduPlane.cpp | 16 ++ ArduPlane/Attitude.cpp | 11 ++ ArduPlane/GCS_Mavlink.cpp | 2 + ArduPlane/Parameters.cpp | 4 + ArduPlane/Parameters.h | 1 + ArduPlane/Plane.h | 7 +- ArduPlane/defines.h | 3 +- ArduPlane/events.cpp | 2 + ArduPlane/make.inc | 5 +- ArduPlane/quadplane.cpp | 307 ++++++++++++++++++++++++++++++++++++++ ArduPlane/quadplane.h | 72 +++++++++ ArduPlane/system.cpp | 10 +- 12 files changed, 436 insertions(+), 4 deletions(-) create mode 100644 ArduPlane/quadplane.cpp create mode 100644 ArduPlane/quadplane.h diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index d28efbfb58..46111f0fbd 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -684,6 +684,21 @@ void Plane::update_flight_mode(void) steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 + + + case HOVER: { + // set nav_roll and nav_pitch using sticks + nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; + nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); + float pitch_input = channel_pitch->norm_input(); + if (pitch_input > 0) { + nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; + } else { + nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); + } + nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + break; + } case INITIALISING: // handled elsewhere @@ -749,6 +764,7 @@ void Plane::update_navigation() case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: + case HOVER: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 1098ea6b08..6396ab55c0 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -140,6 +140,7 @@ void Plane::stabilize_stick_mixing_direct() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || + control_mode == HOVER || control_mode == TRAINING) { return; } @@ -159,6 +160,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || + control_mode == HOVER || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer)) { return; @@ -354,6 +356,8 @@ void Plane::stabilize() stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); + } else if (control_mode == HOVER) { + quadplane.stabilize_hover(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -760,6 +764,9 @@ void Plane::set_servos(void) { int16_t last_throttle = channel_throttle->radio_out; + // do any transition updates for quadplane + quadplane.update(); + if (control_mode == AUTO && auto_state.idle_mode) { // special handling for balloon launch set_servos_idle(); @@ -915,6 +922,10 @@ void Plane::set_servos(void) guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; + } else if (control_mode == HOVER) { + // no forward throttle for now + channel_throttle->servo_out = 0; + channel_throttle->calc_pwm(); } else { // normal throttle calculation based on servo_out channel_throttle->calc_pwm(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e0af13a13d..c0daf2dec7 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -39,6 +39,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: + case HOVER: case CRUISE: base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; @@ -169,6 +170,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case STABILIZE: case FLY_BY_WIRE_A: case AUTOTUNE: + case HOVER: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation break; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 215fa8d5b2..2bfd622fce 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1034,6 +1034,10 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp GOBJECT(adsb, "ADSB_", AP_ADSB), + // @Group: Q_ + // @Path: quadplane.cpp + GOBJECT(quadplane, "Q_", QuadPlane), + // RC channel //----------- // @Group: RC1_ diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6be939aed4..8f8ae66e0c 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -276,6 +276,7 @@ public: k_param_kff_pitch_to_throttle, // unused k_param_kff_throttle_to_pitch, k_param_scaling_speed, + k_param_quadplane, // // 210: flight modes diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 303a5e84ee..c61d9f268b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -96,6 +96,8 @@ #include #include +#include "quadplane.h" + // Configuration #include "config.h" @@ -135,6 +137,7 @@ public: friend class GCS_MAVLINK; friend class Parameters; friend class AP_Arming_Plane; + friend class QuadPlane; Plane(void); @@ -712,7 +715,9 @@ private: // time that rudder arming has been running uint32_t rudder_arm_timer; - + // support for quadcopter-plane + QuadPlane quadplane{ahrs}; + void demo_servos(uint8_t i); void adjust_nav_pitch_throttle(void); void update_load_factor(void); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 438a8f886b..8be362c5ae 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -63,7 +63,8 @@ enum FlightMode { RTL = 11, LOITER = 12, GUIDED = 15, - INITIALISING = 16 + INITIALISING = 16, + HOVER = 17 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index c0989bf390..39217e9f7e 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -13,6 +13,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: + case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -61,6 +62,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: + case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: diff --git a/ArduPlane/make.inc b/ArduPlane/make.inc index 8dbc87585d..6e73b694aa 100644 --- a/ArduPlane/make.inc +++ b/ArduPlane/make.inc @@ -49,4 +49,7 @@ LIBRARIES += AP_RSSI LIBRARIES += AP_RPM LIBRARIES += AP_Parachute LIBRARIES += AP_ADSB - +LIBRARIES += AP_Motors +LIBRARIES += AC_AttitudeControl +LIBRARIES += AC_PID +LIBRARIES += AP_InertialNav diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp new file mode 100644 index 0000000000..2d3891bda3 --- /dev/null +++ b/ArduPlane/quadplane.cpp @@ -0,0 +1,307 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Plane.h" + +const AP_Param::GroupInfo QuadPlane::var_info[] = { + + // @Group: MOT_ + // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp + AP_SUBGROUPINFO(motors, "M_", 1, QuadPlane, AP_MotorsQuad), + + // @Param: RT_RLL_P + // @DisplayName: Roll axis rate controller P gain + // @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_RLL_I + // @DisplayName: Roll axis rate controller I gain + // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_RLL_IMAX + // @DisplayName: Roll axis rate controller I gain maximum + // @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_RLL_D + // @DisplayName: Roll axis rate controller D gain + // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 2, QuadPlane, AC_PID), + + // @Param: RT_PIT_P + // @DisplayName: Pitch axis rate controller P gain + // @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output + // @Range: 0.08 0.30 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_PIT_I + // @DisplayName: Pitch axis rate controller I gain + // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate + // @Range: 0.01 0.5 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_PIT_IMAX + // @DisplayName: Pitch axis rate controller I gain maximum + // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_PIT_D + // @DisplayName: Pitch axis rate controller D gain + // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate + // @Range: 0.001 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 3, QuadPlane, AC_PID), + + // @Param: RT_YAW_P + // @DisplayName: Yaw axis rate controller P gain + // @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output + // @Range: 0.150 0.50 + // @Increment: 0.005 + // @User: Standard + + // @Param: RT_YAW_I + // @DisplayName: Yaw axis rate controller I gain + // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate + // @Range: 0.010 0.05 + // @Increment: 0.01 + // @User: Standard + + // @Param: RT_YAW_IMAX + // @DisplayName: Yaw axis rate controller I gain maximum + // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: Percent*10 + // @User: Standard + + // @Param: RT_YAW_D + // @DisplayName: Yaw axis rate controller D gain + // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate + // @Range: 0.000 0.02 + // @Increment: 0.001 + // @User: Standard + AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 4, QuadPlane, AC_PID), + + // P controllers + //-------------- + // @Param: STB_RLL_P + // @DisplayName: Roll axis stabilize controller P gain + // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 5, QuadPlane, AC_P), + + // @Param: STB_PIT_P + // @DisplayName: Pitch axis stabilize controller P gain + // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 6, QuadPlane, AC_P), + + // @Param: STB_YAW_P + // @DisplayName: Yaw axis stabilize controller P gain + // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate + // @Range: 3.000 6.000 + // @User: Standard + AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 7, QuadPlane, AC_P), + + // @Group: ATC_ + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp + AP_SUBGROUPINFO(attitude_control, "A_", 8, QuadPlane, AC_AttitudeControl), + + // @Param: ANGLE_MAX + // @DisplayName: Angle Max + // @Description: Maximum lean angle in all flight modes + // @Units: Centi-degrees + // @Range: 1000 8000 + // @User: Advanced + AP_GROUPINFO("ANGLE_MAX", 9, QuadPlane, aparm.angle_max, 4500), + + // @Param: TRANSITION_MS + // @DisplayName: Transition time + // @Description: Transition time in milliseconds + // @Units: milli-seconds + // @Range: 0 30000 + // @User: Advanced + AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000), + + // @Param: POS_Z_P + // @DisplayName: Position (vertical) controller P gain + // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller + // @Range: 1.000 3.000 + // @User: Standard + AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P), + + // @Param: POS_XY_P + // @DisplayName: Position (horizonal) controller P gain + // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @Range: 0.500 2.000 + // @User: Standard + AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P), + + // @Param: VEL_XY_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + + // @Param: VEL_XY_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: VEL_XY_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: cm/s/s + // @User: Advanced + AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D), + + // @Param: VEL_Z_P + // @DisplayName: Velocity (vertical) P gain + // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller + // @Range: 1.000 8.000 + // @User: Standard + AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P), + + // @Param: ACCEL_Z_P + // @DisplayName: Throttle acceleration controller P gain + // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output + // @Range: 0.500 1.500 + // @User: Standard + + // @Param: ACCEL_Z_I + // @DisplayName: Throttle acceleration controller I gain + // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration + // @Range: 0.000 3.000 + // @User: Standard + + // @Param: ACCEL_Z_IMAX + // @DisplayName: Throttle acceleration controller I gain maximum + // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate + // @Range: 0 1000 + // @Units: Percent*10 + // @User: Standard + + // @Param: ACCEL_Z_D + // @DisplayName: Throttle acceleration controller D gain + // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration + // @Range: 0.000 0.400 + // @User: Standard + + // @Param: ACCEL_Z_FILT_HZ + // @DisplayName: Throttle acceleration filter + // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. + // @Range: 1.000 100.000 + // @Units: Hz + // @User: Standard + AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID), + + // @Group: POSCON_ + // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp + AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl), + + AP_GROUPEND +}; + +QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) : + ahrs(_ahrs) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void QuadPlane::setup(void) +{ + motors.set_frame_orientation(AP_MOTORS_QUADPLANE); + motors.set_throttle_range(0, 1000, 2000); + motors.set_hover_throttle(500); + motors.set_update_rate(490); + motors.set_interlock(true); + motors.set_loop_rate(plane.ins.get_sample_rate()); + attitude_control.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_roll.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); + pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); + pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); +} + +// stabilize in hover mode +void QuadPlane::stabilize_hover(void) +{ + // max 100 degrees/sec for now + const float yaw_rate_max_dps = 100; + float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, yaw_rate_ef_cds, smoothing_gain); + + // output pilot's throttle + int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; + attitude_control.set_throttle_out(pilot_throttle_scaled, true, 0); + + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); + + last_run_ms = millis(); + last_throttle = pilot_throttle_scaled; +} + +// set motor arming +void QuadPlane::set_armed(bool armed) +{ + motors.armed(armed); + if (armed) { + motors.enable(); + } +} + + +/* + update for transition from quadplane + */ +void QuadPlane::update_transition(void) +{ + if (millis() - last_run_ms < (unsigned)transition_time_ms && plane.control_mode != MANUAL) { + // we are transitioning. Call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, 0, smoothing_gain); + // and degrade throttle + int16_t throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms; + attitude_control.set_throttle_out(throttle_scaled, true, 0); + motors.output(); + } else { + motors.output_min(); + } +} + +/* + update motor output for quadplane + */ +void QuadPlane::update(void) +{ + if (plane.control_mode != HOVER) { + update_transition(); + } else { + motors.output(); + } +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h new file mode 100644 index 0000000000..1abe69f064 --- /dev/null +++ b/ArduPlane/quadplane.h @@ -0,0 +1,72 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#include // Attitude control library +#include +#include + +/* + QuadPlane specific functionality + */ +class QuadPlane +{ +public: + friend class Plane; + QuadPlane(AP_AHRS_NavEKF &_ahrs); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + // setup quadplane + void setup(void); + + // stabilize in hover mode + void stabilize_hover(void); + + // update transition handling + void update(void); + + // set motor arming + void set_armed(bool armed); + +private: + AP_AHRS_NavEKF &ahrs; + AP_Vehicle::MultiCopter aparm; + AP_MotorsQuad motors{50}; + AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02}; + AC_P p_stabilize_roll{4.5}; + AC_P p_stabilize_pitch{4.5}; + AC_P p_stabilize_yaw{4.5}; + + AC_AttitudeControl_Multi attitude_control{ahrs, aparm, motors, + p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, + pid_rate_roll, pid_rate_pitch, pid_rate_yaw}; + + AP_InertialNav_NavEKF inertial_nav{ahrs}; + + AC_P p_pos_xy{1}; + AC_P p_alt_hold{1}; + AC_P p_vel_z{5}; + AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02}; + AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02}; + + AC_PosControl pos_control{ahrs, inertial_nav, motors, attitude_control, + p_alt_hold, p_vel_z, pid_accel_z, + p_pos_xy, pi_vel_xy}; + + // update transition handling + void update_transition(void); + + AP_Int16 transition_time_ms; + + // last time quadplane was active, used for transition + uint32_t last_run_ms; + + // last throttle value when active + int16_t last_throttle; + + const float smoothing_gain = 6; +}; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 657ac37a20..57ec05fa80 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -233,6 +233,8 @@ void Plane::init_ardupilot() startup_ground(); + quadplane.setup(); + // choose the nav controller set_nav_controller(); @@ -396,7 +398,7 @@ void Plane::set_mode(enum FlightMode mode) acro_state.locked_roll = false; acro_state.locked_pitch = false; break; - + case CRUISE: auto_throttle_mode = true; cruise_state.locked_heading = false; @@ -443,6 +445,10 @@ void Plane::set_mode(enum FlightMode mode) guided_WP_loc = current_loc; set_guided_WP(); break; + + case HOVER: + auto_throttle_mode = false; + break; } // start with throttle suppressed in auto_throttle modes @@ -477,6 +483,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case AUTO: case RTL: case LOITER: + case HOVER: set_mode((enum FlightMode)mode); return true; } @@ -743,6 +750,7 @@ void Plane::change_arm_state(void) Log_Arm_Disarm(); hal.util->set_soft_armed(arming.is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); + quadplane.set_armed(hal.util->get_soft_armed()); } /* From ca85c332d6645170c1f89230ac7b4abb14cc34f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 18:45:42 +1100 Subject: [PATCH 067/109] Plane: separate QSTABILIZE and QHOVER modes --- ArduPlane/ArduPlane.cpp | 17 ++++--- ArduPlane/Attitude.cpp | 14 ++++-- ArduPlane/GCS_Mavlink.cpp | 6 ++- ArduPlane/defines.h | 3 +- ArduPlane/events.cpp | 14 +++++- ArduPlane/quadplane.cpp | 99 ++++++++++++++++++++++++++++++++------- ArduPlane/quadplane.h | 13 ++++- ArduPlane/system.cpp | 11 ++++- 8 files changed, 141 insertions(+), 36 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 46111f0fbd..93b20a3ae2 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -35,11 +35,11 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_radio, 50, 700), SCHED_TASK(check_short_failsafe, 50, 1000), - SCHED_TASK(ahrs_update, 50, 6400), + SCHED_TASK(ahrs_update, 400, 6400), SCHED_TASK(update_speed_height, 50, 1600), - SCHED_TASK(update_flight_mode, 50, 1400), - SCHED_TASK(stabilize, 50, 3500), - SCHED_TASK(set_servos, 50, 1600), + SCHED_TASK(update_flight_mode, 400, 1400), + SCHED_TASK(stabilize, 400, 3500), + SCHED_TASK(set_servos, 400, 1600), SCHED_TASK(read_control_switch, 7, 1000), SCHED_TASK(gcs_retry_deferred, 50, 1000), SCHED_TASK(update_GPS_50Hz, 50, 2500), @@ -172,6 +172,9 @@ void Plane::ahrs_update() // frame yaw rate steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt; steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err); + + // update inertial_nav for quadplane + quadplane.inertial_nav.update(G_Dt); } /* @@ -686,7 +689,8 @@ void Plane::update_flight_mode(void) //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 - case HOVER: { + case QSTABILIZE: + case QHOVER: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); @@ -764,7 +768,8 @@ void Plane::update_navigation() case AUTOTUNE: case FLY_BY_WIRE_B: case CIRCLE: - case HOVER: + case QSTABILIZE: + case QHOVER: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 6396ab55c0..dc9ab7bc8d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -140,7 +140,8 @@ void Plane::stabilize_stick_mixing_direct() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || - control_mode == HOVER || + control_mode == QSTABILIZE || + control_mode == QHOVER || control_mode == TRAINING) { return; } @@ -160,7 +161,8 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == AUTOTUNE || control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || - control_mode == HOVER || + control_mode == QSTABILIZE || + control_mode == QHOVER || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer)) { return; @@ -356,8 +358,10 @@ void Plane::stabilize() stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); - } else if (control_mode == HOVER) { - quadplane.stabilize_hover(); + } else if (control_mode == QSTABILIZE) { + quadplane.control_stabilize(); + } else if (control_mode == QHOVER) { + quadplane.control_hover(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -922,7 +926,7 @@ void Plane::set_servos(void) guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; - } else if (control_mode == HOVER) { + } else if (control_mode == QSTABILIZE || control_mode == QHOVER) { // no forward throttle for now channel_throttle->servo_out = 0; channel_throttle->calc_pwm(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c0daf2dec7..2cb3792120 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -39,7 +39,8 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: - case HOVER: + case QSTABILIZE: + case QHOVER: case CRUISE: base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; @@ -170,7 +171,8 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case STABILIZE: case FLY_BY_WIRE_A: case AUTOTUNE: - case HOVER: + case QSTABILIZE: + case QHOVER: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation break; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 8be362c5ae..a91f9b6cd9 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -64,7 +64,8 @@ enum FlightMode { LOITER = 12, GUIDED = 15, INITIALISING = 16, - HOVER = 17 + QSTABILIZE = 17, + QHOVER = 18 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 39217e9f7e..1e8b7d46e5 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -13,7 +13,6 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: - case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -28,6 +27,12 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) } break; + case QSTABILIZE: + failsafe.saved_mode = control_mode; + failsafe.saved_mode_set = 1; + set_mode(QHOVER); + break; + case AUTO: case GUIDED: case LOITER: @@ -44,6 +49,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) case CIRCLE: case RTL: + case QHOVER: default: break; } @@ -62,7 +68,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) case MANUAL: case STABILIZE: case ACRO: - case HOVER: case FLY_BY_WIRE_A: case AUTOTUNE: case FLY_BY_WIRE_B: @@ -76,6 +81,10 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) } break; + case QSTABILIZE: + set_mode(QHOVER); + break; + case AUTO: case GUIDED: case LOITER: @@ -87,6 +96,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) break; case RTL: + case QHOVER: default: break; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2d3891bda3..121c2f7f7d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -141,35 +141,35 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000), - // @Param: POS_Z_P + // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P), - // @Param: POS_XY_P + // @Param: PXY_P // @DisplayName: Position (horizonal) controller P gain // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P), - // @Param: VEL_XY_P + // @Param: VXY_P // @DisplayName: Velocity (horizontal) P gain // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced - // @Param: VEL_XY_I + // @Param: VXY_I // @DisplayName: Velocity (horizontal) I gain // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration // @Range: 0.02 1.00 // @Increment: 0.01 // @User: Advanced - // @Param: VEL_XY_IMAX + // @Param: VXY_IMAX // @DisplayName: Velocity (horizontal) integrator maximum // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output // @Range: 0 4500 @@ -178,39 +178,39 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Advanced AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D), - // @Param: VEL_Z_P + // @Param: VZ_P // @DisplayName: Velocity (vertical) P gain // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller // @Range: 1.000 8.000 // @User: Standard AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P), - // @Param: ACCEL_Z_P + // @Param: AZ_P // @DisplayName: Throttle acceleration controller P gain // @Description: Throttle acceleration controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output // @Range: 0.500 1.500 // @User: Standard - // @Param: ACCEL_Z_I + // @Param: AZ_I // @DisplayName: Throttle acceleration controller I gain // @Description: Throttle acceleration controller I gain. Corrects long-term difference in desired vertical acceleration and actual acceleration // @Range: 0.000 3.000 // @User: Standard - // @Param: ACCEL_Z_IMAX + // @Param: AZ_IMAX // @DisplayName: Throttle acceleration controller I gain maximum // @Description: Throttle acceleration controller I gain maximum. Constrains the maximum pwm that the I term will generate // @Range: 0 1000 // @Units: Percent*10 // @User: Standard - // @Param: ACCEL_Z_D + // @Param: AZ_D // @DisplayName: Throttle acceleration controller D gain // @Description: Throttle acceleration controller D gain. Compensates for short-term change in desired vertical acceleration vs actual acceleration // @Range: 0.000 0.400 // @User: Standard - // @Param: ACCEL_Z_FILT_HZ + // @Param: AZ_FILT_HZ // @DisplayName: Throttle acceleration filter // @Description: Filter applied to acceleration to reduce noise. Lower values reduce noise but add delay. // @Range: 1.000 100.000 @@ -218,10 +218,28 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID), - // @Group: POSCON_ + // @Group: P_ // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl), + + // @Param: VELZ_MAX + // @DisplayName: Pilot maximum vertical speed + // @Description: The maximum vertical velocity the pilot may request in cm/s + // @Units: Centimeters/Second + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("VELZ_MAX", 17, QuadPlane, pilot_velocity_z_max, 250), + // @Param: ACCEL_Z + // @DisplayName: Pilot vertical acceleration + // @Description: The vertical acceleration used when pilot is controlling the altitude + // @Units: cm/s/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250), + AP_GROUPEND }; @@ -244,17 +262,27 @@ void QuadPlane::setup(void) pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); + pos_control.set_dt(plane.ins.get_loop_delta_t()); } -// stabilize in hover mode -void QuadPlane::stabilize_hover(void) +// init quadplane stabilize mode +void QuadPlane::init_stabilize(void) +{ + // nothing to do +} + +// quadplane stabilize mode +void QuadPlane::control_stabilize(void) { // max 100 degrees/sec for now const float yaw_rate_max_dps = 100; float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, yaw_rate_ef_cds, smoothing_gain); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_ef_cds, + smoothing_gain); // output pilot's throttle int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; @@ -267,6 +295,45 @@ void QuadPlane::stabilize_hover(void) last_throttle = pilot_throttle_scaled; } +// init quadplane hover mode +void QuadPlane::init_hover(void) +{ + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control.set_accel_z(pilot_accel_z); + + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); +} + +void QuadPlane::control_hover(void) +{ + // initialize vertical speeds and acceleration + pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control.set_accel_z(pilot_accel_z); + + const float yaw_rate_max_dps = 100; + float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; + + // get pilot desired climb rate + float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, + plane.nav_pitch_cd, + yaw_rate_ef_cds, + smoothing_gain); + + // call position controller + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); + pos_control.update_z_controller(); + last_run_ms = millis(); + + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); +} + // set motor arming void QuadPlane::set_armed(bool armed) { @@ -299,7 +366,7 @@ void QuadPlane::update_transition(void) */ void QuadPlane::update(void) { - if (plane.control_mode != HOVER) { + if (plane.control_mode != QSTABILIZE && plane.control_mode != QHOVER) { update_transition(); } else { motors.output(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1abe69f064..b1d05e3afe 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -21,8 +21,11 @@ public: // setup quadplane void setup(void); - // stabilize in hover mode - void stabilize_hover(void); + // main entry points for VTOL flight modes + void init_stabilize(void); + void control_stabilize(void); + void init_hover(void); + void control_hover(void); // update transition handling void update(void); @@ -57,6 +60,12 @@ private: p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy}; + // maximum vertical velocity the pilot may request + AP_Int16 pilot_velocity_z_max; + + // vertical acceleration the pilot may request + AP_Int16 pilot_accel_z; + // update transition handling void update_transition(void); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 57ec05fa80..254613540c 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -446,8 +446,14 @@ void Plane::set_mode(enum FlightMode mode) set_guided_WP(); break; - case HOVER: + case QSTABILIZE: auto_throttle_mode = false; + quadplane.init_stabilize(); + break; + + case QHOVER: + auto_throttle_mode = false; + quadplane.init_hover(); break; } @@ -483,7 +489,8 @@ bool Plane::mavlink_set_mode(uint8_t mode) case AUTO: case RTL: case LOITER: - case HOVER: + case QSTABILIZE: + case QHOVER: set_mode((enum FlightMode)mode); return true; } From 00ca292160f8186e3811b05a6d446c9063083479 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 19:27:13 +1100 Subject: [PATCH 068/109] Plane: improved quadplane transition --- ArduPlane/quadplane.cpp | 95 +++++++++++++++++++++++++++++++---------- ArduPlane/quadplane.h | 15 ++++++- 2 files changed, 87 insertions(+), 23 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 121c2f7f7d..725bf3f3b0 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -139,7 +139,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Units: milli-seconds // @Range: 0 30000 // @User: Advanced - AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 10000), + AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 3000), // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain @@ -271,8 +271,8 @@ void QuadPlane::init_stabilize(void) // nothing to do } -// quadplane stabilize mode -void QuadPlane::control_stabilize(void) +// hold in stabilize with given throttle +void QuadPlane::hold_stabilize(float throttle_in) { // max 100 degrees/sec for now const float yaw_rate_max_dps = 100; @@ -284,15 +284,22 @@ void QuadPlane::control_stabilize(void) yaw_rate_ef_cds, smoothing_gain); - // output pilot's throttle - int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; - attitude_control.set_throttle_out(pilot_throttle_scaled, true, 0); + attitude_control.set_throttle_out(throttle_in, true, 0); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); + last_throttle = motors.get_throttle(); +} + +// quadplane stabilize mode +void QuadPlane::control_stabilize(void) +{ + int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; + hold_stabilize(pilot_throttle_scaled); + last_run_ms = millis(); - last_throttle = pilot_throttle_scaled; + transition_state = TRANSITION_AIRSPEED_WAIT; } // init quadplane hover mode @@ -307,7 +314,10 @@ void QuadPlane::init_hover(void) pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); } -void QuadPlane::control_hover(void) +/* + hold hover with target climb rate + */ +void QuadPlane::hold_hover(float target_climb_rate) { // initialize vertical speeds and acceleration pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); @@ -316,9 +326,6 @@ void QuadPlane::control_hover(void) const float yaw_rate_max_dps = 100; float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; - // get pilot desired climb rate - float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; - // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, @@ -328,10 +335,25 @@ void QuadPlane::control_hover(void) // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); pos_control.update_z_controller(); - last_run_ms = millis(); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); + + last_throttle = motors.get_throttle(); +} + +/* + control QHOVER mode + */ +void QuadPlane::control_hover(void) +{ + // get pilot desired climb rate + float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; + + hold_hover(target_climb_rate); + + last_run_ms = millis(); + transition_state = TRANSITION_AIRSPEED_WAIT; } // set motor arming @@ -345,19 +367,47 @@ void QuadPlane::set_armed(bool armed) /* - update for transition from quadplane + update for transition from quadplane to fixed wing mode */ void QuadPlane::update_transition(void) { - if (millis() - last_run_ms < (unsigned)transition_time_ms && plane.control_mode != MANUAL) { - // we are transitioning. Call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, 0, smoothing_gain); - // and degrade throttle - int16_t throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms; - attitude_control.set_throttle_out(throttle_scaled, true, 0); - motors.output(); - } else { + if (plane.control_mode == MANUAL) { + // in manual mode quad motors are always off motors.output_min(); + transition_state = TRANSITION_DONE; + return; + } + + switch (transition_state) { + case TRANSITION_AIRSPEED_WAIT: { + // we hold in hover until the required airspeed is reached + float aspeed; + if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) { + last_run_ms = millis(); + transition_state = TRANSITION_TIMER; + } + hold_hover(0); + break; + } + + case TRANSITION_TIMER: { + // after airspeed is reached we degrade throttle over the + // transition time, but continue to stabilize + if (millis() - last_run_ms > (unsigned)transition_time_ms) { + transition_state = TRANSITION_DONE; + } + float throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms; + if (throttle_scaled < 0) { + throttle_scaled = 0; + } + hold_stabilize(throttle_scaled); + motors.output(); + break; + } + + case TRANSITION_DONE: + motors.output_min(); + break; } } @@ -366,7 +416,8 @@ void QuadPlane::update_transition(void) */ void QuadPlane::update(void) { - if (plane.control_mode != QSTABILIZE && plane.control_mode != QHOVER) { + if (plane.control_mode != QSTABILIZE && + plane.control_mode != QHOVER) { update_transition(); } else { motors.output(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index b1d05e3afe..1355a4211b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -68,6 +68,12 @@ private: // update transition handling void update_transition(void); + + // hold hover (for transition) + void hold_hover(float target_climb_rate); + + // hold stabilize (for transition) + void hold_stabilize(float throttle_in); AP_Int16 transition_time_ms; @@ -75,7 +81,14 @@ private: uint32_t last_run_ms; // last throttle value when active - int16_t last_throttle; + float last_throttle; const float smoothing_gain = 6; + + // true if we have reached the airspeed threshold for transition + enum { + TRANSITION_AIRSPEED_WAIT, + TRANSITION_TIMER, + TRANSITION_DONE + } transition_state; }; From 29835760673bf2e93ad07f3d708ba06343e210d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 19:51:05 +1100 Subject: [PATCH 069/109] Plane: added QLOITER mode quadplane loiter --- ArduPlane/ArduPlane.cpp | 4 +- ArduPlane/Attitude.cpp | 8 ++- ArduPlane/GCS_Mavlink.cpp | 2 + ArduPlane/defines.h | 3 +- ArduPlane/events.cpp | 2 + ArduPlane/make.inc | 1 + ArduPlane/quadplane.cpp | 108 +++++++++++++++++++++++++++++--------- ArduPlane/quadplane.h | 17 +++++- ArduPlane/system.cpp | 6 +++ 9 files changed, 122 insertions(+), 29 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 93b20a3ae2..03594dfc40 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -690,7 +690,8 @@ void Plane::update_flight_mode(void) case QSTABILIZE: - case QHOVER: { + case QHOVER: + case QLOITER: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); @@ -770,6 +771,7 @@ void Plane::update_navigation() case CIRCLE: case QSTABILIZE: case QHOVER: + case QLOITER: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index dc9ab7bc8d..abf5bf577e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -142,6 +142,7 @@ void Plane::stabilize_stick_mixing_direct() control_mode == CRUISE || control_mode == QSTABILIZE || control_mode == QHOVER || + control_mode == QLOITER || control_mode == TRAINING) { return; } @@ -163,6 +164,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == CRUISE || control_mode == QSTABILIZE || control_mode == QHOVER || + control_mode == QLOITER || control_mode == TRAINING || (control_mode == AUTO && g.auto_fbw_steer)) { return; @@ -362,6 +364,8 @@ void Plane::stabilize() quadplane.control_stabilize(); } else if (control_mode == QHOVER) { quadplane.control_hover(); + } else if (control_mode == QLOITER) { + quadplane.control_loiter(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -926,7 +930,9 @@ void Plane::set_servos(void) guided_throttle_passthru) { // manual pass through of throttle while in GUIDED channel_throttle->radio_out = channel_throttle->radio_in; - } else if (control_mode == QSTABILIZE || control_mode == QHOVER) { + } else if (control_mode == QSTABILIZE || + control_mode == QHOVER || + control_mode == QLOITER) { // no forward throttle for now channel_throttle->servo_out = 0; channel_throttle->calc_pwm(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 2cb3792120..0fead43dcf 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -41,6 +41,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) case FLY_BY_WIRE_B: case QSTABILIZE: case QHOVER: + case QLOITER: case CRUISE: base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; @@ -173,6 +174,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan) case AUTOTUNE: case QSTABILIZE: case QHOVER: + case QLOITER: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation break; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a91f9b6cd9..94ea0c8aed 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -65,7 +65,8 @@ enum FlightMode { GUIDED = 15, INITIALISING = 16, QSTABILIZE = 17, - QHOVER = 18 + QHOVER = 18, + QLOITER = 19 }; // type of stick mixing enabled diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 1e8b7d46e5..70873627fd 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -28,6 +28,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype) break; case QSTABILIZE: + case QLOITER: failsafe.saved_mode = control_mode; failsafe.saved_mode_set = 1; set_mode(QHOVER); @@ -82,6 +83,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype) break; case QSTABILIZE: + case QLOITER: set_mode(QHOVER); break; diff --git a/ArduPlane/make.inc b/ArduPlane/make.inc index 6e73b694aa..70b8988554 100644 --- a/ArduPlane/make.inc +++ b/ArduPlane/make.inc @@ -53,3 +53,4 @@ LIBRARIES += AP_Motors LIBRARIES += AC_AttitudeControl LIBRARIES += AC_PID LIBRARIES += AP_InertialNav +LIBRARIES += AC_WPNav diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 725bf3f3b0..03c319a0d0 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -135,11 +135,11 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: TRANSITION_MS // @DisplayName: Transition time - // @Description: Transition time in milliseconds + // @Description: Transition time in milliseconds after minimum airspeed is reached // @Units: milli-seconds // @Range: 0 30000 // @User: Advanced - AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 3000), + AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 5000), // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain @@ -240,6 +240,10 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250), + // @Group: WP_ + // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp + AP_SUBGROUPINFO(wp_nav, "WP_", 19, QuadPlane, AC_WPNav), + AP_GROUPEND }; @@ -273,23 +277,17 @@ void QuadPlane::init_stabilize(void) // hold in stabilize with given throttle void QuadPlane::hold_stabilize(float throttle_in) -{ - // max 100 degrees/sec for now - const float yaw_rate_max_dps = 100; - float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; - +{ // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, - yaw_rate_ef_cds, + get_pilot_desired_yaw_rate_cds(), smoothing_gain); attitude_control.set_throttle_out(throttle_in, true, 0); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); - - last_throttle = motors.get_throttle(); } // quadplane stabilize mode @@ -298,8 +296,8 @@ void QuadPlane::control_stabilize(void) int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; hold_stabilize(pilot_throttle_scaled); - last_run_ms = millis(); transition_state = TRANSITION_AIRSPEED_WAIT; + last_throttle = motors.get_throttle(); } // init quadplane hover mode @@ -323,13 +321,10 @@ void QuadPlane::hold_hover(float target_climb_rate) pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control.set_accel_z(pilot_accel_z); - const float yaw_rate_max_dps = 100; - float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; - // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, - yaw_rate_ef_cds, + get_pilot_desired_yaw_rate_cds(), smoothing_gain); // call position controller @@ -347,15 +342,79 @@ void QuadPlane::hold_hover(float target_climb_rate) */ void QuadPlane::control_hover(void) { - // get pilot desired climb rate - float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; + hold_hover(get_pilot_desired_climb_rate_cms()); - hold_hover(target_climb_rate); - - last_run_ms = millis(); transition_state = TRANSITION_AIRSPEED_WAIT; } +void QuadPlane::init_loiter(void) +{ + // set target to current position + wp_nav.init_loiter_target(); + + // initialize vertical speed and acceleration + pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control.set_accel_z(pilot_accel_z); + + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); +} + + +// run quadplane loiter controller +void QuadPlane::control_loiter() +{ + // initialize vertical speed and acceleration + pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control.set_accel_z(pilot_accel_z); + + // process pilot's roll and pitch input + wp_nav.set_pilot_desired_acceleration(plane.channel_roll->control_in, + plane.channel_pitch->control_in); + + // Update EKF speed limit - used to limit speed when we are using optical flow + float ekfGndSpdLimit, ekfNavVelGainScaler; + ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + + // run loiter controller + wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + + // call attitude controller + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), + wp_nav.get_pitch(), + get_pilot_desired_yaw_rate_cds()); + + // nav roll and pitch are controller by loiter controller + plane.nav_roll_cd = wp_nav.get_roll(); + plane.nav_pitch_cd = wp_nav.get_pitch(); + + // update altitude target and call position controller + pos_control.set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false); + pos_control.update_z_controller(); + + // run low level rate controllers that only require IMU data + attitude_control.rate_controller_run(); + + last_throttle = motors.get_throttle(); +} + +/* + get desired yaw rate in cd/s + */ +float QuadPlane::get_pilot_desired_yaw_rate_cds(void) +{ + const float yaw_rate_max_dps = 100; + return plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; +} + +// get pilot desired climb rate in cm/s +float QuadPlane::get_pilot_desired_climb_rate_cms(void) +{ + return pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; +} + + // set motor arming void QuadPlane::set_armed(bool armed) { @@ -383,7 +442,7 @@ void QuadPlane::update_transition(void) // we hold in hover until the required airspeed is reached float aspeed; if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) { - last_run_ms = millis(); + transition_start_ms = millis(); transition_state = TRANSITION_TIMER; } hold_hover(0); @@ -393,10 +452,10 @@ void QuadPlane::update_transition(void) case TRANSITION_TIMER: { // after airspeed is reached we degrade throttle over the // transition time, but continue to stabilize - if (millis() - last_run_ms > (unsigned)transition_time_ms) { + if (millis() - transition_start_ms > (unsigned)transition_time_ms) { transition_state = TRANSITION_DONE; } - float throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms; + float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms; if (throttle_scaled < 0) { throttle_scaled = 0; } @@ -417,7 +476,8 @@ void QuadPlane::update_transition(void) void QuadPlane::update(void) { if (plane.control_mode != QSTABILIZE && - plane.control_mode != QHOVER) { + plane.control_mode != QHOVER && + plane.control_mode != QLOITER) { update_transition(); } else { motors.output(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 1355a4211b..f6ac2884a1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -5,6 +5,7 @@ #include // Attitude control library #include #include +#include /* QuadPlane specific functionality @@ -24,9 +25,13 @@ public: // main entry points for VTOL flight modes void init_stabilize(void); void control_stabilize(void); + void init_hover(void); void control_hover(void); + void init_loiter(void); + void control_loiter(void); + // update transition handling void update(void); @@ -60,6 +65,8 @@ private: p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy, pi_vel_xy}; + AC_WPNav wp_nav{inertial_nav, ahrs, pos_control, attitude_control}; + // maximum vertical velocity the pilot may request AP_Int16 pilot_velocity_z_max; @@ -74,11 +81,17 @@ private: // hold stabilize (for transition) void hold_stabilize(float throttle_in); + + // get desired yaw rate in cd/s + float get_pilot_desired_yaw_rate_cds(void); + + // get desired climb rate in cm/s + float get_pilot_desired_climb_rate_cms(void); AP_Int16 transition_time_ms; - // last time quadplane was active, used for transition - uint32_t last_run_ms; + // timer start for transition + uint32_t transition_start_ms; // last throttle value when active float last_throttle; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 254613540c..8c63237b4c 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -455,6 +455,11 @@ void Plane::set_mode(enum FlightMode mode) auto_throttle_mode = false; quadplane.init_hover(); break; + + case QLOITER: + auto_throttle_mode = false; + quadplane.init_loiter(); + break; } // start with throttle suppressed in auto_throttle modes @@ -491,6 +496,7 @@ bool Plane::mavlink_set_mode(uint8_t mode) case LOITER: case QSTABILIZE: case QHOVER: + case QLOITER: set_mode((enum FlightMode)mode); return true; } From 48e1a0641f18be94850fca2582000627bd76cf89 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 20:13:20 +1100 Subject: [PATCH 070/109] Plane: added throttle wait to quadplane --- ArduPlane/quadplane.cpp | 68 ++++++++++++++++++++++++++++++----------- ArduPlane/quadplane.h | 6 ++++ 2 files changed, 56 insertions(+), 18 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 03c319a0d0..ee2bd04223 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -272,7 +272,7 @@ void QuadPlane::setup(void) // init quadplane stabilize mode void QuadPlane::init_stabilize(void) { - // nothing to do + throttle_wait = false; } // hold in stabilize with given throttle @@ -285,9 +285,6 @@ void QuadPlane::hold_stabilize(float throttle_in) smoothing_gain); attitude_control.set_throttle_out(throttle_in, true, 0); - - // run low level rate controllers that only require IMU data - attitude_control.rate_controller_run(); } // quadplane stabilize mode @@ -296,8 +293,6 @@ void QuadPlane::control_stabilize(void) int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10; hold_stabilize(pilot_throttle_scaled); - transition_state = TRANSITION_AIRSPEED_WAIT; - last_throttle = motors.get_throttle(); } // init quadplane hover mode @@ -310,6 +305,8 @@ void QuadPlane::init_hover(void) // initialise position and desired velocity pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + + init_throttle_wait(); } /* @@ -331,10 +328,6 @@ void QuadPlane::hold_hover(float target_climb_rate) pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); pos_control.update_z_controller(); - // run low level rate controllers that only require IMU data - attitude_control.rate_controller_run(); - - last_throttle = motors.get_throttle(); } /* @@ -342,9 +335,12 @@ void QuadPlane::hold_hover(float target_climb_rate) */ void QuadPlane::control_hover(void) { - hold_hover(get_pilot_desired_climb_rate_cms()); - - transition_state = TRANSITION_AIRSPEED_WAIT; + if (throttle_wait) { + attitude_control.set_throttle_out_unstabilized(0, true, 0); + pos_control.relax_alt_hold_controllers(0); + } else { + hold_hover(get_pilot_desired_climb_rate_cms()); + } } void QuadPlane::init_loiter(void) @@ -359,12 +355,21 @@ void QuadPlane::init_loiter(void) // initialise position and desired velocity pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + + init_throttle_wait(); } // run quadplane loiter controller void QuadPlane::control_loiter() { + if (throttle_wait) { + attitude_control.set_throttle_out_unstabilized(0, true, 0); + pos_control.relax_alt_hold_controllers(0); + wp_nav.init_loiter_target(); + return; + } + // initialize vertical speed and acceleration pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); pos_control.set_accel_z(pilot_accel_z); @@ -392,11 +397,6 @@ void QuadPlane::control_loiter() // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false); pos_control.update_z_controller(); - - // run low level rate controllers that only require IMU data - attitude_control.rate_controller_run(); - - last_throttle = motors.get_throttle(); } /* @@ -415,6 +415,19 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) } +/* + initialise throttle_wait based on throttle and is_flying() + */ +void QuadPlane::init_throttle_wait(void) +{ + if (plane.channel_throttle->control_in >= 10 || + plane.is_flying()) { + throttle_wait = false; + } else { + throttle_wait = true; + } +} + // set motor arming void QuadPlane::set_armed(bool armed) { @@ -440,10 +453,16 @@ void QuadPlane::update_transition(void) switch (transition_state) { case TRANSITION_AIRSPEED_WAIT: { // we hold in hover until the required airspeed is reached + if (transition_start_ms == 0) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition airspeed wait"); + transition_start_ms = millis(); + } + float aspeed; if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) { transition_start_ms = millis(); transition_state = TRANSITION_TIMER; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition airspeed reached %.1f", aspeed); } hold_hover(0); break; @@ -454,6 +473,7 @@ void QuadPlane::update_transition(void) // transition time, but continue to stabilize if (millis() - transition_start_ms > (unsigned)transition_time_ms) { transition_state = TRANSITION_DONE; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition done"); } float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms; if (throttle_scaled < 0) { @@ -480,6 +500,18 @@ void QuadPlane::update(void) plane.control_mode != QLOITER) { update_transition(); } else { + // run low level rate controllers + attitude_control.rate_controller_run(); + + // output to motors motors.output(); + transition_start_ms = 0; + transition_state = TRANSITION_AIRSPEED_WAIT; + last_throttle = motors.get_throttle(); + } + + // disable throttle_wait when throttle rises above 10% + if (throttle_wait && plane.channel_throttle->control_in > 10) { + throttle_wait = false; } } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f6ac2884a1..8efc5b59c1 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -87,6 +87,9 @@ private: // get desired climb rate in cm/s float get_pilot_desired_climb_rate_cms(void); + + // initialise throttle_wait when entering mode + void init_throttle_wait(); AP_Int16 transition_time_ms; @@ -104,4 +107,7 @@ private: TRANSITION_TIMER, TRANSITION_DONE } transition_state; + + // true when waiting for pilot throttle + bool throttle_wait; }; From 0d6b353bcb64759935ed0668975aec3b0a5aac85 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Dec 2015 21:40:40 +1100 Subject: [PATCH 071/109] Plane: added quad assistance and auto support for quadplane --- ArduPlane/ArduPlane.cpp | 14 +- ArduPlane/Attitude.cpp | 13 +- ArduPlane/GCS_Mavlink.cpp | 4 + ArduPlane/Parameters.cpp | 1 + ArduPlane/Plane.h | 3 + ArduPlane/quadplane.cpp | 473 +++++++++++++++++++++++++++++++------- ArduPlane/quadplane.h | 74 ++++-- ArduPlane/system.cpp | 16 +- ArduPlane/wscript | 5 + 9 files changed, 477 insertions(+), 126 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 03594dfc40..ed3a562392 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -486,7 +486,9 @@ void Plane::handle_auto_mode(void) nav_cmd_id = auto_rtl_command.id; } - if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || + if (auto_state.vtol_mode) { + quadplane.control_auto(next_WP_loc); + } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { takeoff_calc_roll(); takeoff_calc_pitch(); @@ -535,6 +537,16 @@ void Plane::update_flight_mode(void) steer_state.hold_course_cd = -1; } + // ensure we are fly-forward + if (effective_mode == QSTABILIZE || + effective_mode == QHOVER || + effective_mode == QLOITER || + (effective_mode == AUTO && auto_state.vtol_mode)) { + ahrs.set_fly_forward(false); + } else { + ahrs.set_fly_forward(true); + } + switch (effective_mode) { case AUTO: diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index abf5bf577e..1fc6ae133b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -360,12 +360,10 @@ void Plane::stabilize() stabilize_training(speed_scaler); } else if (control_mode == ACRO) { stabilize_acro(speed_scaler); - } else if (control_mode == QSTABILIZE) { - quadplane.control_stabilize(); - } else if (control_mode == QHOVER) { - quadplane.control_hover(); - } else if (control_mode == QLOITER) { - quadplane.control_loiter(); + } else if (control_mode == QSTABILIZE || + control_mode == QHOVER || + control_mode == QLOITER) { + quadplane.control_run(); } else { if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { stabilize_stick_mixing_fbw(); @@ -932,7 +930,8 @@ void Plane::set_servos(void) channel_throttle->radio_out = channel_throttle->radio_in; } else if (control_mode == QSTABILIZE || control_mode == QHOVER || - control_mode == QLOITER) { + control_mode == QLOITER || + (control_mode == AUTO && auto_state.vtol_mode)) { // no forward throttle for now channel_throttle->servo_out = 0; channel_throttle->calc_pwm(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 0fead43dcf..b5caf866ac 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1523,6 +1523,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; #endif + case MAV_CMD_DO_VTOL_TRANSITION: + result = plane.quadplane.handle_do_vtol_transition(packet); + break; + default: break; } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 2bfd622fce..8a498f3c4c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -434,6 +434,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: Fly By Wire B altitude change rate // @Description: This sets the rate in m/s at which FBWB and CRUISE modes will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters. // @Range: 1 10 + // @Units: m/s // @Increment: 0.1 // @User: Standard GSCALAR(flybywire_climb_rate, "FBWB_CLIMB_RATE", 2.0f), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c61d9f268b..389176157f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -494,6 +494,9 @@ private: // barometric altitude at start of takeoff float baro_takeoff_alt; + + // are we in VTOL mode? + bool vtol_mode:1; } auto_state; struct { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ee2bd04223..a39f4ce180 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4,9 +4,16 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { - // @Group: MOT_ + // @Param: ENABLE + // @DisplayName: Enable QuadPlane + // @Description: This enables QuadPlane functionality, assuming quad motors on outputs 5 to 8 + // @Values: 0:Disable,1:Enable + // @User: Standard + AP_GROUPINFO("ENABLE", 1, QuadPlane, enable, 0), + + // @Group: M_ // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp - AP_SUBGROUPINFO(motors, "M_", 1, QuadPlane, AP_MotorsQuad), + AP_SUBGROUPPTR(motors, "M_", 2, QuadPlane, AP_MotorsQuad), // @Param: RT_RLL_P // @DisplayName: Roll axis rate controller P gain @@ -36,7 +43,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0.001 0.02 // @Increment: 0.001 // @User: Standard - AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 2, QuadPlane, AC_PID), + AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 3, QuadPlane, AC_PID), // @Param: RT_PIT_P // @DisplayName: Pitch axis rate controller P gain @@ -66,7 +73,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0.001 0.02 // @Increment: 0.001 // @User: Standard - AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 3, QuadPlane, AC_PID), + AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 4, QuadPlane, AC_PID), // @Param: RT_YAW_P // @DisplayName: Yaw axis rate controller P gain @@ -96,7 +103,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 0.000 0.02 // @Increment: 0.001 // @User: Standard - AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 4, QuadPlane, AC_PID), + AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 5, QuadPlane, AC_PID), // P controllers //-------------- @@ -105,25 +112,25 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate // @Range: 3.000 12.000 // @User: Standard - AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 5, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 6, QuadPlane, AC_P), // @Param: STB_PIT_P // @DisplayName: Pitch axis stabilize controller P gain // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate // @Range: 3.000 12.000 // @User: Standard - AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 6, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 7, QuadPlane, AC_P), // @Param: STB_YAW_P // @DisplayName: Yaw axis stabilize controller P gain // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate // @Range: 3.000 6.000 // @User: Standard - AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 7, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 8, QuadPlane, AC_P), // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp - AP_SUBGROUPINFO(attitude_control, "A_", 8, QuadPlane, AC_AttitudeControl), + AP_SUBGROUPPTR(attitude_control, "A_", 9, QuadPlane, AC_AttitudeControl), // @Param: ANGLE_MAX // @DisplayName: Angle Max @@ -131,7 +138,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Units: Centi-degrees // @Range: 1000 8000 // @User: Advanced - AP_GROUPINFO("ANGLE_MAX", 9, QuadPlane, aparm.angle_max, 4500), + AP_GROUPINFO("ANGLE_MAX", 10, QuadPlane, aparm.angle_max, 4500), // @Param: TRANSITION_MS // @DisplayName: Transition time @@ -139,21 +146,21 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Units: milli-seconds // @Range: 0 30000 // @User: Advanced - AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 5000), + AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000), // @Param: PZ_P // @DisplayName: Position (vertical) controller P gain // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard - AP_SUBGROUPINFO(p_alt_hold, "PZ_", 11, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_alt_hold, "PZ_", 12, QuadPlane, AC_P), // @Param: PXY_P // @DisplayName: Position (horizonal) controller P gain // @Description: Loiter position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard - AP_SUBGROUPINFO(p_pos_xy, "PXY_", 12, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_pos_xy, "PXY_", 13, QuadPlane, AC_P), // @Param: VXY_P // @DisplayName: Velocity (horizontal) P gain @@ -176,14 +183,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Increment: 10 // @Units: cm/s/s // @User: Advanced - AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 13, QuadPlane, AC_PI_2D), + AP_SUBGROUPINFO(pi_vel_xy, "VXY_", 14, QuadPlane, AC_PI_2D), // @Param: VZ_P // @DisplayName: Velocity (vertical) P gain // @Description: Velocity (vertical) P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller // @Range: 1.000 8.000 // @User: Standard - AP_SUBGROUPINFO(p_vel_z, "VZ_", 14, QuadPlane, AC_P), + AP_SUBGROUPINFO(p_vel_z, "VZ_", 15, QuadPlane, AC_P), // @Param: AZ_P // @DisplayName: Throttle acceleration controller P gain @@ -216,11 +223,11 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 1.000 100.000 // @Units: Hz // @User: Standard - AP_SUBGROUPINFO(pid_accel_z, "AZ_", 15, QuadPlane, AC_PID), + AP_SUBGROUPINFO(pid_accel_z, "AZ_", 16, QuadPlane, AC_PID), // @Group: P_ // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp - AP_SUBGROUPINFO(pos_control, "P", 16, QuadPlane, AC_PosControl), + AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), // @Param: VELZ_MAX // @DisplayName: Pilot maximum vertical speed @@ -229,7 +236,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 50 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("VELZ_MAX", 17, QuadPlane, pilot_velocity_z_max, 250), + AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max, 250), // @Param: ACCEL_Z // @DisplayName: Pilot vertical acceleration @@ -238,11 +245,47 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Range: 50 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250), + AP_GROUPINFO("ACCEL_Z", 19, QuadPlane, pilot_accel_z, 250), // @Group: WP_ // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp - AP_SUBGROUPINFO(wp_nav, "WP_", 19, QuadPlane, AC_WPNav), + AP_SUBGROUPPTR(wp_nav, "WP_", 20, QuadPlane, AC_WPNav), + + // @Param: RC_SPEED + // @DisplayName: RC output speed in Hz + // @Description: This is the PWM refresh rate in Hz for QuadPlane quad motors + // @Units: Hz + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RC_SPEED", 21, QuadPlane, rc_speed, 490), + + // @Param: THR_MIN_PWM + // @DisplayName: Minimum PWM output + // @Description: This is the minimum PWM output for the quad motors + // @Units: Hz + // @Range: 800 2200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("THR_MIN_PWM", 22, QuadPlane, thr_min_pwm, 1000), + + // @Param: THR_MAX_PWM + // @DisplayName: Maximum PWM output + // @Description: This is the maximum PWM output for the quad motors + // @Units: Hz + // @Range: 800 2200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("THR_MIN_PWM", 23, QuadPlane, thr_max_pwm, 2000), + + // @Param: ASSIST_SPEED + // @DisplayName: Quadplane assistance speed + // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition + // @Units: m/s + // @Range: 0 100 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("ASSIST_SPEED", 24, QuadPlane, assist_speed, 0), AP_GROUPEND }; @@ -255,18 +298,56 @@ QuadPlane::QuadPlane(AP_AHRS_NavEKF &_ahrs) : void QuadPlane::setup(void) { - motors.set_frame_orientation(AP_MOTORS_QUADPLANE); - motors.set_throttle_range(0, 1000, 2000); - motors.set_hover_throttle(500); - motors.set_update_rate(490); - motors.set_interlock(true); - motors.set_loop_rate(plane.ins.get_sample_rate()); - attitude_control.set_dt(plane.ins.get_loop_delta_t()); + if (!enable || initialised || hal.util->get_soft_armed()) { + return; + } + initialised = true; + + /* + dynamically allocate the key objects for quadplane. This ensures + that the objects don't affect the vehicle unless enabled and + also saves memory when not in use + */ + motors = new AP_MotorsQuad(plane.ins.get_sample_rate()); + if (!motors) { + AP_HAL::panic("Unable to allocate motors"); + } + AP_Param::load_object_from_eeprom(motors, motors->var_info); + attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, + p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, + pid_rate_roll, pid_rate_pitch, pid_rate_yaw); + if (!attitude_control) { + AP_HAL::panic("Unable to allocate attitude_control"); + } + AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); + pos_control = new AC_PosControl(ahrs, inertial_nav, *motors, *attitude_control, + p_alt_hold, p_vel_z, pid_accel_z, + p_pos_xy, pi_vel_xy); + if (!pos_control) { + AP_HAL::panic("Unable to allocate pos_control"); + } + AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); + wp_nav = new AC_WPNav(inertial_nav, ahrs, *pos_control, *attitude_control); + if (!pos_control) { + AP_HAL::panic("Unable to allocate wp_nav"); + } + AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); + + motors->set_frame_orientation(AP_MOTORS_QUADPLANE); + motors->set_throttle_range(0, thr_min_pwm, thr_max_pwm); + motors->set_hover_throttle(500); + motors->set_update_rate(rc_speed); + motors->set_interlock(true); + attitude_control->set_dt(plane.ins.get_loop_delta_t()); pid_rate_roll.set_dt(plane.ins.get_loop_delta_t()); pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); - pos_control.set_dt(plane.ins.get_loop_delta_t()); + pos_control->set_dt(plane.ins.get_loop_delta_t()); + + transition_state = TRANSITION_DONE; + + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "QuadPlane initialised"); } // init quadplane stabilize mode @@ -279,12 +360,12 @@ void QuadPlane::init_stabilize(void) void QuadPlane::hold_stabilize(float throttle_in) { // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, - plane.nav_pitch_cd, - get_pilot_desired_yaw_rate_cds(), - smoothing_gain); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, + plane.nav_pitch_cd, + get_pilot_desired_yaw_rate_cds(), + smoothing_gain); - attitude_control.set_throttle_out(throttle_in, true, 0); + attitude_control->set_throttle_out(throttle_in, true, 0); } // quadplane stabilize mode @@ -299,12 +380,12 @@ void QuadPlane::control_stabilize(void) void QuadPlane::init_hover(void) { // initialize vertical speeds and leash lengths - pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); - pos_control.set_accel_z(pilot_accel_z); + pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_accel_z(pilot_accel_z); // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->set_alt_target(inertial_nav.get_altitude()); + pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); init_throttle_wait(); } @@ -315,18 +396,18 @@ void QuadPlane::init_hover(void) void QuadPlane::hold_hover(float target_climb_rate) { // initialize vertical speeds and acceleration - pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); - pos_control.set_accel_z(pilot_accel_z); + pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_accel_z(pilot_accel_z); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, - plane.nav_pitch_cd, - get_pilot_desired_yaw_rate_cds(), - smoothing_gain); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, + plane.nav_pitch_cd, + get_pilot_desired_yaw_rate_cds(), + smoothing_gain); // call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); - pos_control.update_z_controller(); + pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false); + pos_control->update_z_controller(); } @@ -336,8 +417,8 @@ void QuadPlane::hold_hover(float target_climb_rate) void QuadPlane::control_hover(void) { if (throttle_wait) { - attitude_control.set_throttle_out_unstabilized(0, true, 0); - pos_control.relax_alt_hold_controllers(0); + attitude_control->set_throttle_out_unstabilized(0, true, 0); + pos_control->relax_alt_hold_controllers(0); } else { hold_hover(get_pilot_desired_climb_rate_cms()); } @@ -346,57 +427,76 @@ void QuadPlane::control_hover(void) void QuadPlane::init_loiter(void) { // set target to current position - wp_nav.init_loiter_target(); + wp_nav->init_loiter_target(); // initialize vertical speed and acceleration - pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); - pos_control.set_accel_z(pilot_accel_z); + pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_accel_z(pilot_accel_z); // initialise position and desired velocity - pos_control.set_alt_target(inertial_nav.get_altitude()); - pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->set_alt_target(inertial_nav.get_altitude()); + pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); init_throttle_wait(); } +// crude landing detector to prevent tipover +bool QuadPlane::should_relax(void) +{ + bool motor_at_lower_limit = motors->limit.throttle_lower && motors->is_throttle_mix_min(); + if (!motor_at_lower_limit) { + motors_lower_limit_start_ms = 0; + } + if (motor_at_lower_limit && motors_lower_limit_start_ms == 0) { + motors_lower_limit_start_ms = millis(); + } + bool relax_loiter = motors_lower_limit_start_ms != 0 && (millis() - motors_lower_limit_start_ms) > 1000; + return relax_loiter; +} + // run quadplane loiter controller void QuadPlane::control_loiter() { if (throttle_wait) { - attitude_control.set_throttle_out_unstabilized(0, true, 0); - pos_control.relax_alt_hold_controllers(0); - wp_nav.init_loiter_target(); + attitude_control->set_throttle_out_unstabilized(0, true, 0); + pos_control->relax_alt_hold_controllers(0); + wp_nav->init_loiter_target(); return; } + + + if (should_relax()) { + wp_nav->loiter_soften_for_landing(); + } // initialize vertical speed and acceleration - pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); - pos_control.set_accel_z(pilot_accel_z); + pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_accel_z(pilot_accel_z); // process pilot's roll and pitch input - wp_nav.set_pilot_desired_acceleration(plane.channel_roll->control_in, - plane.channel_pitch->control_in); + wp_nav->set_pilot_desired_acceleration(plane.channel_roll->control_in, + plane.channel_pitch->control_in); // Update EKF speed limit - used to limit speed when we are using optical flow float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), - wp_nav.get_pitch(), - get_pilot_desired_yaw_rate_cds()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), + wp_nav->get_pitch(), + get_pilot_desired_yaw_rate_cds()); // nav roll and pitch are controller by loiter controller - plane.nav_roll_cd = wp_nav.get_roll(); - plane.nav_pitch_cd = wp_nav.get_pitch(); + plane.nav_roll_cd = wp_nav->get_roll(); + plane.nav_pitch_cd = wp_nav->get_pitch(); // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false); - pos_control.update_z_controller(); + pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false); + pos_control->update_z_controller(); } /* @@ -404,6 +504,10 @@ void QuadPlane::control_loiter() */ float QuadPlane::get_pilot_desired_yaw_rate_cds(void) { + if (assisted_flight) { + // use bank angle to get desired yaw rate + return desired_yaw_rate_cds(); + } const float yaw_rate_max_dps = 100; return plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps; } @@ -411,7 +515,13 @@ float QuadPlane::get_pilot_desired_yaw_rate_cds(void) // get pilot desired climb rate in cm/s float QuadPlane::get_pilot_desired_climb_rate_cms(void) { - return pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0; + if (plane.failsafe.ch3_failsafe || plane.failsafe.ch3_counter > 0) { + // descend at 0.5m/s for now + return -50; + } + uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); + uint16_t trim = (plane.channel_throttle->radio_max + plane.channel_throttle->radio_min)/2; + return pilot_velocity_z_max * plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) / 100.0f; } @@ -431,13 +541,47 @@ void QuadPlane::init_throttle_wait(void) // set motor arming void QuadPlane::set_armed(bool armed) { - motors.armed(armed); + if (!initialised) { + return; + } + motors->armed(armed); if (armed) { - motors.enable(); + motors->enable(); } } +/* + estimate desired climb rate for assistance (in cm/s) + */ +float QuadPlane::assist_climb_rate_cms(void) +{ + if (plane.auto_throttle_mode) { + // ask TECS for its desired climb rate + return plane.TECS_controller.get_height_rate_demand()*100; + } + // otherwise estimate from pilot input + float climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); + climb_rate *= plane.channel_throttle->control_in; + return climb_rate; +} + +/* + calculate desired yaw rate for assistance + */ +float QuadPlane::desired_yaw_rate_cds(void) +{ + float aspeed; + if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) { + aspeed = plane.aparm.airspeed_min; + } + if (aspeed < 1) { + aspeed = 1; + } + float yaw_rate = degrees(GRAVITY_MSS * tanf(radians(plane.nav_roll_cd*0.01f))/aspeed) * 100; + return yaw_rate; +} + /* update for transition from quadplane to fixed wing mode */ @@ -445,26 +589,44 @@ void QuadPlane::update_transition(void) { if (plane.control_mode == MANUAL) { // in manual mode quad motors are always off - motors.output_min(); + motors->output_min(); transition_state = TRANSITION_DONE; return; } + float aspeed; + bool have_airspeed = ahrs.airspeed_estimate(&aspeed); + + /* + see if we should provide some assistance + */ + if (have_airspeed && aspeed < assist_speed && + (plane.auto_throttle_mode || plane.channel_throttle->control_in>0)) { + // the quad should provide some assistance to the plane + transition_state = TRANSITION_AIRSPEED_WAIT; + transition_start_ms = millis(); + assisted_flight = true; + } else { + assisted_flight = false; + } + switch (transition_state) { case TRANSITION_AIRSPEED_WAIT: { // we hold in hover until the required airspeed is reached if (transition_start_ms == 0) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition airspeed wait"); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed wait"); transition_start_ms = millis(); } - float aspeed; - if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) { + if (have_airspeed && aspeed > plane.aparm.airspeed_min && !assisted_flight) { transition_start_ms = millis(); transition_state = TRANSITION_TIMER; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition airspeed reached %.1f", aspeed); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", aspeed); } - hold_hover(0); + hold_hover(assist_climb_rate_cms()); + attitude_control->rate_controller_run(); + motors->output(); + last_throttle = motors->get_throttle(); break; } @@ -473,19 +635,20 @@ void QuadPlane::update_transition(void) // transition time, but continue to stabilize if (millis() - transition_start_ms > (unsigned)transition_time_ms) { transition_state = TRANSITION_DONE; - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Transition done"); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done"); } float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms; if (throttle_scaled < 0) { throttle_scaled = 0; } hold_stabilize(throttle_scaled); - motors.output(); + attitude_control->rate_controller_run(); + motors->output(); break; } case TRANSITION_DONE: - motors.output_min(); + motors->output_min(); break; } } @@ -495,23 +658,161 @@ void QuadPlane::update_transition(void) */ void QuadPlane::update(void) { - if (plane.control_mode != QSTABILIZE && - plane.control_mode != QHOVER && - plane.control_mode != QLOITER) { + if (!enable) { + return; + } + if (!initialised) { + setup(); + } + + bool quad_mode = (plane.control_mode == QSTABILIZE || + plane.control_mode == QHOVER || + plane.control_mode == QLOITER || + (plane.control_mode == AUTO && plane.auto_state.vtol_mode)); + + if (!quad_mode) { update_transition(); } else { + assisted_flight = false; + // run low level rate controllers - attitude_control.rate_controller_run(); + attitude_control->rate_controller_run(); // output to motors - motors.output(); + motors->output(); transition_start_ms = 0; transition_state = TRANSITION_AIRSPEED_WAIT; - last_throttle = motors.get_throttle(); + last_throttle = motors->get_throttle(); } // disable throttle_wait when throttle rises above 10% - if (throttle_wait && plane.channel_throttle->control_in > 10) { + if (throttle_wait && + (plane.channel_throttle->control_in > 10 || + plane.failsafe.ch3_failsafe || + plane.failsafe.ch3_counter>0)) { throttle_wait = false; } } + +/* + update control mode for quadplane modes + */ +void QuadPlane::control_run(void) +{ + if (!initialised) { + return; + } + + switch (plane.control_mode) { + case QSTABILIZE: + control_stabilize(); + break; + case QHOVER: + control_hover(); + break; + case QLOITER: + control_loiter(); + break; + default: + break; + } + // we also stabilize using fixed wing surfaces + float speed_scaler = plane.get_speed_scaler(); + plane.stabilize_roll(speed_scaler); + plane.stabilize_pitch(speed_scaler); +} + +/* + enter a quadplane mode + */ +bool QuadPlane::init_mode(void) +{ + setup(); + if (!initialised) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "QuadPlane mode refused"); + return false; + } + switch (plane.control_mode) { + case QSTABILIZE: + init_stabilize(); + break; + case QHOVER: + init_hover(); + break; + case QLOITER: + init_loiter(); + break; + default: + break; + } + return true; +} + +/* + handle a MAVLink DO_VTOL_TRANSITION + */ +bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet) +{ + if (!available()) { + plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL not available"); + return MAV_RESULT_FAILED; + } + if (plane.control_mode != AUTO) { + plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); + return MAV_RESULT_FAILED; + } + switch ((uint8_t)packet.param1) { + case MAV_VTOL_STATE_MC: + if (!plane.auto_state.vtol_mode) { + plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); + } + plane.auto_state.vtol_mode = true; + return MAV_RESULT_ACCEPTED; + case MAV_VTOL_STATE_FW: + if (plane.auto_state.vtol_mode) { + plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Exited VTOL mode"); + } + plane.auto_state.vtol_mode = false; + return MAV_RESULT_ACCEPTED; + } + + plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Invalid VTOL mode"); + return MAV_RESULT_FAILED; +} + +/* + handle auto-mode when auto_state.vtol_mode is true + */ +void QuadPlane::control_auto(const Location &loc) +{ + Location origin = inertial_nav.get_origin(); + Vector2f diff2d; + Vector3f target; + diff2d = location_diff(origin, loc); + target.x = diff2d.x * 100; + target.y = diff2d.y * 100; + target.z = loc.alt - origin.alt; + + printf("target %.2f %.2f %.2f\n", target.x*0.01, target.y*0.01, target.z*0.01); + + if (!locations_are_same(loc, last_auto_target)) { + wp_nav->set_wp_destination(target); + last_auto_target = loc; + } + + // initialize vertical speed and acceleration + pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_accel_z(pilot_accel_z); + + // run loiter controller + wp_nav->update_wpnav(); + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_nav->get_yaw(), true); + + // nav roll and pitch are controller by loiter controller + plane.nav_roll_cd = wp_nav->get_roll(); + plane.nav_pitch_cd = wp_nav->get_pitch(); + + pos_control->update_z_controller(); +} diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 8efc5b59c1..4d6bb5598b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -19,29 +19,27 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - // setup quadplane + void control_run(void); + void control_auto(const Location &loc); + bool init_mode(void); void setup(void); - // main entry points for VTOL flight modes - void init_stabilize(void); - void control_stabilize(void); - - void init_hover(void); - void control_hover(void); - - void init_loiter(void); - void control_loiter(void); - // update transition handling void update(void); // set motor arming void set_armed(bool armed); - + + // is VTOL available? + bool available(void) const { + return initialised; + } + + bool handle_do_vtol_transition(const mavlink_command_long_t &packet); + private: AP_AHRS_NavEKF &ahrs; AP_Vehicle::MultiCopter aparm; - AP_MotorsQuad motors{50}; AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02}; AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02}; AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02}; @@ -49,10 +47,6 @@ private: AC_P p_stabilize_pitch{4.5}; AC_P p_stabilize_yaw{4.5}; - AC_AttitudeControl_Multi attitude_control{ahrs, aparm, motors, - p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, - pid_rate_roll, pid_rate_pitch, pid_rate_yaw}; - AP_InertialNav_NavEKF inertial_nav{ahrs}; AC_P p_pos_xy{1}; @@ -61,11 +55,10 @@ private: AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02}; AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02}; - AC_PosControl pos_control{ahrs, inertial_nav, motors, attitude_control, - p_alt_hold, p_vel_z, pid_accel_z, - p_pos_xy, pi_vel_xy}; - - AC_WPNav wp_nav{inertial_nav, ahrs, pos_control, attitude_control}; + AP_MotorsQuad *motors; + AC_AttitudeControl_Multi *attitude_control; + AC_PosControl *pos_control; + AC_WPNav *wp_nav; // maximum vertical velocity the pilot may request AP_Int16 pilot_velocity_z_max; @@ -90,12 +83,43 @@ private: // initialise throttle_wait when entering mode void init_throttle_wait(); + + // main entry points for VTOL flight modes + void init_stabilize(void); + void control_stabilize(void); + + void init_hover(void); + void control_hover(void); + + void init_loiter(void); + void control_loiter(void); + + float assist_climb_rate_cms(void); + + // calculate desired yaw rate for assistance + float desired_yaw_rate_cds(void); + + bool should_relax(void); AP_Int16 transition_time_ms; + + AP_Int16 rc_speed; + + // min and max PWM for throttle + AP_Int16 thr_min_pwm; + AP_Int16 thr_max_pwm; + + // speed below which quad assistance is given + AP_Float assist_speed; + + AP_Int8 enable; + bool initialised; // timer start for transition uint32_t transition_start_ms; + Location last_auto_target; + // last throttle value when active float last_throttle; @@ -110,4 +134,10 @@ private: // true when waiting for pilot throttle bool throttle_wait; + + // true when quad is assisting a fixed wing mode + bool assisted_flight; + + // time when motors reached lower limit + uint32_t motors_lower_limit_start_ms; }; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 8c63237b4c..582e6773e7 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -419,6 +419,7 @@ void Plane::set_mode(enum FlightMode mode) case AUTO: auto_throttle_mode = true; + auto_state.vtol_mode = false; next_WP_loc = prev_WP_loc = current_loc; // start or resume the mission, based on MIS_AUTORESET mission.start_or_resume(); @@ -447,18 +448,13 @@ void Plane::set_mode(enum FlightMode mode) break; case QSTABILIZE: - auto_throttle_mode = false; - quadplane.init_stabilize(); - break; - case QHOVER: - auto_throttle_mode = false; - quadplane.init_hover(); - break; - case QLOITER: - auto_throttle_mode = false; - quadplane.init_loiter(); + if (!quadplane.init_mode()) { + control_mode = previous_mode; + } else { + auto_throttle_mode = false; + } break; } diff --git a/ArduPlane/wscript b/ArduPlane/wscript index f6e0b63e00..1aacbdfe55 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -28,6 +28,11 @@ def build(bld): 'AP_ServoRelayEvents', 'AP_SpdHgtControl', 'AP_TECS', + 'AP_InertialNav', + 'AC_WPNav', + 'AC_AttitudeControl', + 'AP_Motors', + 'AC_PID' ], ) From 9ae4c3ec9948f41daa3bf246a4348ca55da10cea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Dec 2015 22:43:43 +1100 Subject: [PATCH 072/109] Plane: mark Q_ENABLE parameter as an enable parameter --- ArduPlane/quadplane.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a39f4ce180..d2d2e81746 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -9,7 +9,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Description: This enables QuadPlane functionality, assuming quad motors on outputs 5 to 8 // @Values: 0:Disable,1:Enable // @User: Standard - AP_GROUPINFO("ENABLE", 1, QuadPlane, enable, 0), + AP_GROUPINFO_FLAGS("ENABLE", 1, QuadPlane, enable, 0, AP_PARAM_FLAG_ENABLE), // @Group: M_ // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp From aab98ff757bdfe01dd8f736178dfbcc738016c93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 15:36:03 +1100 Subject: [PATCH 073/109] Plane: fixed quad yaw assistance during transition timer --- ArduPlane/quadplane.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d2d2e81746..268ca3ce91 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -641,6 +641,7 @@ void QuadPlane::update_transition(void) if (throttle_scaled < 0) { throttle_scaled = 0; } + assisted_flight = true; hold_stabilize(throttle_scaled); attitude_control->rate_controller_run(); motors->output(); From 4f70ca22c18a113630a1b46782231b10d1ff1ef8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 17:36:18 +1100 Subject: [PATCH 074/109] AP_SpdHgtControl: added VTOL flight stage --- libraries/AP_SpdHgtControl/AP_SpdHgtControl.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h index a9c56e5420..8ac1e0b8a4 100644 --- a/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h +++ b/libraries/AP_SpdHgtControl/AP_SpdHgtControl.h @@ -31,9 +31,10 @@ public: enum FlightStage { FLIGHT_NORMAL = 1, FLIGHT_TAKEOFF = 2, - FLIGHT_LAND_APPROACH = 3, - FLIGHT_LAND_FINAL = 4, - FLIGHT_LAND_ABORT = 5 + FLIGHT_VTOL = 3, + FLIGHT_LAND_APPROACH = 4, + FLIGHT_LAND_FINAL = 5, + FLIGHT_LAND_ABORT = 6 }; // Update of the pitch and throttle demands From b3bd83b1c6ce5d96efe74fffdfada0e3c716f5f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 17:36:29 +1100 Subject: [PATCH 075/109] AP_TECS: no underspeed when in VTOL --- libraries/AP_TECS/AP_TECS.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 320c941817..94800d5f77 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -454,7 +454,9 @@ void AP_TECS::_update_height_demand(void) void AP_TECS::_detect_underspeed(void) { - if (((_integ5_state < _TASmin * 0.9f) && + if (_flight_stage == AP_TECS::FLIGHT_VTOL) { + _underspeed = false; + } else if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f) && _flight_stage != AP_TECS::FLIGHT_LAND_FINAL) || ((_height < _hgt_dem_adj) && _underspeed)) From 7afa2a493d4f2ac57acc355c0f2bbb1b125c40ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 17:36:41 +1100 Subject: [PATCH 076/109] Plane: implement VTOL flight stage --- ArduPlane/ArduPlane.cpp | 12 ++++++++++-- ArduPlane/is_flying.cpp | 9 +++++++++ ArduPlane/quadplane.h | 5 +++++ 3 files changed, 24 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ed3a562392..046598c891 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -824,6 +824,7 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs) case AP_SpdHgtControl::FLIGHT_LAND_FINAL: case AP_SpdHgtControl::FLIGHT_NORMAL: + case AP_SpdHgtControl::FLIGHT_VTOL: case AP_SpdHgtControl::FLIGHT_TAKEOFF: break; } @@ -866,7 +867,9 @@ void Plane::update_flight_stage(void) // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { - if (auto_state.takeoff_complete == false) { + if (auto_state.vtol_mode) { + set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); + } else if (auto_state.takeoff_complete == false) { set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { @@ -887,10 +890,15 @@ void Plane::update_flight_stage(void) set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } - + } else if (quadplane.in_assisted_flight()) { + set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } + } else if (control_mode == QSTABILIZE || + control_mode == QHOVER || + control_mode == QLOITER) { + set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 6ea3050193..e10717d327 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -83,6 +83,10 @@ void Plane::update_is_flying_5Hz(void) } break; + case AP_SpdHgtControl::FLIGHT_VTOL: + // TODO: detect ground impacts + break; + case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: if (fabsf(auto_state.sink_rate) > 0.2f) { is_flying_bool = true; @@ -198,6 +202,11 @@ void Plane::crash_detection_update(void) // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; + case AP_SpdHgtControl::FLIGHT_VTOL: + // we need a totally new method for this + crashed = false; + break; + case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: crashed = true; // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 4d6bb5598b..ce071480b8 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -35,6 +35,11 @@ public: return initialised; } + // is quadplane assisting? + bool in_assisted_flight(void) const { + return available() && assisted_flight; + } + bool handle_do_vtol_transition(const mavlink_command_long_t &packet); private: From 5e784ddb5c215ed5382e76e139f4e78e92ade734 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 18:18:53 +1100 Subject: [PATCH 077/109] Plane: consider flying status for throttle_wait --- ArduPlane/ArduPlane.cpp | 11 +++++++---- ArduPlane/Attitude.cpp | 2 +- ArduPlane/quadplane.cpp | 20 +++++++++++++++----- ArduPlane/quadplane.h | 3 +++ 4 files changed, 26 insertions(+), 10 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 046598c891..1673e00408 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -895,10 +895,6 @@ void Plane::update_flight_stage(void) } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } - } else if (control_mode == QSTABILIZE || - control_mode == QHOVER || - control_mode == QLOITER) { - set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } @@ -913,6 +909,13 @@ void Plane::update_flight_stage(void) if (should_log(MASK_LOG_TECS)) { Log_Write_TECS_Tuning(); } + } else if (control_mode == QSTABILIZE || + control_mode == QHOVER || + control_mode == QLOITER || + quadplane.in_assisted_flight()) { + set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); + } else { + set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } // tell AHRS the airspeed to true airspeed ratio diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 1fc6ae133b..8ebddcd595 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -1103,7 +1103,7 @@ void Plane::demo_servos(uint8_t i) void Plane::adjust_nav_pitch_throttle(void) { uint8_t throttle = throttle_percentage(); - if (throttle < aparm.throttle_cruise) { + if (throttle < aparm.throttle_cruise && flight_stage != AP_SpdHgtControl::FLIGHT_VTOL) { float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise; nav_pitch_cd -= g.stab_pitch_down * 100.0f * p; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 268ca3ce91..69d0438f03 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -469,6 +469,11 @@ void QuadPlane::control_loiter() if (should_relax()) { wp_nav->loiter_soften_for_landing(); } + + if (millis() - last_loiter_ms > 500) { + wp_nav->init_loiter_target(); + } + last_loiter_ms = millis(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); @@ -601,7 +606,9 @@ void QuadPlane::update_transition(void) see if we should provide some assistance */ if (have_airspeed && aspeed < assist_speed && - (plane.auto_throttle_mode || plane.channel_throttle->control_in>0)) { + (plane.auto_throttle_mode || + plane.channel_throttle->control_in>0 || + plane.is_flying())) { // the quad should provide some assistance to the plane transition_state = TRANSITION_AIRSPEED_WAIT; transition_start_ms = millis(); @@ -682,7 +689,11 @@ void QuadPlane::update(void) // output to motors motors->output(); transition_start_ms = 0; - transition_state = TRANSITION_AIRSPEED_WAIT; + if (throttle_wait && !plane.is_flying()) { + transition_state = TRANSITION_DONE; + } else { + transition_state = TRANSITION_AIRSPEED_WAIT; + } last_throttle = motors->get_throttle(); } @@ -794,12 +805,11 @@ void QuadPlane::control_auto(const Location &loc) target.y = diff2d.y * 100; target.z = loc.alt - origin.alt; - printf("target %.2f %.2f %.2f\n", target.x*0.01, target.y*0.01, target.z*0.01); - - if (!locations_are_same(loc, last_auto_target)) { + if (!locations_are_same(loc, last_auto_target) || millis() - last_loiter_ms > 500) { wp_nav->set_wp_destination(target); last_auto_target = loc; } + last_loiter_ms = millis(); // initialize vertical speed and acceleration pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ce071480b8..f2d9de8bdf 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -145,4 +145,7 @@ private: // time when motors reached lower limit uint32_t motors_lower_limit_start_ms; + + // time we last set the loiter target + uint32_t last_loiter_ms; }; From 712a45eb22c3cf9a960ab46b5e9471c12a03281f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Jan 2016 20:41:17 +1100 Subject: [PATCH 078/109] GCS_MAVLink: added VTOL_TAKEOFF and VTOL_LAND --- .../message_definitions/common.xml | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index 87b757b997..2bbf5ca587 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -677,6 +677,26 @@ Longitude/Y of goal Altitude/Z of goal + + Takeoff from ground using VTOL mode + Empty + Empty + Empty + Yaw angle in degrees + Latitude + Longitude + Altitude + + + Land using VTOL mode + Empty + Empty + Empty + Yaw angle in degrees + Latitude + Longitude + Altitude +