mirror of https://github.com/ArduPilot/ardupilot
AP_Winch_Daiwa: fixes from peer review
simplify constructor init calls backend init fixup parsing to remove unnecessary INT32_MAX/MIN check fixup comments
This commit is contained in:
Randy Mackay
parent
a6c8bb06ff
commit
b84423c110
|
|
@ -12,8 +12,8 @@ bool AP_Winch_Daiwa::healthy() const
|
|||
|
||||
void AP_Winch_Daiwa::init()
|
||||
{
|
||||
// initialise rc input and output
|
||||
init_input_and_output();
|
||||
// call superclass init
|
||||
AP_Winch_Backend::init();
|
||||
|
||||
// initialise serial connection to winch
|
||||
const AP_SerialManager &serial_manager = AP::serialmanager();
|
||||
|
|
@ -109,76 +109,71 @@ void AP_Winch_Daiwa::read_data_from_winch()
|
|||
// add digits to buffer
|
||||
buff[buff_len] = b;
|
||||
buff_len++;
|
||||
if (buff_len >= buff_len_max) {
|
||||
if (buff_len >= ARRAY_SIZE(buff)) {
|
||||
buff_len = 0;
|
||||
parse_state = ParseState::WAITING_FOR_TIME;
|
||||
}
|
||||
} else if (b == ',' || b == '\r') {
|
||||
// comma or carriage return signals end of current value
|
||||
// parse number received and empty buffer
|
||||
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
|
||||
long int value = (int32_t)strtol(buff, nullptr, 16);
|
||||
buff_len = 0;
|
||||
switch (parse_state) {
|
||||
case ParseState::WAITING_FOR_TIME:
|
||||
intermediate.time_ms = (uint32_t)value;
|
||||
parse_state = ParseState::WAITING_FOR_SPOOL;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_SPOOL:
|
||||
intermediate.line_length = (int32_t)value * line_length_correction_factor;
|
||||
parse_state = ParseState::WAITING_FOR_TENSION1;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_TENSION1:
|
||||
intermediate.tension_uncorrected = (uint16_t)value;
|
||||
parse_state = ParseState::WAITING_FOR_TENSION2;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_TENSION2:
|
||||
intermediate.tension_corrected = (uint16_t)value;
|
||||
parse_state = ParseState::WAITING_FOR_THREAD_END;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_THREAD_END:
|
||||
intermediate.thread_end = (value > 0);
|
||||
parse_state = ParseState::WAITING_FOR_MOVING;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_MOVING:
|
||||
intermediate.moving = constrain_int32(value, 0, UINT8_MAX);
|
||||
parse_state = ParseState::WAITING_FOR_CLUTCH;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_CLUTCH:
|
||||
intermediate.clutch = constrain_int32(value, 0, UINT8_MAX);
|
||||
parse_state = ParseState::WAITING_FOR_SPEED;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_SPEED:
|
||||
intermediate.speed_pct = constrain_int32(value, 0, UINT8_MAX);
|
||||
parse_state = ParseState::WAITING_FOR_VOLTAGE;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_VOLTAGE:
|
||||
intermediate.voltage = (float)value * 0.1f;
|
||||
parse_state = ParseState::WAITING_FOR_CURRENT;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_CURRENT:
|
||||
intermediate.current = (float)value * 0.1f;
|
||||
parse_state = ParseState::WAITING_FOR_PCB_TEMP;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_PCB_TEMP:
|
||||
intermediate.pcb_temp = (float)value * 0.1f;
|
||||
parse_state = ParseState::WAITING_FOR_MOTOR_TEMP;
|
||||
break;
|
||||
case ParseState::WAITING_FOR_MOTOR_TEMP:
|
||||
intermediate.motor_temp = (float)value * 0.1f;
|
||||
// successfully parsed a complete message
|
||||
latest = intermediate;
|
||||
data_update_ms = AP_HAL::millis();
|
||||
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;
|
||||
}
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// carriage return or unexpected characters
|
||||
// line feed or unexpected characters
|
||||
buff_len = 0;
|
||||
parse_state = ParseState::WAITING_FOR_TIME;
|
||||
}
|
||||
|
|
@ -188,7 +183,7 @@ void AP_Winch_Daiwa::read_data_from_winch()
|
|||
// update pwm outputs to control winch
|
||||
void AP_Winch_Daiwa::control_winch()
|
||||
{
|
||||
uint32_t now_ms = AP_HAL::millis();
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
float dt = (now_ms - control_update_ms) / 1000.0f;
|
||||
if (dt > 1.0f) {
|
||||
dt = 0.0f;
|
||||
|
|
|
|||
|
|
@ -13,6 +13,19 @@
|
|||
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.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_Winch/AP_Winch_Backend.h>
|
||||
|
|
@ -21,8 +34,7 @@
|
|||
class AP_Winch_Daiwa : public AP_Winch_Backend {
|
||||
public:
|
||||
|
||||
AP_Winch_Daiwa(struct AP_Winch::Backend_Config &_config) :
|
||||
AP_Winch_Backend(_config) { }
|
||||
using AP_Winch_Backend::AP_Winch_Backend;
|
||||
|
||||
// true if winch is healthy
|
||||
bool healthy() const override;
|
||||
|
|
@ -30,7 +42,7 @@ public:
|
|||
// initialise the winch
|
||||
void init() override;
|
||||
|
||||
// control the winch
|
||||
// read telemetry from the winch and output controls
|
||||
void update() override;
|
||||
|
||||
// returns current length of line deployed
|
||||
|
|
|
|||
Loading…
Reference in New Issue