diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp index 51d9c62f1e..7a77da1422 100644 --- a/libraries/AP_Winch/AP_Winch.cpp +++ b/libraries/AP_Winch/AP_Winch.cpp @@ -1,5 +1,6 @@ #include "AP_Winch.h" #include "AP_Winch_Servo.h" +#include "AP_Winch_Daiwa.h" extern const AP_HAL::HAL& hal; @@ -10,7 +11,7 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = { // @DisplayName: Winch Type // @Description: Winch Type // @User: Standard - // @Values: 0:None, 1:Servo + // @Values: 0:None, 1:Servo, 2:Daiwa AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Winch, config.type, (int8_t)WinchType::NONE, AP_PARAM_FLAG_ENABLE), // @Param: _RATE_MAX @@ -69,6 +70,9 @@ void AP_Winch::init() case WinchType::SERVO: backend = new AP_Winch_Servo(config); break; + case WinchType::DAIWA: + backend = new AP_Winch_Daiwa(config); + break; default: break; } diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h index 458ab7c9a5..7da443df48 100644 --- a/libraries/AP_Winch/AP_Winch.h +++ b/libraries/AP_Winch/AP_Winch.h @@ -25,6 +25,7 @@ class AP_Winch_Backend; class AP_Winch { friend class AP_Winch_Backend; friend class AP_Winch_Servo; + friend class AP_Winch_Daiwa; public: AP_Winch(); @@ -75,6 +76,7 @@ private: enum class WinchType { NONE = 0, SERVO = 1, + DAIWA = 2 }; // winch states diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp new file mode 100644 index 0000000000..0cddaf047b --- /dev/null +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -0,0 +1,227 @@ +#include +#include + +extern const AP_HAL::HAL& hal; + +// true if winch is healthy +bool AP_Winch_Daiwa::healthy() const +{ + // healthy if we have received data within 3 seconds + return (AP_HAL::millis() - data_update_ms < 3000); +} + +void AP_Winch_Daiwa::init() +{ + // initialise rc input and output + init_input_and_output(); + + // initialise serial connection to winch + const AP_SerialManager &serial_manager = AP::serialmanager(); + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Winch, 0); + if (uart != nullptr) { + // always use baudrate of 115200 + uart->begin(115200); + } +} + +void AP_Winch_Daiwa::update() +{ + // return immediately if no servo is assigned to control the winch + if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) { + return; + } + + // read latest data from winch + read_data_from_winch(); + + // read pilot input + read_pilot_desired_rate(); + + // send outputs to winch + control_winch(); +} + +//send generator status +void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel) +{ + // prepare status bitmask + uint32_t status_bitmask = 0; + if (healthy()) { + status_bitmask |= MAV_WINCH_STATUS_HEALTHY; + } + if (latest.thread_end) { + status_bitmask |= MAV_WINCH_STATUS_FULLY_RETRACTED; + } + if (latest.moving > 0) { + status_bitmask |= MAV_WINCH_STATUS_MOVING; + } + if (latest.clutch > 0) { + status_bitmask |= MAV_WINCH_STATUS_CLUTCH_ENGAGED; + } + + // convert speed percentage to absolute speed + const float speed_ms = fabsf(config.rate_max) * (float)latest.speed_pct; + + // send status + mavlink_msg_winch_status_send( + channel.get_chan(), + AP_HAL::micros64(), + latest.line_length, + speed_ms, + (float)latest.tension_corrected * 0.01f, + latest.voltage, + latest.current, + latest.motor_temp, + status_bitmask); +} + +// write log +void AP_Winch_Daiwa::write_log() +{ + AP::logger().Write_Winch( + healthy(), + latest.thread_end, + latest.moving, + latest.clutch, + (uint8_t)config.control_mode, + config.length_desired, + get_current_length(), + config.rate_desired, + latest.tension_corrected, + latest.voltage, + constrain_int16(latest.motor_temp, INT8_MIN, INT8_MAX)); +} + +// read incoming data from winch and update intermediate and latest structures +void AP_Winch_Daiwa::read_data_from_winch() +{ + // return immediately if serial port is not configured + if (uart == nullptr) { + return; + } + + // read any available characters from serial port and send to GCS + int16_t nbytes = uart->available(); + while (nbytes-- > 0) { + int16_t b = uart->read(); + + if ((b >= '0' && b <= '9') || (b >= 'A' && b <= 'F') || (b >= 'a' && b <= 'f')) { + // add digits to buffer + buff[buff_len] = b; + buff_len++; + if (buff_len >= buff_len_max) { + buff_len = 0; + parse_state = ParseState::WAITING_FOR_TIME; + } + } else if (b == ',' || b == '\r') { + // comma or carriage return signals end of current value + buff[buff_len] = '\0'; + long int ret = (int32_t)strtol(buff, nullptr, 16); + if (ret >= (long)INT32_MAX || ret <= (long)INT32_MIN) { + // failed to get valid number, throw away packet + parse_state = ParseState::WAITING_FOR_TIME; + } else { + // parse number received and empty buffer + buff_len = 0; + switch (parse_state) { + case ParseState::WAITING_FOR_TIME: + intermediate.time_ms = (uint32_t)ret; + parse_state = ParseState::WAITING_FOR_SPOOL; + break; + case ParseState::WAITING_FOR_SPOOL: + intermediate.line_length = (int32_t)ret * line_length_correction_factor; + parse_state = ParseState::WAITING_FOR_TENSION1; + break; + case ParseState::WAITING_FOR_TENSION1: + intermediate.tension_uncorrected = (uint16_t)ret; + parse_state = ParseState::WAITING_FOR_TENSION2; + break; + case ParseState::WAITING_FOR_TENSION2: + intermediate.tension_corrected = (uint16_t)ret; + parse_state = ParseState::WAITING_FOR_THREAD_END; + break; + case ParseState::WAITING_FOR_THREAD_END: + intermediate.thread_end = (ret > 0); + parse_state = ParseState::WAITING_FOR_MOVING; + break; + case ParseState::WAITING_FOR_MOVING: + intermediate.moving = constrain_int16(ret, 0, UINT8_MAX); + parse_state = ParseState::WAITING_FOR_CLUTCH; + break; + case ParseState::WAITING_FOR_CLUTCH: + intermediate.clutch = constrain_int16(ret, 0, UINT8_MAX); + parse_state = ParseState::WAITING_FOR_SPEED; + break; + case ParseState::WAITING_FOR_SPEED: + intermediate.speed_pct = constrain_int16(ret, 0, UINT8_MAX); + parse_state = ParseState::WAITING_FOR_VOLTAGE; + break; + case ParseState::WAITING_FOR_VOLTAGE: + intermediate.voltage = (float)ret * 0.1f; + parse_state = ParseState::WAITING_FOR_CURRENT; + break; + case ParseState::WAITING_FOR_CURRENT: + intermediate.current = (float)ret * 0.1f; + parse_state = ParseState::WAITING_FOR_PCB_TEMP; + break; + case ParseState::WAITING_FOR_PCB_TEMP: + intermediate.pcb_temp = (float)ret * 0.1f; + parse_state = ParseState::WAITING_FOR_MOTOR_TEMP; + break; + case ParseState::WAITING_FOR_MOTOR_TEMP: + intermediate.motor_temp = (float)ret * 0.1f; + // successfully parsed a complete message + latest = intermediate; + data_update_ms = AP_HAL::millis(); + parse_state = ParseState::WAITING_FOR_TIME; + break; + } + } + } else { + // carriage return or unexpected characters + buff_len = 0; + parse_state = ParseState::WAITING_FOR_TIME; + } + } +} + +// update pwm outputs to control winch +void AP_Winch_Daiwa::control_winch() +{ + uint32_t now_ms = AP_HAL::millis(); + float dt = (now_ms - control_update_ms) / 1000.0f; + if (dt > 1.0f) { + dt = 0.0f; + } + control_update_ms = now_ms; + + // if real doing any control output trim value + if (config.control_mode == AP_Winch::ControlMode::RELAXED) { + // if not doing any control output release clutch and move winch to trim + SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MAX); + SRV_Channels::set_output_scaled(SRV_Channel::k_winch, 0); + + // rate used for acceleration limiting reset to zero + set_previous_rate(0.0f); + return; + } + + // release clutch + SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MIN); + + // if doing position control, calculate position error to desired rate + if ((config.control_mode == AP_Winch::ControlMode::POSITION) && healthy()) { + float position_error = config.length_desired - latest.line_length; + config.rate_desired = constrain_float(position_error * config.pos_p, -config.rate_max, config.rate_max); + } + + // apply acceleration limited to rate + const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt); + + // use linear interpolation to calculate output to move winch at desired rate + int16_t scaled_output = 0; + if (!is_zero(rate_limited)) { + scaled_output = linear_interpolate(output_dz, 1000, fabsf(rate_limited), 0, config.rate_max) * (is_positive(rate_limited) ? 1.0f : -1.0f); + } + SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output); +} diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.h b/libraries/AP_Winch/AP_Winch_Daiwa.h new file mode 100644 index 0000000000..782da20087 --- /dev/null +++ b/libraries/AP_Winch/AP_Winch_Daiwa.h @@ -0,0 +1,96 @@ +/* + 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 . + */ + +#pragma once + +#include +#include + +class AP_Winch_Daiwa : public AP_Winch_Backend { +public: + + AP_Winch_Daiwa(struct AP_Winch::Backend_Config &_config) : + AP_Winch_Backend(_config) { } + + // true if winch is healthy + bool healthy() const override; + + // initialise the winch + void init() override; + + // control the winch + void update() override; + + // returns current length of line deployed + float get_current_length() const override { return latest.line_length; } + + // send status to ground station + void send_status(const GCS_MAVLINK &channel) override; + + // write log + void write_log() override; + +private: + + // read incoming data from winch and update intermediate and latest structures + void read_data_from_winch(); + + // update pwm outputs to control winch + void control_winch(); + + static const uint8_t buff_len_max = 20; // buffer maximum length + static const int16_t output_dz = 100; // output deadzone in scale of -1000 to +1000 + const float line_length_correction_factor = 0.0357f; // convert winch counter to meters + + AP_HAL::UARTDriver *uart; + char buff[buff_len_max]; // buffer holding latest data from winch + uint8_t buff_len; // number of bytes in buff + + // winch data + // latest holds most recent complete data received + // intermediate holds partial results currently being processed + struct WinchData { + uint32_t time_ms; // winch system time in milliseconds + float line_length; // length of line released in meters + uint16_t tension_uncorrected; // uncorrected tension in grams (0 to 1024) + uint16_t tension_corrected; // corrected tension in grams (0 to 1024) + bool thread_end; // true if end of thread has been detected + uint8_t moving; // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset + uint8_t clutch; // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely + uint8_t speed_pct; // speed motor is moving as a percentage + float voltage; // battery voltage (in voltes) + float current; // current draw (in amps) + float pcb_temp; // PCB temp in C + float motor_temp; // motor temp in C + } latest, intermediate; + uint32_t data_update_ms; // system time that latest was last updated + uint32_t control_update_ms; // last time control_winch was called + + // parsing state + enum class ParseState : uint8_t { + WAITING_FOR_TIME = 0, + WAITING_FOR_SPOOL, + WAITING_FOR_TENSION1, + WAITING_FOR_TENSION2, + WAITING_FOR_THREAD_END, + WAITING_FOR_MOVING, + WAITING_FOR_CLUTCH, + WAITING_FOR_SPEED, + WAITING_FOR_VOLTAGE, + WAITING_FOR_CURRENT, + WAITING_FOR_PCB_TEMP, + WAITING_FOR_MOTOR_TEMP + } parse_state; +};