diff --git a/Tools/scripts/flash_via_remote.sh b/Tools/scripts/flash_via_remote.sh index f1f9faf6905f0..97bea1978a948 100755 --- a/Tools/scripts/flash_via_remote.sh +++ b/Tools/scripts/flash_via_remote.sh @@ -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 @@ -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 @@ -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 diff --git a/libraries/AP_IrisOrca/AP_IrisOrca.cpp b/libraries/AP_IrisOrca/AP_IrisOrca.cpp index c85dc00010433..f2d9f8db154b8 100644 --- a/libraries/AP_IrisOrca/AP_IrisOrca.cpp +++ b/libraries/AP_IrisOrca/AP_IrisOrca.cpp @@ -24,22 +24,19 @@ #include #include -#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; } @@ -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(Register::CTRL_REG_3): // Mode of operation was set - state.mode = - static_cast(rcvd_buff[WriteRegRsp::Idx::DATA_LO]); break; default: GCS_SEND_TEXT(MAV_SEVERITY_WARNING, @@ -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; + } } } @@ -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)){ @@ -801,13 +823,13 @@ bool AP_IrisOrca::parse_message() switch (static_cast(_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; diff --git a/libraries/AP_IrisOrca/AP_IrisOrca.h b/libraries/AP_IrisOrca/AP_IrisOrca.h index 456727e4c837c..adc026274ed92 100644 --- a/libraries/AP_IrisOrca/AP_IrisOrca.h +++ b/libraries/AP_IrisOrca/AP_IrisOrca.h @@ -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.