Skip to content

Commit

Permalink
Merge pull request #4 from HavocAI/orca_sm_fixes
Browse files Browse the repository at this point in the history 
Orca state machine bug fixes
  • Loading branch information
@agregghai
agregghai authored Jan 16, 2025
2 parents a4c00ef + ec6b727 commit ea5ae78
Show file tree
Hide file tree
Showing 3 changed files with 104 additions and 84 deletions.
8 changes: 4 additions & 4 deletions Tools/scripts/flash_via_remote.sh
View file
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
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
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

0 comments on commit ea5ae78

Please sign in to comment.