diff --git a/firmware/lib/tvc_actuators/TVC_Actuators.cpp b/firmware/lib/tvc_actuators/TVC_Actuators.cpp index 48f0c3c..b70ba8e 100644 --- a/firmware/lib/tvc_actuators/TVC_Actuators.cpp +++ b/firmware/lib/tvc_actuators/TVC_Actuators.cpp @@ -1,17 +1,94 @@ #include "TVC_Actuators.h" #include "GimbalKinematics.h" +#include "UltramotionActuator.hpp" +#include "ec_pins.h" +#include "fdcan_toad.h" +#include "toad_can_bus.h" +#include namespace TVC_Actuators { +uint32_t ASSUMED_ACTUATOR_TELEMETRY_INTERVAL_MS = 1000; +uint32_t ACTUATOR_HANDSHAKE_TIMEOUT_MS = 2 * ASSUMED_ACTUATOR_TELEMETRY_INTERVAL_MS + 500; +bool tvc_debug_mode = false; + +// TODO - PLACEHOLDER scaling. Assumes a straight linear map from physical +// actuator length (mm) to the actuator's raw target_pos range (0-65535). +// Needs the actuator's real stroke length and pMin/pMax from its +// datasheet/CONFIG.TXT before this is trustworthy. +uint16_t length_to_target_pos(float length_mm) { + constexpr float MIN_LENGTH_MM = 0.0f; // TODO + constexpr float MAX_LENGTH_MM = 100.0f; // TODO + constexpr uint16_t POS_MAX = 65535; + + float clamped = constrain(length_mm, MIN_LENGTH_MM, MAX_LENGTH_MM); + return (uint16_t)((clamped - MIN_LENGTH_MM) / (MAX_LENGTH_MM - MIN_LENGTH_MM) * POS_MAX); +} + bool begin() { - return true; + bool heard_pitch = false; + bool heard_yaw = false; + + uint32_t start_time = millis(); + while (millis() - start_time < ACTUATOR_HANDSHAKE_TIMEOUT_MS) { + while (can_tvc.available() > 0) { + arduino::CanMsg msg = can_tvc.read(); + uint32_t id = msg.isStandardId() ? msg.getStandardId() : msg.getExtendedId(); + + if (id == CAN_ID_TVC_PITCH) { + heard_pitch = true; + } else if (id == CAN_ID_TVC_YAW) { + heard_yaw = true; + } + } + + if (heard_pitch && heard_yaw) { + return true; + } + } + + if (!heard_pitch) { + CommsSerial.println("No telemetry heard from PITCH actuator"); + } + if (!heard_yaw) { + CommsSerial.println("No telemetry heard from YAW actuator"); + } + + return false; } -// TODO - this function void set_angles_pitch_yaw(float pitch, float yaw) { - float pitch_len; - float yaw_len; - calc_actuator_lengths(pitch, yaw, &pitch_len, &yaw_len); + float pitch_len, yaw_len; + calc_actuator_lengths(pitch, yaw, &pitch_len, &yaw_len); // already implemented + + send_target_pos(can_tvc, CAN_ID_TVC_PITCH, length_to_target_pos(pitch_len)); + send_target_pos(can_tvc, CAN_ID_TVC_YAW, length_to_target_pos(yaw_len)); +} + +// Call every flight_loop() iteration - nothing currently does. Drains +// whatever arrived in the RX FIFO since the last call and decodes anything +// addressed to the TVC actuators. +void poll() { + while (can_tvc.available() > 0) { + arduino::CanMsg msg = can_tvc.read(); + uint32_t id = msg.isStandardId() ? msg.getStandardId() : msg.getExtendedId(); + + if (id == CAN_ID_TVC_PITCH || id == CAN_ID_TVC_YAW) { + // Fixed-format decode for exactly what flight code acts on - no + // letter-code dispatch, no unused fields. + tvc_actuator_telemetry_t telem = parse_tvc_telemetry(msg.data); + + if (tvc_debug_mode) { + // Full generic decode/print, bench debugging only - never feeds a + // flight decision. Matches "KLMGHEFY", the factory-default txData. + char decode_str[] = {'K', 'L', 'M', 'G', 'H', 'E', 'F', 'Y'}; + struct telem full_frame; + parse_CAN_frame(msg.data, msg.data_length, decode_str, sizeof(decode_str), &full_frame); + } + } + // CAN_ID_STEPPER_OX / CAN_ID_STEPPER_FU frames also arrive on this same + // bus - dispatch to ThrottleValves here too once it reads feedback this way. + } } } // namespace TVC_Actuators \ No newline at end of file diff --git a/firmware/lib/tvc_actuators/TVC_Actuators.h b/firmware/lib/tvc_actuators/TVC_Actuators.h index 658515c..0506d1b 100644 --- a/firmware/lib/tvc_actuators/TVC_Actuators.h +++ b/firmware/lib/tvc_actuators/TVC_Actuators.h @@ -3,7 +3,11 @@ namespace TVC_Actuators { bool begin(); - void set_angles_pitch_yaw(float pitch, float yaw); -} \ No newline at end of file +// Drains and processes any actuator feedback waiting on the Actuators CAN +// bus. Must be called from flight_loop() (or loop()) for feedback to ever be +// seen - nothing does this automatically. +void poll(); + +} // namespace TVC_Actuators \ No newline at end of file diff --git a/firmware/lib/tvc_actuators/UltramotionActuator.cpp b/firmware/lib/tvc_actuators/UltramotionActuator.cpp new file mode 100644 index 0000000..a9285fc --- /dev/null +++ b/firmware/lib/tvc_actuators/UltramotionActuator.cpp @@ -0,0 +1,338 @@ +#include "UltramotionActuator.hpp" +#include "CommsSerial.h" +#include "fdcan_toad.h" + +// Changes from the previous version: sends now go through the real fdcan_toad +// build an arduino::CanMsg and call bus.write() directly. Everything else +// (status_codes[], print_new_status_codes(), parse_CAN_byte(), +// print_can_data()) is unchanged. + +using namespace arduino; + +const char *status_codes[STATUS_CODE_COUNT] = { + "Position at or beyond retracted physical stop rPos", // 0 - 8 + "Position at or beyond extended physical stop ePos", + "Position beyond retracted software limit spMin", + "Position beyond extended software limit spMax", + "Supply voltage low, motor in COAST (<6.75 VDC, 1 V hysteresis)", + "Supply voltage high, motor in dynamic brake (>44.0 VDC, 2 V hysteresis)", + "Torque output greater than ovTorq limit", + "Torque command at maxTorq limit", + "Speed below \xe2\x80\x9cstop\xe2\x80\x9d threshold", // 9 - 16 + "Direction is extend", + "Position at target (position near target within posWin for posTime)", + "Following error (position error larger than fErrWin for time period fErrTime)", + "Command RX error (message not received in [rxTO * 800 \xc2\xb5s])", + "Telemetry TX error (message not sent over full telemetry interval)", + "CAN position command input capped at low limit pMin", + "CAN position command input capped at upper limit pMax", + "Trajectory move active ", // 17 - 24 + "Heating active", + "Temperature at PCB greater than ovTemp value", + "Temperature at PCB less than unTemp value", + "Relative humidity at PCB greater than ovHumi value", + "Fatal error in CONFIG.TXT or HARDWARE.TXT", + "Fault output bit of DRV8323RS (Bridge Driver)", + "Erroneous warm reset of the CPU has occurred", + "opMode (CLI = 0, CAN = 1)", // 25 - 32 + "Interpolation enabled ", + "Heating enabled ", + "CAN bus module in passive mode ", + "USB connected", + "Opto input 1", + "Opto input 2", + "Opto input 3"}; + +uint32_t current_status = 0; // limitation - don't mix latched status with normal status for the same variable +uint32_t current_status_latched_high = 0; +uint32_t current_status_latched_low = 0; + +void reset_status_state() { + current_status = 0; + current_status_latched_high = 0; + current_status_latched_low = 0; +} + +void print_new_status_codes(uint32_t new_status, uint32_t old_status) { + uint32_t status_dif = new_status ^ old_status; + + CommsSerial.println(" New Status Messages: "); + uint32_t current_status_shift = new_status; + uint32_t status_dif_shift = status_dif; + for (int i = 0; i < STATUS_CODE_COUNT; i++) { + if (status_dif_shift & 0x1 && current_status_shift & 0x1) { // select bits, check if message is NEW and ACTIVE + CommsSerial.print(" "); + CommsSerial.println(status_codes[i]); + }; + status_dif_shift = status_dif_shift >> 1; + current_status_shift = current_status_shift >> 1; + } + CommsSerial.println(" END"); + + CommsSerial.println("\n Cleared Status: "); + current_status_shift = new_status; + status_dif_shift = status_dif; + for (int i = 0; i < STATUS_CODE_COUNT; i++) { + if (status_dif_shift & 0x1 && !(current_status_shift & 0x1)) { // select bits, check if message is NEW and INACTIVE + CommsSerial.print(" "); + CommsSerial.println(status_codes[i]); + }; + status_dif_shift = status_dif_shift >> 1; + current_status_shift = current_status_shift >> 1; + } + CommsSerial.println(" END"); +} + +void parse_CAN_byte(uint8_t can_msg, char decode_str, telem *current_telem_frame) { + uint16_t can_msg_16bit = can_msg; + uint32_t can_msg_32bit = can_msg; + + if (decode_str == 'A') { + current_telem_frame->status_word += can_msg_32bit; + } else if (decode_str == 'B') { + current_telem_frame->status_word += can_msg_32bit << 8; + } else if (decode_str == 'C') { + current_telem_frame->status_word += can_msg_32bit << 16; + } else if (decode_str == 'D') { + current_telem_frame->status_word += can_msg_32bit << 24; + } else if (decode_str == 'E') { + current_telem_frame->avg_motor_current += can_msg_16bit; + } else if (decode_str == 'F') { + current_telem_frame->avg_motor_current += can_msg_16bit << 8; + } else if (decode_str == 'G') { + current_telem_frame->abs_servo_cylinder_pos += can_msg_16bit; + } else if (decode_str == 'H') { + current_telem_frame->abs_servo_cylinder_pos += can_msg_16bit << 8; + } else if (decode_str == 'I') { + current_telem_frame->rel_servo_cylinder_pos += can_msg_16bit; + } else if (decode_str == 'J') { + current_telem_frame->rel_servo_cylinder_pos += can_msg_16bit << 8; + } else if (decode_str == 'K') { + current_telem_frame->latch_high_status_word += can_msg_32bit; + } else if (decode_str == 'L') { + current_telem_frame->latch_high_status_word += can_msg_32bit << 8; + } else if (decode_str == 'M') { + current_telem_frame->latch_high_status_word += can_msg_32bit << 16; + } else if (decode_str == 'N') { + current_telem_frame->latch_high_status_word += can_msg_32bit << 24; + } else if (decode_str == 'O') { + current_telem_frame->latch_low_status_word += can_msg_32bit; + } else if (decode_str == 'P') { + current_telem_frame->latch_low_status_word += can_msg_32bit << 8; + } else if (decode_str == 'Q') { + current_telem_frame->latch_low_status_word += can_msg_32bit << 16; + } else if (decode_str == 'R') { + current_telem_frame->latch_low_status_word += can_msg_32bit << 24; + } else if (decode_str == 'S') { + current_telem_frame->phys_stop_pos = can_msg; + } else if (decode_str == 'T') { + current_telem_frame->motor_current_avg_16 = can_msg; + } else if (decode_str == 'U') { + current_telem_frame->bus_voltage = can_msg; + } else if (decode_str == 'V') { + current_telem_frame->motor_current_avg = can_msg; + } else if (decode_str == 'W') { + current_telem_frame->max_motor_current_8_bit = can_msg; + } else if (decode_str == 'X') { + current_telem_frame->signed_PCB_temp_sensor = can_msg; + } else if (decode_str == 'Y') { + current_telem_frame->unsigned_PCB_temp_sensor = can_msg; + } else if (decode_str == 'Z') { + current_telem_frame->PCB_relative_humidity = can_msg; + } else if (decode_str == 'm') { + current_telem_frame->max_motor_current_16_bit += can_msg_16bit; + } else if (decode_str == 'c') { + current_telem_frame->max_motor_current_16_bit += can_msg_16bit << 8; + } else if (decode_str == 'p') { + current_telem_frame->unitID += can_msg_32bit; + } else if (decode_str == 'q') { + current_telem_frame->unitID += can_msg_32bit << 8; + } else if (decode_str == 'r') { + current_telem_frame->unitID += can_msg_32bit << 16; + } else if (decode_str == 's') { + current_telem_frame->unitID += can_msg_32bit << 24; + } else if (decode_str == 't') { + current_telem_frame->target_pos += can_msg_16bit; + } else if (decode_str == 'u') { + current_telem_frame->target_pos += can_msg_16bit << 8; + } else { + CommsSerial.print("FATAL ERROR - CAN frame contained non-decodable char: "); + CommsSerial.println(decode_str); + } +} + +void print_can_data(char decode_str, telem *current_telem_frame) { + switch (decode_str) { + case 'A': + case 'B': + case 'C': + case 'D': + if (!current_telem_frame->has_printed_status) { + CommsSerial.println("Status: "); + print_new_status_codes(current_telem_frame->status_word, current_status); + current_status = current_telem_frame->status_word; + } + current_telem_frame->has_printed_status = true; + break; + case 'E': // prevent dup + case 'G': + case 'I': + case 'm': + case 't': + break; + case 'F': // and E + CommsSerial.print("Average motor current over telemetry interval: "); + CommsSerial.println(current_telem_frame->avg_motor_current); + break; + case 'H': // and G + CommsSerial.print("Servo Cylinder position, absolute encoder value: "); + CommsSerial.println(current_telem_frame->abs_servo_cylinder_pos); + break; + case 'J': // and I + CommsSerial.print("Position converted to input range (pMin to pMax): "); + CommsSerial.println(current_telem_frame->rel_servo_cylinder_pos); + break; + case 'K': + case 'L': + case 'M': + case 'N': + if (!current_telem_frame->has_printed_high_status) { + CommsSerial.println("Status (Latched High): "); + print_new_status_codes(current_telem_frame->latch_high_status_word, current_status_latched_high); + current_status_latched_high = current_telem_frame->latch_high_status_word; + } + current_telem_frame->has_printed_high_status = true; + break; + case 'O': + case 'P': + case 'Q': + case 'R': + if (!current_telem_frame->has_printed_low_status) { + CommsSerial.println("Status (Latched Low): "); + print_new_status_codes(current_telem_frame->latch_low_status_word, current_status_latched_low); + current_status_latched_low = current_telem_frame->latch_low_status_word; + } + current_telem_frame->has_printed_low_status = true; + break; + case 'S': + CommsSerial.print("8-bit position between physical stops (rPos to ePos): "); + CommsSerial.println(current_telem_frame->phys_stop_pos); + break; + case 'T': + CommsSerial.print("8-bit motor current 16-sample average of last 16 ms: "); + CommsSerial.println(current_telem_frame->motor_current_avg_16); + break; + case 'U': { + CommsSerial.print("8-bit bus voltage 0 VDC to +50 VDC: "); + float voltage = current_telem_frame->bus_voltage; + CommsSerial.print(voltage * 50 / 255); + CommsSerial.println(" VDC"); + break; + } + case 'V': + CommsSerial.print("8-bit average motor current over telemetry interval: "); + CommsSerial.println(current_telem_frame->motor_current_avg); + break; + case 'W': + CommsSerial.print("8-bit max motor current over telemetry interval: "); + CommsSerial.println(current_telem_frame->max_motor_current_8_bit); + break; + case 'X': + CommsSerial.print("8-bit signed integer PCB temp sensor C (-50 to +127): "); + CommsSerial.println(current_telem_frame->signed_PCB_temp_sensor); + break; + case 'Y': { + CommsSerial.print("8-bit unsigned PCB temp sensor: "); + int pcb_temp = current_telem_frame->unsigned_PCB_temp_sensor; + CommsSerial.print(pcb_temp - 50); + CommsSerial.println(" deg C"); + break; + } + case 'Z': + CommsSerial.print("8-bit PCB relative humidity: "); + CommsSerial.print(current_telem_frame->PCB_relative_humidity); + CommsSerial.println("%"); + break; + case 'c': // and m + CommsSerial.print("Max motor current over telemetry interval: "); + CommsSerial.println(current_telem_frame->max_motor_current_16_bit); + break; + case 'p': + case 'q': + case 'r': + case 's': + if (!current_telem_frame->has_printed_ID) { + CommsSerial.print("ID: 0x"); + CommsSerial.println(current_telem_frame->unitID, HEX); + } + current_telem_frame->has_printed_ID = true; + break; + case 'u': // and t + CommsSerial.print("Target position, absolute encoder value: "); + CommsSerial.println(current_telem_frame->target_pos); + break; + default: + CommsSerial.print("FATAL ERROR - CAN frame contained non-decodable char: "); + CommsSerial.println(decode_str); + break; + } +} + +void parse_CAN_frame(const uint8_t can_msg[], uint8_t msg_length, char decode_str[], uint8_t decode_length, + telem *out) { + if (msg_length != decode_length) { + CommsSerial.println("FATAL ERROR - CAN frame and decode code lengths do not match!"); + } + + if (msg_length > 8) { + CommsSerial.println("FATAL ERROR - CAN frame longer than max!"); + } + + telem current_telem_frame; // values are all set to 0 + + for (uint8_t i = 0; i < msg_length; i++) { + parse_CAN_byte(can_msg[i], decode_str[i], ¤t_telem_frame); + } + + CommsSerial.println("CAN FRAME START ++++++++++++++++++++++++++"); + for (uint8_t i = 0; i < msg_length; i++) { + print_can_data(decode_str[i], ¤t_telem_frame); + } + CommsSerial.println("CAN FRAME END ---------------------------\n"); + + if (out != nullptr) { + *out = current_telem_frame; + } +} + +tvc_actuator_telemetry_t parse_tvc_telemetry(const uint8_t data[8]) { + tvc_actuator_telemetry_t out; + out.status_word = (uint32_t)data[0] | ((uint32_t)data[1] << 8) | ((uint32_t)data[2] << 16); // K,L,M + out.position = (uint16_t)data[3] | ((uint16_t)data[4] << 8); // G,H + out.avg_motor_current = (uint16_t)data[5] | ((uint16_t)data[6] << 8); // E,F + out.pcb_temp_c = (int16_t)data[7] - 50; // Y + return out; +} + +void send_target_pos(CAN &bus, uint16_t id, uint16_t target_pos) { + // Same byte layout as the original prep_CAN_msg(id, target_pos): 2-byte + // frame, little-endian target position. toad_can_bus.h documents TOAD's + // CAN IDs as 11-bit standard, so CanStandardId (not CanExtendedId) is used. + uint8_t data[2] = { + static_cast(target_pos & 0xFF), + static_cast(target_pos >> 8), + }; + CanMsg msg(CanStandardId(id), sizeof(data), data); + bus.write(msg); +} + +void send_target_pos(CAN &bus, uint16_t id, uint16_t target_pos, uint16_t max_torque) { + uint8_t data[4] = { + static_cast(target_pos & 0xFF), + static_cast(target_pos >> 8), + static_cast(max_torque & 0xFF), + static_cast(max_torque >> 8), + }; + CanMsg msg(CanStandardId(id), sizeof(data), data); + bus.write(msg); +} \ No newline at end of file diff --git a/firmware/lib/tvc_actuators/UltramotionActuator.hpp b/firmware/lib/tvc_actuators/UltramotionActuator.hpp new file mode 100644 index 0000000..79df06d --- /dev/null +++ b/firmware/lib/tvc_actuators/UltramotionActuator.hpp @@ -0,0 +1,67 @@ +#include + +#ifndef UltramotionActuator_H +#define UltramotionActuator_H + +// +// Forward declare rather than #include "fdcan_toad.h" here - a reference is +// all this header needs, and fdcan_toad.h drags in the STM32 HAL headers, +// which callers of this file shouldn't have to pull in just to see these +// prototypes. +class CAN; + +struct telem { + uint32_t status_word = 0; // A-D: Status word byte + uint32_t latch_high_status_word = 0; // K-N: Latched high copy of status word byte + uint32_t latch_low_status_word = 0; // O-R: Latched low copy of status word byte + uint32_t unitID = 0; // p-s: unitID (CAN ID) + + uint16_t avg_motor_current = 0; // E-F: Average motor current over telemetry interval (0 to 32767) + uint16_t abs_servo_cylinder_pos = 0; // G-H: Servo Cylinder position, absolute encoder value (0 to 65535) + uint16_t rel_servo_cylinder_pos = 0; // I-J: Position converted to input range (pMin to pMax) + uint8_t phys_stop_pos = 0; // S: 8-bit position between physical stops (rPos to ePos) + uint8_t motor_current_avg_16 = 0; // T: 8-bit motor current 16-sample average of last 16 ms (0 to 255) + uint8_t bus_voltage = 0; // U: 8-bit bus voltage 0 VDC to +50 VDC (0 to 255) + uint8_t motor_current_avg = 0; // V: 8-bit average motor current over telemetry interval (0 to 255) + uint8_t max_motor_current_8_bit = 0; // W: 8-bit max motor current over telemetry interval (0 to 255) + int8_t signed_PCB_temp_sensor = 0; // X: 8-bit signed integer PCB temp sensor C (-50 to +127) + uint8_t unsigned_PCB_temp_sensor = + 0; // Y: 8-bit unsigned PCB temp sensor in deg_C where 0 = -50deg_C and 200 = +150deg_C (0 to 200) + uint8_t PCB_relative_humidity = 0; // Z: 8-bit PCB relative humidity % (0 to 100) + uint16_t max_motor_current_16_bit = 0; // mc: Max motor current over telemetry interval (0 to 32767) + uint16_t target_pos = 0; // tu: Target position, absolute encoder value + + bool has_printed_status = false; + bool has_printed_high_status = false; + bool has_printed_low_status = false; + bool has_printed_ID = false; +}; + +#define STATUS_CODE_COUNT 32 + +// treat all active messages as new +void reset_status_state(); + +// Decodes a raw CAN frame and prints its contents. +// If `out` is non-null, also copies the decoded telem struct into it so the +// caller (e.g. TVC_Actuators) can act on status/position/fault data instead +// of only logging it. +void parse_CAN_frame(const uint8_t can_msg[], uint8_t msg_length, char decode_str[], uint8_t decode_length, + telem *out = nullptr); + +// Sends a CAN frame commanding the actuator at `id` to target_pos, over `bus` - data fmt '<>' +void send_target_pos(CAN &bus, uint16_t id, uint16_t target_pos); + +// Sends a CAN frame commanding the actuator at `id` to target_pos with a max_torque limit - data fmt '<>()' +void send_target_pos(CAN &bus, uint16_t id, uint16_t target_pos, uint16_t max_torque); + +struct tvc_actuator_telemetry_t { + uint32_t status_word; // latched-high status word, bits 0-23 only (byte 3/N not included by default) + uint16_t position; // absolute servo cylinder position + uint16_t avg_motor_current; // raw units, 0-32767 - see UM711293 (hardware manual) for conversion to amps + int16_t pcb_temp_c; // decoded PCB temperature in degrees C (raw byte "Y" is 0-200; 0 = -50C, 200 = +150C) +}; + +tvc_actuator_telemetry_t parse_tvc_telemetry(const uint8_t data[8]); + +#endif \ No newline at end of file