Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Orca state machine bug fixes #4

Merged
merged 5 commits into from
Jan 16, 2025
Merged

Orca state machine bug fixes #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f59a926
Modified auto-zero state to fix sync errors with state of Orca. Added…
agregghai Dec 24, 2024
fc31f05
Merge remote-tracking branch 'origin/master' into orca_sm_fixes
agregghai Dec 24, 2024
b283a8d
wip
agregghai Jan 4, 2025
ea18e05
Tested on bench and cannot replicate failure to AZ. Some cleanup/chan…
agregg18 Jan 4, 2025
ec6b727
update flash script to work with balena builds
agregghai Jan 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions Tools/scripts/flash_via_remote.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ PASSWORD=$2
ARDUROVER_DIR=${3:-../../build/CubeOrangePlus/bin}

# Define the remote directory
REMOTE_DIR="/home/havoc/ardurover_flash"
REMOTE_DIR="/havoc/ardurover_flash"

# Check if the ARDUROVER_DIR exists and is a directory, if not, display error and exit
if [ ! -d "$ARDUROVER_DIR" ]; then
Expand All @@ -71,8 +71,8 @@ if [ $? -ne 0 ]; then
fi

# Copy uploader.py and contents of ARDUROVER_DIR to the remote machine
sshpass -p "$PASSWORD" scp -o StrictHostKeyChecking=no uploader.py "$IP_ADDRESS:$REMOTE_DIR/"
sshpass -p "$PASSWORD" scp -o StrictHostKeyChecking=no -r "$ARDUROVER_DIR"/* "$IP_ADDRESS:$REMOTE_DIR/"
sshpass -p "$PASSWORD" scp -O -o StrictHostKeyChecking=no uploader.py "$IP_ADDRESS:$REMOTE_DIR/"
sshpass -p "$PASSWORD" scp -O -o StrictHostKeyChecking=no -r "$ARDUROVER_DIR"/* "$IP_ADDRESS:$REMOTE_DIR/"

# Check the exit status of the scp commands, if they failed, display error and exit
if [ $? -ne 0 ]; then
Expand All @@ -83,7 +83,7 @@ fi
# Run the uploader.py script on the remote machine
sshpass -p "$PASSWORD" ssh -o StrictHostKeyChecking=no "$IP_ADDRESS" << EOF
cd "$REMOTE_DIR"
python3 uploader.py ardurover.apj
python3 uploader.py ardurover.apj --port /dev/ttyACM0
EOF

# Check the exit status of the ssh command, if it failed, display error and exit
Expand Down
176 changes: 99 additions & 77 deletions libraries/AP_IrisOrca/AP_IrisOrca.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,22 +24,19 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_SerialManager/AP_SerialManager.h>

#define IRISORCA_SERIAL_BAUD 19200 // communication is always at 19200
#define IRISORCA_SERIAL_PARITY 2 // communication is always even parity
#define IRISORCA_LOG_ORCA_INTERVAL_MS 5000// log ORCA message at this interval in milliseconds
#define IRISORCA_SEND_ACTUATOR_POSITION_INTERVAL_MS 100 // actuator position sent at 10hz if connected to actuator
#define IRISORCA_SEND_ACTUATOR_STATUS_REQUEST_INTERVAL_MS 400 // actuator status requested every 0.4sec if connected to actuator
#define IRISORCA_SEND_ACTUATOR_PARAM_REQUEST_INTERVAL_MS 400 // actuator param requested every 0.4sec if connected to actuator
#define IRISORCA_REPLY_TIMEOUT_MS 25 // stop waiting for replies after 25ms
#define IRISORCA_ERROR_REPORT_INTERVAL_MAX_MS 10000 // errors reported to user at no less than once every 10 seconds
#define IRISORCA_SERIAL_BAUD 19200 // communication is always at 19200
#define IRISORCA_SERIAL_PARITY 2 // communication is always even parity
#define IRISORCA_LOG_ORCA_INTERVAL_MS 5000 // log ORCA message at this interval in milliseconds
#define IRISORCA_SEND_ACTUATOR_CMD_INTERVAL_MS 100 // actuator commands sent at 10hz if connected to actuator
#define IRISORCA_REPLY_TIMEOUT_MS 25 // stop waiting for replies after 25ms
#define IRISORCA_ERROR_REPORT_INTERVAL_MAX_MS 10000 // errors reported to user at no less than once every 10 seconds

#define HIGHWORD(x) ((uint16_t)((x) >> 16))
#define LOWWORD(x) ((uint16_t)(x))

namespace orca {

bool parse_write_register(uint8_t *rcvd_buff, uint8_t buff_len,
ActuatorState &state) {
bool parse_write_register(uint8_t *rcvd_buff, uint8_t buff_len) {
if (buff_len < WRITE_REG_MSG_RSP_LEN) {
return false;
}
Expand All @@ -49,8 +46,6 @@ bool parse_write_register(uint8_t *rcvd_buff, uint8_t buff_len,
rcvd_buff[WriteRegRsp::Idx::REG_ADDR_LO]) {
case static_cast<uint16_t>(Register::CTRL_REG_3):
// Mode of operation was set
state.mode =
static_cast<OperatingMode>(rcvd_buff[WriteRegRsp::Idx::DATA_LO]);
break;
default:
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
Expand Down Expand Up @@ -301,93 +296,120 @@ void AP_IrisOrca::thread_main()
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Initialized");

_control_state = orca::MotorControlState::CONFIGURING;
bool waiting_for_auto_zero = false;
bool auto_zero_commanded = false;
bool auto_zero_in_progress = false;

while (true)
{
// send a single command depending on the control state
// or send a sleep command if there is an active error
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _last_send_actuator_ms > IRISORCA_SEND_ACTUATOR_POSITION_INTERVAL_MS)
if (now_ms - _last_send_actuator_ms > IRISORCA_SEND_ACTUATOR_CMD_INTERVAL_MS)
{
if (_actuator_state.errors != 0) {
// send sleep command if in error state to attempt to clear the error
// Note: Errors are initialized to 2048 (comm error) so that sleep should be the
// first command sent on boot
send_actuator_sleep_cmd();
if (safe_to_send()) {
send_actuator_sleep_cmd();
}
if (now_ms - _last_error_report_ms > IRISORCA_ERROR_REPORT_INTERVAL_MAX_MS) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "IrisOrca: Error %i", _actuator_state.errors);
_last_error_report_ms = now_ms;
}
continue;
}

// no errors - execute per control state
switch (_control_state)
else
{
case orca::MotorControlState::CONFIGURING:
// Send a write multiple registers command to set the position controller params
// and the auto-zero params
// Exit this mode to auto-zero mode if both are set
if (!_actuator_state.pc_params_set) {
if (safe_to_send()) {
send_position_controller_params();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuring position controller params");
switch (_control_state)
{
case orca::MotorControlState::CONFIGURING:
// Send a write multiple registers command to set the position controller params
// and the auto-zero params
// Exit this mode to auto-zero mode if both are set
if (!_actuator_state.pc_params_set) {
if (safe_to_send()) {
send_position_controller_params();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuring position controller params");
}
}
}
else if (!_actuator_state.auto_zero_params_set) {
if (safe_to_send()) {
send_auto_zero_params();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuring zero params");
else if (!_actuator_state.auto_zero_params_set) {
if (safe_to_send()) {
send_auto_zero_params();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuring zero params");
}
}
}
else {
// both sets of params have been set
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuration complete");
_control_state = orca::MotorControlState::AUTO_ZERO;
}
break;

case orca::MotorControlState::AUTO_ZERO:
// Auto-zero mode is initiated by sending a write register command to the actuator with the
// mode set to AUTO_ZERO. The actuator will then transition to AUTO_ZERO mode and will exit this mode
// to another mode (either SLEEP or POSITION) when the zero position is found.
if (!waiting_for_auto_zero) {
if (_actuator_state.mode == orca::OperatingMode::AUTO_ZERO) {
// Capture the entry into auto-zero mode
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero started");
waiting_for_auto_zero = true;
else {
// both sets of params have been set
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Configuration complete");
_control_state = orca::MotorControlState::AUTO_ZERO;
if (safe_to_send()) {
// might as well send a status request during the state transition
send_actuator_status_request();
}
}
else if (safe_to_send()) {
break;

case orca::MotorControlState::AUTO_ZERO:
// Auto-zero mode is initiated by sending a write register command to the actuator with the
// mode set to AUTO_ZERO. The actuator will then transition to AUTO_ZERO op mode and will exit this mode
// to another op mode (we set it to enter POSITION) when the zero position is found.
if (!auto_zero_commanded) {
// Initiate auto-zero mode
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero commanded");
send_auto_zero_mode_cmd();
if (safe_to_send()) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero commanded");
send_auto_zero_mode_cmd();
auto_zero_commanded = true;
auto_zero_in_progress = false;
break;
}
}
else if (auto_zero_commanded && !auto_zero_in_progress){
// check if the actuator reports that it is in auto-zero mode
if (_actuator_state.mode == orca::OperatingMode::AUTO_ZERO) {
auto_zero_in_progress = true;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Starting auto-zero");
}
else {
// try to set the mode again on next loop (this always happens once)
auto_zero_commanded = false;
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero command failed");
}
}
}
else {
// waiting for auto-zero to complete
if (_actuator_state.mode != orca::OperatingMode::AUTO_ZERO) {
// was auto-zeroing and has exited to another mode, therefore auto-zeroing is complete
waiting_for_auto_zero = false;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero complete");
_control_state = orca::MotorControlState::POSITION_CONTROL;
else if (auto_zero_in_progress)
{
if (_actuator_state.mode == orca::OperatingMode::POSITION) {
// auto-zero complete (Orca exits to position mode), exit to position control mode
auto_zero_commanded = false;
auto_zero_in_progress = false;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero complete");
_control_state = orca::MotorControlState::POSITION_CONTROL;
}
else if (_actuator_state.mode == orca::OperatingMode::SLEEP) {
// there was an error during the auto-zero and we put the actuator into sleep mode
// try to set the mode again on next loop
auto_zero_commanded = false;
auto_zero_in_progress = false;
}
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IrisOrca: Auto-zero in progress...");
}
else if (safe_to_send()) {
// read the mode of operation to check if auto-zero is complete
// read the mode of operation
if (safe_to_send()) {
send_actuator_status_request();
}
}
break;

case orca::MotorControlState::POSITION_CONTROL:
// Send a position control command
if (safe_to_send()) {
send_actuator_position_cmd();
}
break;

default:
break;
break;
case orca::MotorControlState::POSITION_CONTROL:
// Send a position control command
if (safe_to_send()) {
send_actuator_position_cmd();
}
break;

default:
break;
}
}
}

Expand Down Expand Up @@ -748,7 +770,7 @@ void AP_IrisOrca::send_auto_zero_params()
data[2] = HIGHBYTE(_auto_zero_f_max);
data[3] = LOWBYTE(_auto_zero_f_max);
data[4] = 0x00;
data[5] = 0x01; // Exit to sleep mode after auto-zero
data[5] = 0x03; // Exit to position mode after auto-zero

// send message
if (write_multiple_registers((uint16_t)orca::Register::ZERO_MODE, 3, (uint8_t *)data)){
Expand Down Expand Up @@ -801,13 +823,13 @@ bool AP_IrisOrca::parse_message()
switch (static_cast<orca::FunctionCode>(_received_buff[1]))
{
case orca::FunctionCode::WRITE_REGISTER:
return parse_write_register(_received_buff, _reply_msg_len, _actuator_state);
return orca::parse_write_register(_received_buff, _reply_msg_len);
case orca::FunctionCode::WRITE_MULTIPLE_REGISTERS:
return parse_multiple_write_registers(_received_buff, _reply_msg_len, _actuator_state);
return orca::parse_multiple_write_registers(_received_buff, _reply_msg_len, _actuator_state);
case orca::FunctionCode::MOTOR_COMMAND_STREAM:
return parse_motor_command_stream(_received_buff, _reply_msg_len, _actuator_state);
return orca::parse_motor_command_stream(_received_buff, _reply_msg_len, _actuator_state);
case orca::FunctionCode::MOTOR_READ_STREAM:
return parse_motor_read_stream(_received_buff, _reply_msg_len, _actuator_state);
return orca::parse_motor_read_stream(_received_buff, _reply_msg_len, _actuator_state);
default:
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "IrisOrca: Unexpected message");
return false;
Expand Down
4 changes: 1 addition & 3 deletions libraries/AP_IrisOrca/AP_IrisOrca.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -282,12 +282,10 @@ namespace orca {
*
* @param[in] rcvd_buff The buffer containing received response data
* @param[in] buff_len The length of the received buffer
* @param[out] state (output parameter) State data of the actuator to populate with response.
* Currently only updates the mode.
* @return true response successfully parsed
* @return false response parsing failed
*/
bool parse_write_register(uint8_t *rcvd_buff, uint8_t buff_len, ActuatorState &state);
bool parse_write_register(uint8_t *rcvd_buff, uint8_t buff_len);

/**
* @brief Parse the response to a 0x10 Multiple Write Registers message.
Expand Down
Loading