ardupilot/libraries/AP_Winch/AP_Winch_Daiwa.h

109 lines
4.3 KiB
C
Raw Normal View History

2020-07-24 05:29:43 -03:00
/*
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 <http://www.gnu.org/licenses/>.
*/
/*
The Daiwa winch is produced by a Japanese company called Okaya.
There are two PWM controls supported:
- the rate control for releasing (high PWM) or retracting (low PWM) the line
- the clutch control has three settings:
- released (high PWM) lets the winch spin freely
- engaged soft (middle PWM) allows the rate control to work but it may slip
if too much tension is required. This driver does not use this setting.
- engaged hard (low PWM) allows the rate control to work regardless of tension.
A telemetry output from the winch is connected to the autopilot and provides
the amount of line released, tension, clutch setting, etc.
*/
2020-07-24 05:29:43 -03:00
#pragma once
#include <AP_Winch/AP_Winch_Backend.h>
#include <SRV_Channel/SRV_Channel.h>
class AP_Winch_Daiwa : public AP_Winch_Backend {
public:
using AP_Winch_Backend::AP_Winch_Backend;
2020-07-24 05:29:43 -03:00
// true if winch is healthy
bool healthy() const override;
// initialise the winch
void init() override;
// read telemetry from the winch and output controls
2020-07-24 05:29:43 -03:00
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.003333f; // convert winch counter to meters
2020-07-24 05:29:43 -03:00
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;
};