AP_Winch: Options param for init state and verbose output

This commit is contained in:
Randy Mackay 2023-09-25 16:35:54 +09:00
parent 28e5ea152e
commit 7a56d887b9
5 changed files with 119 additions and 0 deletions

View File

@ -2,6 +2,7 @@
#if AP_WINCH_ENABLED
#include <GCS_MAVLink/GCS.h>
#include "AP_Winch_PWM.h"
#include "AP_Winch_Daiwa.h"
@ -32,6 +33,13 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = {
// @User: Standard
AP_GROUPINFO("_POS_P", 3, AP_Winch, config.pos_p, 1.0f),
// @Param: _OPTIONS
// @DisplayName: Winch options
// @Description: Winch options
// @Bitmask: 0:Spin freely on startup, 1:Verbose output
// @User: Standard
AP_GROUPINFO("_OPTIONS", 4, AP_Winch, config.options, 3.0f),
// 4 was _RATE_PID
AP_GROUPEND
@ -81,6 +89,13 @@ void AP_Winch::init()
}
if (backend != nullptr) {
backend->init();
// initialise control mode
if (backend->option_enabled(Options::SpinFreelyOnStartup)) {
relax();
} else {
set_desired_rate(0);
}
}
}
@ -92,6 +107,11 @@ void AP_Winch::release_length(float length)
}
config.length_desired = backend->get_current_length() + length;
config.control_mode = ControlMode::POSITION;
// display verbose output to user
if (backend->option_enabled(Options::VerboseOutput)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Winch: lowering %4.1fm to %4.1fm", (double)length, (double)config.length_desired);
}
}
// deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)

View File

@ -82,6 +82,12 @@ private:
DAIWA = 2
};
// enum for OPTIONS parameter
enum class Options : int16_t {
SpinFreelyOnStartup = (1U << 0),
VerboseOutput = (1U << 1),
};
// winch states
enum class ControlMode : uint8_t {
RELAXED = 0, // winch is realxed
@ -94,6 +100,7 @@ private:
AP_Int8 type; // winch type
AP_Float rate_max; // deploy or retract rate maximum (in m/s).
AP_Float pos_p; // position error P gain
AP_Int16 options; // options bitmask
ControlMode control_mode; // state of winch control (using target position or target rate)
float length_desired; // target desired length (in meters)
float rate_desired; // target deploy rate (in m/s, +ve = deploying, -ve = retracting)

View File

@ -42,6 +42,11 @@ public:
// write log
virtual void write_log() = 0;
// helper to check if option enabled
bool option_enabled(AP_Winch::Options option) const {
return (config.options & uint16_t(option)) != 0;
}
protected:
// calculate the pilot desired rate (+ve deploys line, -ve retracts line, 0 stops) from rc input

View File

@ -8,6 +8,8 @@
extern const AP_HAL::HAL& hal;
const char* AP_Winch_Daiwa::send_text_prefix = "Winch:";
// true if winch is healthy
bool AP_Winch_Daiwa::healthy() const
{
@ -40,6 +42,9 @@ void AP_Winch_Daiwa::update()
// send outputs to winch
control_winch();
// update_user
update_user();
}
//send generator status
@ -222,4 +227,74 @@ void AP_Winch_Daiwa::control_winch()
SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
}
// update user with changes to winch state via send text messages
void AP_Winch_Daiwa::update_user()
{
// exit immediately if verbose output disabled
if (!option_enabled(AP_Winch::Options::VerboseOutput)) {
return;
}
// send updates at no more than 2hz
uint32_t now_ms = AP_HAL::millis();
if (now_ms - user_update.last_ms < 500) {
return;
}
bool update_sent = false;
// health change
const bool now_healthy = healthy();
if (user_update.healthy != now_healthy) {
user_update.healthy = now_healthy;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, now_healthy ? "healthy" : "not healthy");
update_sent = true;
}
// thread end
if (latest.thread_end && (user_update.thread_end != latest.thread_end)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s thread end", send_text_prefix);
update_sent = true;
}
user_update.thread_end = latest.thread_end;
// moving state
if (user_update.moving != latest.moving) {
// 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset
static const char* moving_str[] = {"stopped", "raising", "lowering", "free spinning", "zero reset"};
if (latest.moving < ARRAY_SIZE(moving_str)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, moving_str[latest.moving]);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s move state uknown", send_text_prefix);
}
update_sent = true;
}
user_update.moving = latest.moving;
// clutch state
if (user_update.clutch != latest.clutch) {
// 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely
static const char* clutch_str[] = {"off", "weak", "strong (free)"};
if (user_update.clutch < ARRAY_SIZE(clutch_str)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch %s", send_text_prefix, clutch_str[latest.moving]);
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch state uknown", send_text_prefix);
}
update_sent = true;
}
user_update.clutch = latest.clutch;
// length in meters
const float latest_line_length_rounded = roundf(latest.line_length);
if (!is_equal(user_update.line_length, latest_line_length_rounded)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %dm", send_text_prefix, (int)latest_line_length_rounded);
update_sent = true;
}
user_update.line_length = latest_line_length_rounded;
// record time message last sent to user
if (update_sent) {
user_update.last_ms = now_ms;
}
}
#endif // AP_WINCH_DAIWA_ENABLED

View File

@ -106,6 +106,18 @@ private:
WAITING_FOR_PCB_TEMP,
WAITING_FOR_MOTOR_TEMP
} parse_state;
// update user with changes to winch state via send text messages
static const char* send_text_prefix;// send text prefix string to reduce flash cost
void update_user();
struct {
uint32_t last_ms; // system time of last update to user
bool healthy; // latest reported health
float line_length;
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
} user_update;
};
#endif // AP_WINCH_DAIWA_ENABLED