From adfc24997127dcd97d0999bd6e6fbc584b5e4066 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Sun, 29 Sep 2024 14:05:37 -0600 Subject: [PATCH] run make format --- inc/adc_interface_client.hpp | 48 +- inc/anticogging_client.hpp | 77 +- inc/anticogging_pro_client.hpp | 123 ++-- inc/arming_handler_client.hpp | 126 ++-- inc/bipbuffer.h | 552 +++++++-------- inc/brushless_drive_client.hpp | 499 ++++++------- inc/buzzer_control_client.hpp | 93 +-- inc/byte_queue.h | 14 +- inc/client_communication.cpp | 88 ++- inc/client_communication.hpp | 325 +++++---- inc/coil_temperature_estimator_client.hpp | 186 ++--- inc/communication_interface.h | 121 ++-- inc/esc_propeller_input_parser_client.hpp | 110 +-- inc/generic_interface.hpp | 149 ++-- inc/gpio_controller_client.hpp | 114 +-- inc/hobby_input_client.hpp | 79 ++- ...art_flight_controller_interface_client.hpp | 144 ++-- inc/mag_alpha_client.hpp | 99 +-- inc/multi_turn_angle_control_client.hpp | 294 ++++---- inc/packet_finder.h | 94 +-- inc/persistent_memory_client.hpp | 75 +- inc/power_monitor_client.hpp | 127 ++-- inc/power_safety_client.hpp | 146 ++-- inc/propeller_motor_control_client.hpp | 196 +++--- ...ulsing_rectangular_input_parser_client.hpp | 48 +- inc/pwm_interface_client.hpp | 56 +- inc/serial_interface_client.hpp | 45 +- inc/servo_input_parser_client.hpp | 63 +- inc/step_direction_input_client.hpp | 53 +- inc/stopping_handler_client.hpp | 48 +- inc/stow_user_interface_client.hpp | 114 +-- inc/system_control_client.hpp | 234 +++--- inc/temperature_estimator_client.hpp | 79 ++- inc/temperature_monitor_uc_client.hpp | 79 ++- inc/uavcan_node_client.hpp | 122 ++-- inc/voltage_superposition_client.hpp | 164 ++--- src/byte_queue.c | 192 ++--- src/crc_helper.c | 43 +- src/generic_interface.cpp | 168 ++--- src/packet_finder.c | 664 ++++++++++-------- tests/2306_pulsing_test.cpp | 362 +++++----- tests/8108_speed_v0_0_7_test.cpp | 282 ++++---- tests/fortiq_test.cpp | 182 ++--- 43 files changed, 3536 insertions(+), 3341 deletions(-) diff --git a/inc/adc_interface_client.hpp b/inc/adc_interface_client.hpp index ddf0c65..aa8f8df 100644 --- a/inc/adc_interface_client.hpp +++ b/inc/adc_interface_client.hpp @@ -20,29 +20,31 @@ const uint8_t kTypeAdcInterface = 91; -class AdcInterfaceClient : public ClientAbstract { - public: - AdcInterfaceClient(uint8_t obj_idn) - : ClientAbstract(kTypeAdcInterface, obj_idn), - adc_voltage_(kTypeAdcInterface, obj_idn, kSubAdcVoltage), - raw_value_(kTypeAdcInterface, obj_idn, kSubRawValue){}; - - // Client Entries - ClientEntry adc_voltage_; - ClientEntry raw_value_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubRawValue + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &adc_voltage_, // 0 - &raw_value_ // 1 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubAdcVoltage = 0; - static const uint8_t kSubRawValue = 1; +class AdcInterfaceClient : public ClientAbstract +{ +public: + AdcInterfaceClient(uint8_t obj_idn) + : ClientAbstract(kTypeAdcInterface, obj_idn), + adc_voltage_(kTypeAdcInterface, obj_idn, kSubAdcVoltage), + raw_value_(kTypeAdcInterface, obj_idn, kSubRawValue) {}; + + // Client Entries + ClientEntry adc_voltage_; + ClientEntry raw_value_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubRawValue + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &adc_voltage_, // 0 + &raw_value_ // 1 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubAdcVoltage = 0; + static const uint8_t kSubRawValue = 1; }; #endif /* ADC_INTERFACE_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/anticogging_client.hpp b/inc/anticogging_client.hpp index a292052..49a65a8 100644 --- a/inc/anticogging_client.hpp +++ b/inc/anticogging_client.hpp @@ -20,44 +20,45 @@ const uint8_t kTypeAnticogging = 71; -class AnticoggingClient: public ClientAbstract{ - public: - AnticoggingClient(uint8_t obj_idn): - ClientAbstract( kTypeAnticogging, obj_idn), - table_size_( kTypeAnticogging, obj_idn, kSubTableSize), - is_data_valid_( kTypeAnticogging, obj_idn, kSubIsDataValid), - is_enabled_( kTypeAnticogging, obj_idn, kSubIsEnabled), - erase_( kTypeAnticogging, obj_idn, kSubErase), - left_shift_( kTypeAnticogging, obj_idn, kSubLeftShift) - {}; - - // Client Entries - ClientEntry table_size_; - ClientEntry is_data_valid_; - ClientEntry is_enabled_; - ClientEntryVoid erase_; - ClientEntry left_shift_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubLeftShift+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &table_size_, // 0 - &is_data_valid_, // 1 - &is_enabled_, // 2 - &erase_, // 3 - &left_shift_ // 4 - }; - - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubTableSize = 0; - static const uint8_t kSubIsDataValid = 1; - static const uint8_t kSubIsEnabled = 2; - static const uint8_t kSubErase = 3; - static const uint8_t kSubLeftShift = 4; +class AnticoggingClient: public ClientAbstract +{ +public: + AnticoggingClient(uint8_t obj_idn): + ClientAbstract(kTypeAnticogging, obj_idn), + table_size_(kTypeAnticogging, obj_idn, kSubTableSize), + is_data_valid_(kTypeAnticogging, obj_idn, kSubIsDataValid), + is_enabled_(kTypeAnticogging, obj_idn, kSubIsEnabled), + erase_(kTypeAnticogging, obj_idn, kSubErase), + left_shift_(kTypeAnticogging, obj_idn, kSubLeftShift) + {}; + + // Client Entries + ClientEntry table_size_; + ClientEntry is_data_valid_; + ClientEntry is_enabled_; + ClientEntryVoid erase_; + ClientEntry left_shift_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubLeftShift + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &table_size_, // 0 + &is_data_valid_, // 1 + &is_enabled_, // 2 + &erase_, // 3 + &left_shift_ // 4 + }; + + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubTableSize = 0; + static const uint8_t kSubIsDataValid = 1; + static const uint8_t kSubIsEnabled = 2; + static const uint8_t kSubErase = 3; + static const uint8_t kSubLeftShift = 4; }; #endif // ANTICOGGING_CLIENT_H diff --git a/inc/anticogging_pro_client.hpp b/inc/anticogging_pro_client.hpp index afbe7ba..c6ea8b6 100644 --- a/inc/anticogging_pro_client.hpp +++ b/inc/anticogging_pro_client.hpp @@ -8,9 +8,9 @@ /* Name: anticogging_pro_client.hpp - Last update: 2023/06/29 by Ben Quan - Author: Ben Quan - Contributors: + Last update: 2023/06/29 by Ben Quan + Author: Ben Quan + Contributors: */ #ifndef ANTICOGGING_PRO_CLIENT_HPP @@ -20,68 +20,69 @@ const uint8_t kTypeAnticoggingPro = 79; -class AnticoggingProClient: public ClientAbstract{ - public: - AnticoggingProClient(uint8_t obj_idn): - ClientAbstract( kTypeAnticoggingPro, obj_idn), - enabled_(kTypeAnticoggingPro, obj_idn, kSubEnabled), - tau_(kTypeAnticoggingPro, obj_idn, kSubTau), - num_harmonics_(kTypeAnticoggingPro, obj_idn, kSubNumHarmonics), - voltage_(kTypeAnticoggingPro, obj_idn, kSubVoltage), - index_(kTypeAnticoggingPro, obj_idn, kSubIndex), - harmonic_(kTypeAnticoggingPro, obj_idn, kSubHarmonic), - a_(kTypeAnticoggingPro, obj_idn, kSubA), - phase_(kTypeAnticoggingPro, obj_idn, kSubPhase), - phase_total_(kTypeAnticoggingPro, obj_idn, kSubPhaseTotal), - amplitude_(kTypeAnticoggingPro, obj_idn, kSubAmplitude), - max_harmonics_(kTypeAnticoggingPro, obj_idn, kSubMaxHarmonics) - {}; +class AnticoggingProClient: public ClientAbstract +{ +public: + AnticoggingProClient(uint8_t obj_idn): + ClientAbstract(kTypeAnticoggingPro, obj_idn), + enabled_(kTypeAnticoggingPro, obj_idn, kSubEnabled), + tau_(kTypeAnticoggingPro, obj_idn, kSubTau), + num_harmonics_(kTypeAnticoggingPro, obj_idn, kSubNumHarmonics), + voltage_(kTypeAnticoggingPro, obj_idn, kSubVoltage), + index_(kTypeAnticoggingPro, obj_idn, kSubIndex), + harmonic_(kTypeAnticoggingPro, obj_idn, kSubHarmonic), + a_(kTypeAnticoggingPro, obj_idn, kSubA), + phase_(kTypeAnticoggingPro, obj_idn, kSubPhase), + phase_total_(kTypeAnticoggingPro, obj_idn, kSubPhaseTotal), + amplitude_(kTypeAnticoggingPro, obj_idn, kSubAmplitude), + max_harmonics_(kTypeAnticoggingPro, obj_idn, kSubMaxHarmonics) + {}; - // Client Entries - ClientEntry enabled_; - ClientEntry tau_; - ClientEntry num_harmonics_; - ClientEntry voltage_; - ClientEntry index_; - ClientEntry harmonic_; - ClientEntry a_; - ClientEntry phase_; - ClientEntry phase_total_; - ClientEntry amplitude_; - ClientEntry max_harmonics_; + // Client Entries + ClientEntry enabled_; + ClientEntry tau_; + ClientEntry num_harmonics_; + ClientEntry voltage_; + ClientEntry index_; + ClientEntry harmonic_; + ClientEntry a_; + ClientEntry phase_; + ClientEntry phase_total_; + ClientEntry amplitude_; + ClientEntry max_harmonics_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubMaxHarmonics+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &enabled_, // 0 - &tau_, // 1 - &num_harmonics_, // 2 - &voltage_, // 3 - &index_, // 4 - &harmonic_, // 5 - &a_, // 6 - &phase_, // 7 - &phase_total_, // 8 - &litude_, // 9 - &max_harmonics_ // 10 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubMaxHarmonics + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &enabled_, // 0 + &tau_, // 1 + &num_harmonics_, // 2 + &voltage_, // 3 + &index_, // 4 + &harmonic_, // 5 + &a_, // 6 + &phase_, // 7 + &phase_total_, // 8 + &litude_, // 9 + &max_harmonics_ // 10 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubEnabled = 0; - static const uint8_t kSubTau = 1; - static const uint8_t kSubNumHarmonics = 2; - static const uint8_t kSubVoltage = 3; - static const uint8_t kSubIndex = 4; - static const uint8_t kSubHarmonic = 5; - static const uint8_t kSubA = 6; - static const uint8_t kSubPhase = 7; - static const uint8_t kSubPhaseTotal = 8; - static const uint8_t kSubAmplitude = 9; - static const uint8_t kSubMaxHarmonics = 10; +private: + static const uint8_t kSubEnabled = 0; + static const uint8_t kSubTau = 1; + static const uint8_t kSubNumHarmonics = 2; + static const uint8_t kSubVoltage = 3; + static const uint8_t kSubIndex = 4; + static const uint8_t kSubHarmonic = 5; + static const uint8_t kSubA = 6; + static const uint8_t kSubPhase = 7; + static const uint8_t kSubPhaseTotal = 8; + static const uint8_t kSubAmplitude = 9; + static const uint8_t kSubMaxHarmonics = 10; }; #endif // ANTICOGGING_PRO_CLIENT_H diff --git a/inc/arming_handler_client.hpp b/inc/arming_handler_client.hpp index 916d8a6..58aac07 100644 --- a/inc/arming_handler_client.hpp +++ b/inc/arming_handler_client.hpp @@ -20,71 +20,73 @@ const uint8_t kTypeArmingHandler = 86; -class ArmingHandlerClient : public ClientAbstract { - public: - ArmingHandlerClient(uint8_t obj_idn) - : ClientAbstract(kTypeArmingHandler, obj_idn), - always_armed_(kTypeArmingHandler, obj_idn, kSubAlwaysArmed), - arm_on_throttle_(kTypeArmingHandler, obj_idn, kSubArmOnThrottle), - arm_throttle_upper_limit_(kTypeArmingHandler, obj_idn, kSubArmThrottleUpperLimit), - arm_throttle_lower_limit_(kTypeArmingHandler, obj_idn, kSubArmThrottleLowerLimit), - disarm_on_throttle_(kTypeArmingHandler, obj_idn, kSubDisarmOnThrottle), - disarm_throttle_upper_limit_(kTypeArmingHandler, obj_idn, kSubDisarmThrottleUpperLimit), - disarm_throttle_lower_limit_(kTypeArmingHandler, obj_idn, kSubDisarmThrottleLowerLimit), - consecutive_arming_throttles_to_arm_(kTypeArmingHandler, obj_idn, - kSubConsecutiveArmingThrottlesToArm), - disarm_behavior_(kTypeArmingHandler, obj_idn, kSubDisarmBehavior), - disarm_song_option_(kTypeArmingHandler, obj_idn, kSubDisarmSongOption), - manual_arming_throttle_source_(kTypeArmingHandler, obj_idn, kSubManualArmingThrottleSource), - motor_armed_(kTypeArmingHandler, obj_idn, kSubMotorArmed){}; +class ArmingHandlerClient : public ClientAbstract +{ +public: + ArmingHandlerClient(uint8_t obj_idn) + : ClientAbstract(kTypeArmingHandler, obj_idn), + always_armed_(kTypeArmingHandler, obj_idn, kSubAlwaysArmed), + arm_on_throttle_(kTypeArmingHandler, obj_idn, kSubArmOnThrottle), + arm_throttle_upper_limit_(kTypeArmingHandler, obj_idn, kSubArmThrottleUpperLimit), + arm_throttle_lower_limit_(kTypeArmingHandler, obj_idn, kSubArmThrottleLowerLimit), + disarm_on_throttle_(kTypeArmingHandler, obj_idn, kSubDisarmOnThrottle), + disarm_throttle_upper_limit_(kTypeArmingHandler, obj_idn, kSubDisarmThrottleUpperLimit), + disarm_throttle_lower_limit_(kTypeArmingHandler, obj_idn, kSubDisarmThrottleLowerLimit), + consecutive_arming_throttles_to_arm_(kTypeArmingHandler, obj_idn, + kSubConsecutiveArmingThrottlesToArm), + disarm_behavior_(kTypeArmingHandler, obj_idn, kSubDisarmBehavior), + disarm_song_option_(kTypeArmingHandler, obj_idn, kSubDisarmSongOption), + manual_arming_throttle_source_(kTypeArmingHandler, obj_idn, kSubManualArmingThrottleSource), + motor_armed_(kTypeArmingHandler, obj_idn, kSubMotorArmed) {}; - // Client Entries - ClientEntry always_armed_; - ClientEntry arm_on_throttle_; - ClientEntry arm_throttle_upper_limit_; - ClientEntry arm_throttle_lower_limit_; - ClientEntry disarm_on_throttle_; - ClientEntry disarm_throttle_upper_limit_; - ClientEntry disarm_throttle_lower_limit_; - ClientEntry consecutive_arming_throttles_to_arm_; - ClientEntry disarm_behavior_; - ClientEntry disarm_song_option_; - ClientEntry manual_arming_throttle_source_; - ClientEntry motor_armed_; + // Client Entries + ClientEntry always_armed_; + ClientEntry arm_on_throttle_; + ClientEntry arm_throttle_upper_limit_; + ClientEntry arm_throttle_lower_limit_; + ClientEntry disarm_on_throttle_; + ClientEntry disarm_throttle_upper_limit_; + ClientEntry disarm_throttle_lower_limit_; + ClientEntry consecutive_arming_throttles_to_arm_; + ClientEntry disarm_behavior_; + ClientEntry disarm_song_option_; + ClientEntry manual_arming_throttle_source_; + ClientEntry motor_armed_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubMotorArmed + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - nullptr, // 0 - &always_armed_, // 1 - &arm_on_throttle_, // 2 - &arm_throttle_upper_limit_, // 3 - &arm_throttle_lower_limit_, // 4 - &disarm_on_throttle_, // 5 - &disarm_throttle_upper_limit_, // 6 - &disarm_throttle_lower_limit_, // 7 - &consecutive_arming_throttles_to_arm_, // 8 - &disarm_behavior_, // 9 - &disarm_song_option_, // 10 - &manual_arming_throttle_source_, // 11 - &motor_armed_ // 12 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubMotorArmed + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + nullptr, // 0 + &always_armed_, // 1 + &arm_on_throttle_, // 2 + &arm_throttle_upper_limit_, // 3 + &arm_throttle_lower_limit_, // 4 + &disarm_on_throttle_, // 5 + &disarm_throttle_upper_limit_, // 6 + &disarm_throttle_lower_limit_, // 7 + &consecutive_arming_throttles_to_arm_, // 8 + &disarm_behavior_, // 9 + &disarm_song_option_, // 10 + &manual_arming_throttle_source_, // 11 + &motor_armed_ // 12 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubAlwaysArmed = 1; - static const uint8_t kSubArmOnThrottle = 2; - static const uint8_t kSubArmThrottleUpperLimit = 3; - static const uint8_t kSubArmThrottleLowerLimit = 4; - static const uint8_t kSubDisarmOnThrottle = 5; - static const uint8_t kSubDisarmThrottleUpperLimit = 6; - static const uint8_t kSubDisarmThrottleLowerLimit = 7; - static const uint8_t kSubConsecutiveArmingThrottlesToArm = 8; - static const uint8_t kSubDisarmBehavior = 9; - static const uint8_t kSubDisarmSongOption = 10; - static const uint8_t kSubManualArmingThrottleSource = 11; - static const uint8_t kSubMotorArmed = 12; +private: + static const uint8_t kSubAlwaysArmed = 1; + static const uint8_t kSubArmOnThrottle = 2; + static const uint8_t kSubArmThrottleUpperLimit = 3; + static const uint8_t kSubArmThrottleLowerLimit = 4; + static const uint8_t kSubDisarmOnThrottle = 5; + static const uint8_t kSubDisarmThrottleUpperLimit = 6; + static const uint8_t kSubDisarmThrottleLowerLimit = 7; + static const uint8_t kSubConsecutiveArmingThrottlesToArm = 8; + static const uint8_t kSubDisarmBehavior = 9; + static const uint8_t kSubDisarmSongOption = 10; + static const uint8_t kSubManualArmingThrottleSource = 11; + static const uint8_t kSubMotorArmed = 12; }; #endif /* ARMING_HANDLER_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/bipbuffer.h b/inc/bipbuffer.h index e912a6c..3db5236 100644 --- a/inc/bipbuffer.h +++ b/inc/bipbuffer.h @@ -16,7 +16,7 @@ Contributors: Matthew Piccoli, Raphael Van Hoffelen */ -/* +/* Changed header gaurd from pragma once to ifndef BipBuffer_H. Chaning types to fixed width integers. Removed destructor, AllocateBuffer, FreeBuffer. @@ -33,289 +33,281 @@ class BipBuffer { private: - uint8_t* pBuffer; // Pointer to the data buffer - uint16_t ixa; // Starting index of region A - uint16_t sza; // Size of region A - uint16_t ixb; // Starting index of region B - uint16_t szb; // Size of region B - uint16_t buflen; // Length of full buffer - uint16_t ixResrv; // Starting index of reserved region - uint16_t szResrv; // Size of the reserved region + uint8_t *pBuffer; // Pointer to the data buffer + uint16_t ixa; // Starting index of region A + uint16_t sza; // Size of region A + uint16_t ixb; // Starting index of region B + uint16_t szb; // Size of region B + uint16_t buflen; // Length of full buffer + uint16_t ixResrv; // Starting index of reserved region + uint16_t szResrv; // Size of the reserved region public: - BipBuffer() : pBuffer(NULL), ixa(0), sza(0), ixb(0), szb(0), buflen(0), ixResrv(0), szResrv(0) - { - - } - BipBuffer(uint8_t* buffer_in, uint16_t buffer_length_in) : ixa(0), sza(0), ixb(0), szb(0), ixResrv(0), szResrv(0) - { - pBuffer = buffer_in; - buflen = buffer_length_in; - } - - /// - /// \brief Clears the buffer of any allocations. - /// - /// Clears the buffer of any allocations or reservations. Note; it - /// does not wipe the buffer memory; it merely resets all pointers, - /// returning the buffer to a completely empty state ready for new - /// allocations. - /// - void Clear() - { - ixa = sza = ixb = szb = ixResrv = szResrv = 0; - } - - // Reserve - // - // Reserves space in the buffer for a memory write operation - // - // Parameters: - // int size amount of space to reserve - // uint16_t& reserved size of space actually reserved - // - // Returns: - // uint8_t* pointer to the reserved block - // - // Notes: - // Will return NULL for the pointer if no space can be allocated. - // Can return any value from 1 to size in reserved. - // Will return NULL if a previous reservation has not been committed. - uint8_t* Reserve(uint16_t size, uint16_t& reserved) - { - // We always allocate on B if B exists; this means we have two blocks and our buffer is filling. - if (szb) - { - uint16_t freespace = GetBFreeSpace(); - - if (size < freespace) freespace = size; - - if (freespace == 0) return NULL; - - szResrv = freespace; - reserved = freespace; - ixResrv = ixb + szb; - return pBuffer + ixResrv; - } - else - { - // Block b does not exist, so we can check if the space AFTER a is bigger than the space - // before A, and allocate the bigger one. - uint16_t freespace = GetSpaceAfterA(); - if (freespace >= ixa) // If space after A > space before - { - if (freespace == 0) return NULL; - if (size < freespace) freespace = size; - - szResrv = freespace; - reserved = freespace; - ixResrv = ixa + sza; - return pBuffer + ixResrv; - } - else // space before A > space after A - { - if (ixa == 0) return NULL; - if (ixa < size) size = ixa; - szResrv = size; - reserved = size; - ixResrv = 0; - return pBuffer; - } - } - } - - // Commit - // - // Commits space that has been written to in the buffer - // - // Parameters: - // uint16_t size number of bytes to commit - // - // Notes: - // Committing a size > than the reserved size will cause an assert in a debug build; - // in a release build, the actual reserved size will be used. - // Committing a size < than the reserved size will commit that amount of data, and release - // the rest of the space. - // Committing a size of 0 will release the reservation. - void Commit(uint16_t size) - { - if (size == 0) - { - // decommit any reservation - szResrv = ixResrv = 0; - return; - } - - CommitPartial(size); - - // Decommit rest of reservation - ixResrv = 0; - szResrv = 0; - } - - // CommitPartial - // - // Commits space that has been written to in the buffer, but does not cancel the rest of the reservation - // - // Parameters: - // uint16_t size number of bytes to commit - // - // Returns: - // uint16_t number of bytes actually committed - // - // Notes: - // Committing a size > than the reserved size will cause an assert in a debug build; - // in a release build, the actual reserved size will be used. - // Committing a size < than the reserved size will commit that amount of data, but will not realese the remaining reservation - // Committing a size of 0 does nothing (space is still reserved) - uint16_t CommitPartial(uint16_t size) - { - // If we try to commit more space than we asked for, clip to the size we asked for. - if (size > szResrv) - { - size = szResrv; - } - - // If we have no blocks being used currently, we create one in A. - if (sza == 0 && szb == 0) - { - ixa = ixResrv; - } - - // If the reserve index is at the end of block A - if (ixResrv == sza + ixa) - { - sza += size; // Grow A by committed size - } - else - { - szb += size; // Otherwise grow B by committed size - } - - // Advance the reserved index - ixResrv += size; - // Update reserved size - szResrv -= size; - - return size; - } - - // GetContiguousBlock - // - // Gets a pointer to the first contiguous block in the buffer, and returns the size of that block. - // - // Parameters: - // uint16_t & size returns the size of the first contiguous block - // - // Returns: - // uint8_t* pointer to the first contiguous block, or NULL if empty. - uint8_t* GetContiguousBlock(uint16_t& size) - { - if (sza == 0) - { - size = 0; - return NULL; - } - - size = sza; - return pBuffer + ixa; - - } - - // DecommitBlock - // - // Decommits space from the first contiguous block - // - // Parameters: - // int size amount of memory to decommit - // - // Returns: - // nothing - void DecommitBlock(uint16_t size) - { - if (size >= sza) - { - ixa = ixb; - sza = szb; - ixb = 0; - szb = 0; - } - else - { - sza -= size; - ixa += size; - } - } - - // GetCommittedSize - // - // Queries how much data (in total) has been committed in the buffer - // - // Parameters: - // none - // - // Returns: - // uint16_t total amount of committed data in the buffer - uint16_t GetCommittedSize() const - { - return sza + szb; - } - - // GetReservationSize - // - // Queries how much space has been reserved in the buffer. - // - // Parameters: - // none - // - // Returns: - // uint16_t number of bytes that have been reserved - // - // Notes: - // A return value of 0 indicates that no space has been reserved - uint16_t GetReservationSize() const - { - return szResrv; - } - - // GetBufferSize - // - // Queries the maximum total size of the buffer - // - // Parameters: - // none - // - // Returns: - // uint16_t total size of buffer - uint16_t GetBufferSize() const - { - return buflen; - } - - // IsInitialized - // - // Queries whether or not the buffer has been allocated - // - // Parameters: - // none - // - // Returns: - // uint8_t true if the buffer has been allocated - uint8_t IsInitialized() const - { - return pBuffer != NULL; - } + BipBuffer() : pBuffer(NULL), ixa(0), sza(0), ixb(0), szb(0), buflen(0), ixResrv(0), szResrv(0) + { + + } + BipBuffer(uint8_t *buffer_in, uint16_t buffer_length_in) : ixa(0), sza(0), ixb(0), szb(0), ixResrv(0), szResrv(0) + { + pBuffer = buffer_in; + buflen = buffer_length_in; + } + + /// + /// \brief Clears the buffer of any allocations. + /// + /// Clears the buffer of any allocations or reservations. Note; it + /// does not wipe the buffer memory; it merely resets all pointers, + /// returning the buffer to a completely empty state ready for new + /// allocations. + /// + void Clear() + { + ixa = sza = ixb = szb = ixResrv = szResrv = 0; + } + + // Reserve + // + // Reserves space in the buffer for a memory write operation + // + // Parameters: + // int size amount of space to reserve + // uint16_t& reserved size of space actually reserved + // + // Returns: + // uint8_t* pointer to the reserved block + // + // Notes: + // Will return NULL for the pointer if no space can be allocated. + // Can return any value from 1 to size in reserved. + // Will return NULL if a previous reservation has not been committed. + uint8_t *Reserve(uint16_t size, uint16_t &reserved) + { + // We always allocate on B if B exists; this means we have two blocks and our buffer is filling. + if (szb) { + uint16_t freespace = GetBFreeSpace(); + + if (size < freespace) { freespace = size; } + + if (freespace == 0) { return NULL; } + + szResrv = freespace; + reserved = freespace; + ixResrv = ixb + szb; + return pBuffer + ixResrv; + + } else { + // Block b does not exist, so we can check if the space AFTER a is bigger than the space + // before A, and allocate the bigger one. + uint16_t freespace = GetSpaceAfterA(); + + if (freespace >= ixa) { // If space after A > space before + if (freespace == 0) { return NULL; } + + if (size < freespace) { freespace = size; } + + szResrv = freespace; + reserved = freespace; + ixResrv = ixa + sza; + return pBuffer + ixResrv; + + } else { // space before A > space after A + if (ixa == 0) { return NULL; } + + if (ixa < size) { size = ixa; } + + szResrv = size; + reserved = size; + ixResrv = 0; + return pBuffer; + } + } + } + + // Commit + // + // Commits space that has been written to in the buffer + // + // Parameters: + // uint16_t size number of bytes to commit + // + // Notes: + // Committing a size > than the reserved size will cause an assert in a debug build; + // in a release build, the actual reserved size will be used. + // Committing a size < than the reserved size will commit that amount of data, and release + // the rest of the space. + // Committing a size of 0 will release the reservation. + void Commit(uint16_t size) + { + if (size == 0) { + // decommit any reservation + szResrv = ixResrv = 0; + return; + } + + CommitPartial(size); + + // Decommit rest of reservation + ixResrv = 0; + szResrv = 0; + } + + // CommitPartial + // + // Commits space that has been written to in the buffer, but does not cancel the rest of the reservation + // + // Parameters: + // uint16_t size number of bytes to commit + // + // Returns: + // uint16_t number of bytes actually committed + // + // Notes: + // Committing a size > than the reserved size will cause an assert in a debug build; + // in a release build, the actual reserved size will be used. + // Committing a size < than the reserved size will commit that amount of data, but will not realese the remaining reservation + // Committing a size of 0 does nothing (space is still reserved) + uint16_t CommitPartial(uint16_t size) + { + // If we try to commit more space than we asked for, clip to the size we asked for. + if (size > szResrv) { + size = szResrv; + } + + // If we have no blocks being used currently, we create one in A. + if (sza == 0 && szb == 0) { + ixa = ixResrv; + } + + // If the reserve index is at the end of block A + if (ixResrv == sza + ixa) { + sza += size; // Grow A by committed size + + } else { + szb += size; // Otherwise grow B by committed size + } + + // Advance the reserved index + ixResrv += size; + // Update reserved size + szResrv -= size; + + return size; + } + + // GetContiguousBlock + // + // Gets a pointer to the first contiguous block in the buffer, and returns the size of that block. + // + // Parameters: + // uint16_t & size returns the size of the first contiguous block + // + // Returns: + // uint8_t* pointer to the first contiguous block, or NULL if empty. + uint8_t *GetContiguousBlock(uint16_t &size) + { + if (sza == 0) { + size = 0; + return NULL; + } + + size = sza; + return pBuffer + ixa; + + } + + // DecommitBlock + // + // Decommits space from the first contiguous block + // + // Parameters: + // int size amount of memory to decommit + // + // Returns: + // nothing + void DecommitBlock(uint16_t size) + { + if (size >= sza) { + ixa = ixb; + sza = szb; + ixb = 0; + szb = 0; + + } else { + sza -= size; + ixa += size; + } + } + + // GetCommittedSize + // + // Queries how much data (in total) has been committed in the buffer + // + // Parameters: + // none + // + // Returns: + // uint16_t total amount of committed data in the buffer + uint16_t GetCommittedSize() const + { + return sza + szb; + } + + // GetReservationSize + // + // Queries how much space has been reserved in the buffer. + // + // Parameters: + // none + // + // Returns: + // uint16_t number of bytes that have been reserved + // + // Notes: + // A return value of 0 indicates that no space has been reserved + uint16_t GetReservationSize() const + { + return szResrv; + } + + // GetBufferSize + // + // Queries the maximum total size of the buffer + // + // Parameters: + // none + // + // Returns: + // uint16_t total size of buffer + uint16_t GetBufferSize() const + { + return buflen; + } + + // IsInitialized + // + // Queries whether or not the buffer has been allocated + // + // Parameters: + // none + // + // Returns: + // uint8_t true if the buffer has been allocated + uint8_t IsInitialized() const + { + return pBuffer != NULL; + } private: - uint16_t GetSpaceAfterA() const - { - return buflen - ixa - sza; - } - - uint16_t GetBFreeSpace() const - { - return ixa - ixb - szb; - } + uint16_t GetSpaceAfterA() const + { + return buflen - ixa - sza; + } + + uint16_t GetBFreeSpace() const + { + return ixa - ixb - szb; + } }; #endif // BipBuffer_H \ No newline at end of file diff --git a/inc/brushless_drive_client.hpp b/inc/brushless_drive_client.hpp index 6221529..468ccab 100644 --- a/inc/brushless_drive_client.hpp +++ b/inc/brushless_drive_client.hpp @@ -20,260 +20,261 @@ const uint8_t kTypeBrushlessDrive = 50; -class BrushlessDriveClient: public ClientAbstract{ - public: - BrushlessDriveClient(uint8_t obj_idn): - ClientAbstract( kTypeBrushlessDrive, obj_idn), - drive_mode_( kTypeBrushlessDrive, obj_idn, kSubDriveMode), - drive_phase_pwm_( kTypeBrushlessDrive, obj_idn, kSubDrivePhasePwm), - drive_phase_volts_( kTypeBrushlessDrive, obj_idn, kSubDrivePhaseVolts), - drive_spin_pwm_( kTypeBrushlessDrive, obj_idn, kSubDriveSpinPwm), - drive_spin_volts_( kTypeBrushlessDrive, obj_idn, kSubDriveSpinVolts), - drive_brake_( kTypeBrushlessDrive, obj_idn, kSubDriveBrake), - drive_coast_( kTypeBrushlessDrive, obj_idn, kSubDriveCoast), - drive_angle_offset_( kTypeBrushlessDrive, obj_idn, kSubDriveAngleOffset), - drive_pwm_( kTypeBrushlessDrive, obj_idn, kSubDrivePwm), - drive_volts_( kTypeBrushlessDrive, obj_idn, kSubDriveVolts), - mech_lead_angle_( kTypeBrushlessDrive, obj_idn, kSubMechLeadAngle), - obs_supply_volts_( kTypeBrushlessDrive, obj_idn, kSubObsSupplyVolts), - obs_angle_( kTypeBrushlessDrive, obj_idn, kSubObsAngle), - obs_velocity_( kTypeBrushlessDrive, obj_idn, kSubObsVelocity), - motor_pole_pairs_( kTypeBrushlessDrive, obj_idn, kSubMotorPolePairs), - motor_emf_shape_( kTypeBrushlessDrive, obj_idn, kSubMotorEmfShape), - permute_wires_( kTypeBrushlessDrive, obj_idn, kSubPermuteWires), - calibration_angle_( kTypeBrushlessDrive, obj_idn, kSubOCalibrationAngle), - lead_time_( kTypeBrushlessDrive, obj_idn, kSubLeadTime), - commutation_hz_( kTypeBrushlessDrive, obj_idn, kSubCommutationHz), - phase_angle_( kTypeBrushlessDrive, obj_idn, kSubPhaseAngle), - drive_volts_addition_( kTypeBrushlessDrive, obj_idn, kSubDriveVoltsAddition), - angle_adjust_enable_( kTypeBrushlessDrive, obj_idn, kSubAngleAdjustEnable), - motor_emf_calc_( kTypeBrushlessDrive, obj_idn, kSubMotorEmfCalc), - angle_adjustment_( kTypeBrushlessDrive, obj_idn, kSubAngleAdjustment), - angle_adjust_max_( kTypeBrushlessDrive, obj_idn, kSubAngleAdjustMax), - angle_adjust_kp_( kTypeBrushlessDrive, obj_idn, kSubAngleAdjustKp), - angle_adjust_ki_( kTypeBrushlessDrive, obj_idn, kSubAngleAdjustKi), - v_max_start_( kTypeBrushlessDrive, obj_idn, kSubVMaxStart), - motor_Kv_( kTypeBrushlessDrive, obj_idn, kSubMotorKv), - motor_R_ohm_( kTypeBrushlessDrive, obj_idn, kSubMotorROhm), - motor_I_max_( kTypeBrushlessDrive, obj_idn, kSubMotorIMax), - volts_limit_( kTypeBrushlessDrive, obj_idn, kSubVoltsLimit), - est_motor_amps_( kTypeBrushlessDrive, obj_idn, kSubEstMotorAmps), - est_motor_torque_( kTypeBrushlessDrive, obj_idn, kSubEstMotorTorque), - motor_redline_start_( kTypeBrushlessDrive, obj_idn, kSubMotorRedlineStart), - motor_redline_end_( kTypeBrushlessDrive, obj_idn, kSubMotorRedlineEnd), - motor_l_( kTypeBrushlessDrive, obj_idn, kSubMotorL), - derate_( kTypeBrushlessDrive, obj_idn, kSubDerate), - motor_i_soft_start_( kTypeBrushlessDrive, obj_idn, kSubMotorISoftStart), - motor_i_soft_end_( kTypeBrushlessDrive, obj_idn, kSubMotorISoftEnd), - emf_( kTypeBrushlessDrive, obj_idn, kSubEmf), - volts_at_max_amps_( kTypeBrushlessDrive, obj_idn, kSubVoltsAtMaxAmps), - slew_volts_per_second_( kTypeBrushlessDrive, obj_idn, kSubSlewVoltsPerSecond), - slew_enable_( kTypeBrushlessDrive, obj_idn, kSubSlewEnable), - motoring_supply_current_limit_( kTypeBrushlessDrive, obj_idn, kSubMotoringSupplyCurrentLimit), - regen_supply_current_limit_( kTypeBrushlessDrive, obj_idn, kSubRegenSupplyCurrentLimit), - supply_current_limit_enable_( kTypeBrushlessDrive, obj_idn, kSubSupplyCurrentLimitEnable), - regen_limiting_( kTypeBrushlessDrive, obj_idn, kSubRegenLimiting), - regen_limit_adjust_( kTypeBrushlessDrive, obj_idn, kSubRegenLimitAdjust), - motoring_limiting_( kTypeBrushlessDrive, obj_idn, kSubMotoringLimiting), - motoring_limit_adjust_( kTypeBrushlessDrive, obj_idn, kSubMotoringLimitAdjust), - regen_limit_kp_( kTypeBrushlessDrive, obj_idn, kSubRegenLimitKp), - regen_limit_ki_( kTypeBrushlessDrive, obj_idn, kSubRegenLimitKi), - regen_limit_max_( kTypeBrushlessDrive, obj_idn, kSubRegenLimitMax), - motoring_limit_kp_( kTypeBrushlessDrive, obj_idn, kSubMotoringLimitKp), - motoring_limit_ki_( kTypeBrushlessDrive, obj_idn, kSubMotoringLimitKi), - motoring_limit_max_( kTypeBrushlessDrive, obj_idn, kSubMotoringLimitMax) - {}; +class BrushlessDriveClient: public ClientAbstract +{ +public: + BrushlessDriveClient(uint8_t obj_idn): + ClientAbstract(kTypeBrushlessDrive, obj_idn), + drive_mode_(kTypeBrushlessDrive, obj_idn, kSubDriveMode), + drive_phase_pwm_(kTypeBrushlessDrive, obj_idn, kSubDrivePhasePwm), + drive_phase_volts_(kTypeBrushlessDrive, obj_idn, kSubDrivePhaseVolts), + drive_spin_pwm_(kTypeBrushlessDrive, obj_idn, kSubDriveSpinPwm), + drive_spin_volts_(kTypeBrushlessDrive, obj_idn, kSubDriveSpinVolts), + drive_brake_(kTypeBrushlessDrive, obj_idn, kSubDriveBrake), + drive_coast_(kTypeBrushlessDrive, obj_idn, kSubDriveCoast), + drive_angle_offset_(kTypeBrushlessDrive, obj_idn, kSubDriveAngleOffset), + drive_pwm_(kTypeBrushlessDrive, obj_idn, kSubDrivePwm), + drive_volts_(kTypeBrushlessDrive, obj_idn, kSubDriveVolts), + mech_lead_angle_(kTypeBrushlessDrive, obj_idn, kSubMechLeadAngle), + obs_supply_volts_(kTypeBrushlessDrive, obj_idn, kSubObsSupplyVolts), + obs_angle_(kTypeBrushlessDrive, obj_idn, kSubObsAngle), + obs_velocity_(kTypeBrushlessDrive, obj_idn, kSubObsVelocity), + motor_pole_pairs_(kTypeBrushlessDrive, obj_idn, kSubMotorPolePairs), + motor_emf_shape_(kTypeBrushlessDrive, obj_idn, kSubMotorEmfShape), + permute_wires_(kTypeBrushlessDrive, obj_idn, kSubPermuteWires), + calibration_angle_(kTypeBrushlessDrive, obj_idn, kSubOCalibrationAngle), + lead_time_(kTypeBrushlessDrive, obj_idn, kSubLeadTime), + commutation_hz_(kTypeBrushlessDrive, obj_idn, kSubCommutationHz), + phase_angle_(kTypeBrushlessDrive, obj_idn, kSubPhaseAngle), + drive_volts_addition_(kTypeBrushlessDrive, obj_idn, kSubDriveVoltsAddition), + angle_adjust_enable_(kTypeBrushlessDrive, obj_idn, kSubAngleAdjustEnable), + motor_emf_calc_(kTypeBrushlessDrive, obj_idn, kSubMotorEmfCalc), + angle_adjustment_(kTypeBrushlessDrive, obj_idn, kSubAngleAdjustment), + angle_adjust_max_(kTypeBrushlessDrive, obj_idn, kSubAngleAdjustMax), + angle_adjust_kp_(kTypeBrushlessDrive, obj_idn, kSubAngleAdjustKp), + angle_adjust_ki_(kTypeBrushlessDrive, obj_idn, kSubAngleAdjustKi), + v_max_start_(kTypeBrushlessDrive, obj_idn, kSubVMaxStart), + motor_Kv_(kTypeBrushlessDrive, obj_idn, kSubMotorKv), + motor_R_ohm_(kTypeBrushlessDrive, obj_idn, kSubMotorROhm), + motor_I_max_(kTypeBrushlessDrive, obj_idn, kSubMotorIMax), + volts_limit_(kTypeBrushlessDrive, obj_idn, kSubVoltsLimit), + est_motor_amps_(kTypeBrushlessDrive, obj_idn, kSubEstMotorAmps), + est_motor_torque_(kTypeBrushlessDrive, obj_idn, kSubEstMotorTorque), + motor_redline_start_(kTypeBrushlessDrive, obj_idn, kSubMotorRedlineStart), + motor_redline_end_(kTypeBrushlessDrive, obj_idn, kSubMotorRedlineEnd), + motor_l_(kTypeBrushlessDrive, obj_idn, kSubMotorL), + derate_(kTypeBrushlessDrive, obj_idn, kSubDerate), + motor_i_soft_start_(kTypeBrushlessDrive, obj_idn, kSubMotorISoftStart), + motor_i_soft_end_(kTypeBrushlessDrive, obj_idn, kSubMotorISoftEnd), + emf_(kTypeBrushlessDrive, obj_idn, kSubEmf), + volts_at_max_amps_(kTypeBrushlessDrive, obj_idn, kSubVoltsAtMaxAmps), + slew_volts_per_second_(kTypeBrushlessDrive, obj_idn, kSubSlewVoltsPerSecond), + slew_enable_(kTypeBrushlessDrive, obj_idn, kSubSlewEnable), + motoring_supply_current_limit_(kTypeBrushlessDrive, obj_idn, kSubMotoringSupplyCurrentLimit), + regen_supply_current_limit_(kTypeBrushlessDrive, obj_idn, kSubRegenSupplyCurrentLimit), + supply_current_limit_enable_(kTypeBrushlessDrive, obj_idn, kSubSupplyCurrentLimitEnable), + regen_limiting_(kTypeBrushlessDrive, obj_idn, kSubRegenLimiting), + regen_limit_adjust_(kTypeBrushlessDrive, obj_idn, kSubRegenLimitAdjust), + motoring_limiting_(kTypeBrushlessDrive, obj_idn, kSubMotoringLimiting), + motoring_limit_adjust_(kTypeBrushlessDrive, obj_idn, kSubMotoringLimitAdjust), + regen_limit_kp_(kTypeBrushlessDrive, obj_idn, kSubRegenLimitKp), + regen_limit_ki_(kTypeBrushlessDrive, obj_idn, kSubRegenLimitKi), + regen_limit_max_(kTypeBrushlessDrive, obj_idn, kSubRegenLimitMax), + motoring_limit_kp_(kTypeBrushlessDrive, obj_idn, kSubMotoringLimitKp), + motoring_limit_ki_(kTypeBrushlessDrive, obj_idn, kSubMotoringLimitKi), + motoring_limit_max_(kTypeBrushlessDrive, obj_idn, kSubMotoringLimitMax) + {}; - // Client Entries - ClientEntry drive_mode_; - ClientEntry drive_phase_pwm_; - ClientEntry drive_phase_volts_; - ClientEntry drive_spin_pwm_; - ClientEntry drive_spin_volts_; - ClientEntryVoid drive_brake_; - ClientEntryVoid drive_coast_; - ClientEntry drive_angle_offset_; - ClientEntry drive_pwm_; - ClientEntry drive_volts_; - ClientEntry mech_lead_angle_; - ClientEntry obs_supply_volts_; - ClientEntry obs_angle_; - ClientEntry obs_velocity_; - ClientEntry motor_pole_pairs_; - ClientEntry motor_emf_shape_; - ClientEntry permute_wires_; - ClientEntry calibration_angle_; - ClientEntry lead_time_; - ClientEntry commutation_hz_; - ClientEntry phase_angle_; - ClientEntry drive_volts_addition_; - ClientEntry angle_adjust_enable_; - ClientEntry motor_emf_calc_; - ClientEntry angle_adjustment_; - ClientEntry angle_adjust_max_; - ClientEntry angle_adjust_kp_; - ClientEntry angle_adjust_ki_; - ClientEntry v_max_start_; - ClientEntry motor_Kv_; - ClientEntry motor_R_ohm_; - ClientEntry motor_I_max_; - ClientEntry volts_limit_; - ClientEntry est_motor_amps_; - ClientEntry est_motor_torque_; - ClientEntry motor_redline_start_; - ClientEntry motor_redline_end_; - ClientEntry motor_l_; - ClientEntry derate_; - ClientEntry motor_i_soft_start_; - ClientEntry motor_i_soft_end_; - ClientEntry emf_; - ClientEntry volts_at_max_amps_; - ClientEntry slew_volts_per_second_; - ClientEntry slew_enable_; - ClientEntry motoring_supply_current_limit_; - ClientEntry regen_supply_current_limit_; - ClientEntry supply_current_limit_enable_; - ClientEntry regen_limiting_; - ClientEntry regen_limit_adjust_; - ClientEntry motoring_limiting_; - ClientEntry motoring_limit_adjust_; - ClientEntry regen_limit_kp_; - ClientEntry regen_limit_ki_; - ClientEntry regen_limit_max_; - ClientEntry motoring_limit_kp_; - ClientEntry motoring_limit_ki_; - ClientEntry motoring_limit_max_; + // Client Entries + ClientEntry drive_mode_; + ClientEntry drive_phase_pwm_; + ClientEntry drive_phase_volts_; + ClientEntry drive_spin_pwm_; + ClientEntry drive_spin_volts_; + ClientEntryVoid drive_brake_; + ClientEntryVoid drive_coast_; + ClientEntry drive_angle_offset_; + ClientEntry drive_pwm_; + ClientEntry drive_volts_; + ClientEntry mech_lead_angle_; + ClientEntry obs_supply_volts_; + ClientEntry obs_angle_; + ClientEntry obs_velocity_; + ClientEntry motor_pole_pairs_; + ClientEntry motor_emf_shape_; + ClientEntry permute_wires_; + ClientEntry calibration_angle_; + ClientEntry lead_time_; + ClientEntry commutation_hz_; + ClientEntry phase_angle_; + ClientEntry drive_volts_addition_; + ClientEntry angle_adjust_enable_; + ClientEntry motor_emf_calc_; + ClientEntry angle_adjustment_; + ClientEntry angle_adjust_max_; + ClientEntry angle_adjust_kp_; + ClientEntry angle_adjust_ki_; + ClientEntry v_max_start_; + ClientEntry motor_Kv_; + ClientEntry motor_R_ohm_; + ClientEntry motor_I_max_; + ClientEntry volts_limit_; + ClientEntry est_motor_amps_; + ClientEntry est_motor_torque_; + ClientEntry motor_redline_start_; + ClientEntry motor_redline_end_; + ClientEntry motor_l_; + ClientEntry derate_; + ClientEntry motor_i_soft_start_; + ClientEntry motor_i_soft_end_; + ClientEntry emf_; + ClientEntry volts_at_max_amps_; + ClientEntry slew_volts_per_second_; + ClientEntry slew_enable_; + ClientEntry motoring_supply_current_limit_; + ClientEntry regen_supply_current_limit_; + ClientEntry supply_current_limit_enable_; + ClientEntry regen_limiting_; + ClientEntry regen_limit_adjust_; + ClientEntry motoring_limiting_; + ClientEntry motoring_limit_adjust_; + ClientEntry regen_limit_kp_; + ClientEntry regen_limit_ki_; + ClientEntry regen_limit_max_; + ClientEntry motoring_limit_kp_; + ClientEntry motoring_limit_ki_; + ClientEntry motoring_limit_max_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubMotoringLimitMax+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &drive_mode_, // 0 - &drive_phase_pwm_, // 1 - &drive_phase_volts_, // 2 - &drive_spin_pwm_, // 3 - &drive_spin_volts_, // 4 - &drive_brake_, // 5 - &drive_coast_, // 6 - &drive_angle_offset_, // 7 - &drive_pwm_, // 8 - &drive_volts_, // 9 - &mech_lead_angle_, // 10 - &obs_supply_volts_, // 11 - &obs_angle_, // 12 - &obs_velocity_, // 13 - &motor_pole_pairs_, // 14 - &motor_emf_shape_, // 15 - &permute_wires_, // 16 - &calibration_angle_, // 17 - &lead_time_, // 18 - &commutation_hz_, // 19 - &phase_angle_, // 20 - &drive_volts_addition_, // 21 - &angle_adjust_enable_, // 22 - &motor_emf_calc_, // 23 - &angle_adjustment_, // 24 - &angle_adjust_max_, // 25 - &angle_adjust_kp_, // 26 - &angle_adjust_ki_, // 27 - nullptr, // 28 - &v_max_start_, // 29 - nullptr, // 30 - nullptr, // 31 - &motor_Kv_, // 32 - &motor_R_ohm_, // 33 - &motor_I_max_, // 34 - &volts_limit_, // 35 - &est_motor_amps_, // 36 - &est_motor_torque_, // 37 - &motor_redline_start_, // 38 - &motor_redline_end_, // 39 - &motor_l_, // 40 - &derate_, // 41 - &motor_i_soft_start_, // 42 - &motor_i_soft_end_, // 43 - &emf_, // 44 - &volts_at_max_amps_, // 45 - &slew_volts_per_second_, // 46 - &slew_enable_, // 47 - &motoring_supply_current_limit_, // 48 - ®en_supply_current_limit_, // 49 - &supply_current_limit_enable_, // 50 - ®en_limiting_, // 51 - ®en_limit_adjust_, // 52 - &motoring_limiting_, // 53 - &motoring_limit_adjust_, // 54 - ®en_limit_kp_, // 55 - ®en_limit_ki_, // 56 - ®en_limit_max_, // 57 - &motoring_limit_kp_, // 58 - &motoring_limit_ki_, // 59 - &motoring_limit_max_ // 60 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubMotoringLimitMax + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &drive_mode_, // 0 + &drive_phase_pwm_, // 1 + &drive_phase_volts_, // 2 + &drive_spin_pwm_, // 3 + &drive_spin_volts_, // 4 + &drive_brake_, // 5 + &drive_coast_, // 6 + &drive_angle_offset_, // 7 + &drive_pwm_, // 8 + &drive_volts_, // 9 + &mech_lead_angle_, // 10 + &obs_supply_volts_, // 11 + &obs_angle_, // 12 + &obs_velocity_, // 13 + &motor_pole_pairs_, // 14 + &motor_emf_shape_, // 15 + &permute_wires_, // 16 + &calibration_angle_, // 17 + &lead_time_, // 18 + &commutation_hz_, // 19 + &phase_angle_, // 20 + &drive_volts_addition_, // 21 + &angle_adjust_enable_, // 22 + &motor_emf_calc_, // 23 + &angle_adjustment_, // 24 + &angle_adjust_max_, // 25 + &angle_adjust_kp_, // 26 + &angle_adjust_ki_, // 27 + nullptr, // 28 + &v_max_start_, // 29 + nullptr, // 30 + nullptr, // 31 + &motor_Kv_, // 32 + &motor_R_ohm_, // 33 + &motor_I_max_, // 34 + &volts_limit_, // 35 + &est_motor_amps_, // 36 + &est_motor_torque_, // 37 + &motor_redline_start_, // 38 + &motor_redline_end_, // 39 + &motor_l_, // 40 + &derate_, // 41 + &motor_i_soft_start_, // 42 + &motor_i_soft_end_, // 43 + &emf_, // 44 + &volts_at_max_amps_, // 45 + &slew_volts_per_second_, // 46 + &slew_enable_, // 47 + &motoring_supply_current_limit_, // 48 + ®en_supply_current_limit_, // 49 + &supply_current_limit_enable_, // 50 + ®en_limiting_, // 51 + ®en_limit_adjust_, // 52 + &motoring_limiting_, // 53 + &motoring_limit_adjust_, // 54 + ®en_limit_kp_, // 55 + ®en_limit_ki_, // 56 + ®en_limit_max_, // 57 + &motoring_limit_kp_, // 58 + &motoring_limit_ki_, // 59 + &motoring_limit_max_ // 60 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubDriveMode = 0; - static const uint8_t kSubDrivePhasePwm = 1; - static const uint8_t kSubDrivePhaseVolts = 2; - static const uint8_t kSubDriveSpinPwm = 3; - static const uint8_t kSubDriveSpinVolts = 4; - static const uint8_t kSubDriveBrake = 5; - static const uint8_t kSubDriveCoast = 6; - static const uint8_t kSubDriveAngleOffset = 7; - static const uint8_t kSubDrivePwm = 8; - static const uint8_t kSubDriveVolts = 9; - static const uint8_t kSubMechLeadAngle = 10; - static const uint8_t kSubObsSupplyVolts = 11; - static const uint8_t kSubObsAngle = 12; - static const uint8_t kSubObsVelocity = 13; - static const uint8_t kSubMotorPolePairs = 14; - static const uint8_t kSubMotorEmfShape = 15; - static const uint8_t kSubPermuteWires = 16; - static const uint8_t kSubOCalibrationAngle = 17; - static const uint8_t kSubLeadTime = 18; - static const uint8_t kSubCommutationHz = 19; - static const uint8_t kSubPhaseAngle = 20; - static const uint8_t kSubDriveVoltsAddition = 21; - static const uint8_t kSubAngleAdjustEnable = 22; - static const uint8_t kSubMotorEmfCalc = 23; - static const uint8_t kSubAngleAdjustment = 24; - static const uint8_t kSubAngleAdjustMax = 25; - static const uint8_t kSubAngleAdjustKp = 26; - static const uint8_t kSubAngleAdjustKi = 27; - static const uint8_t kSubVMaxStart = 29; - static const uint8_t kSubMotorKv = 32; - static const uint8_t kSubMotorROhm = 33; - static const uint8_t kSubMotorIMax = 34; - static const uint8_t kSubVoltsLimit = 35; - static const uint8_t kSubEstMotorAmps = 36; - static const uint8_t kSubEstMotorTorque = 37; - static const uint8_t kSubMotorRedlineStart = 38; - static const uint8_t kSubMotorRedlineEnd = 39; - static const uint8_t kSubMotorL = 40; - static const uint8_t kSubDerate = 41; - static const uint8_t kSubMotorISoftStart = 42; - static const uint8_t kSubMotorISoftEnd = 43; - static const uint8_t kSubEmf = 44; - static const uint8_t kSubVoltsAtMaxAmps = 45; - static const uint8_t kSubSlewVoltsPerSecond = 46; - static const uint8_t kSubSlewEnable = 47; - static const uint8_t kSubMotoringSupplyCurrentLimit = 48; - static const uint8_t kSubRegenSupplyCurrentLimit = 49; - static const uint8_t kSubSupplyCurrentLimitEnable = 50; - static const uint8_t kSubRegenLimiting = 51; - static const uint8_t kSubRegenLimitAdjust = 52; - static const uint8_t kSubMotoringLimiting = 53; - static const uint8_t kSubMotoringLimitAdjust = 54; - static const uint8_t kSubRegenLimitKp = 55; - static const uint8_t kSubRegenLimitKi = 56; - static const uint8_t kSubRegenLimitMax = 57; - static const uint8_t kSubMotoringLimitKp = 58; - static const uint8_t kSubMotoringLimitKi = 59; - static const uint8_t kSubMotoringLimitMax = 60; +private: + static const uint8_t kSubDriveMode = 0; + static const uint8_t kSubDrivePhasePwm = 1; + static const uint8_t kSubDrivePhaseVolts = 2; + static const uint8_t kSubDriveSpinPwm = 3; + static const uint8_t kSubDriveSpinVolts = 4; + static const uint8_t kSubDriveBrake = 5; + static const uint8_t kSubDriveCoast = 6; + static const uint8_t kSubDriveAngleOffset = 7; + static const uint8_t kSubDrivePwm = 8; + static const uint8_t kSubDriveVolts = 9; + static const uint8_t kSubMechLeadAngle = 10; + static const uint8_t kSubObsSupplyVolts = 11; + static const uint8_t kSubObsAngle = 12; + static const uint8_t kSubObsVelocity = 13; + static const uint8_t kSubMotorPolePairs = 14; + static const uint8_t kSubMotorEmfShape = 15; + static const uint8_t kSubPermuteWires = 16; + static const uint8_t kSubOCalibrationAngle = 17; + static const uint8_t kSubLeadTime = 18; + static const uint8_t kSubCommutationHz = 19; + static const uint8_t kSubPhaseAngle = 20; + static const uint8_t kSubDriveVoltsAddition = 21; + static const uint8_t kSubAngleAdjustEnable = 22; + static const uint8_t kSubMotorEmfCalc = 23; + static const uint8_t kSubAngleAdjustment = 24; + static const uint8_t kSubAngleAdjustMax = 25; + static const uint8_t kSubAngleAdjustKp = 26; + static const uint8_t kSubAngleAdjustKi = 27; + static const uint8_t kSubVMaxStart = 29; + static const uint8_t kSubMotorKv = 32; + static const uint8_t kSubMotorROhm = 33; + static const uint8_t kSubMotorIMax = 34; + static const uint8_t kSubVoltsLimit = 35; + static const uint8_t kSubEstMotorAmps = 36; + static const uint8_t kSubEstMotorTorque = 37; + static const uint8_t kSubMotorRedlineStart = 38; + static const uint8_t kSubMotorRedlineEnd = 39; + static const uint8_t kSubMotorL = 40; + static const uint8_t kSubDerate = 41; + static const uint8_t kSubMotorISoftStart = 42; + static const uint8_t kSubMotorISoftEnd = 43; + static const uint8_t kSubEmf = 44; + static const uint8_t kSubVoltsAtMaxAmps = 45; + static const uint8_t kSubSlewVoltsPerSecond = 46; + static const uint8_t kSubSlewEnable = 47; + static const uint8_t kSubMotoringSupplyCurrentLimit = 48; + static const uint8_t kSubRegenSupplyCurrentLimit = 49; + static const uint8_t kSubSupplyCurrentLimitEnable = 50; + static const uint8_t kSubRegenLimiting = 51; + static const uint8_t kSubRegenLimitAdjust = 52; + static const uint8_t kSubMotoringLimiting = 53; + static const uint8_t kSubMotoringLimitAdjust = 54; + static const uint8_t kSubRegenLimitKp = 55; + static const uint8_t kSubRegenLimitKi = 56; + static const uint8_t kSubRegenLimitMax = 57; + static const uint8_t kSubMotoringLimitKp = 58; + static const uint8_t kSubMotoringLimitKi = 59; + static const uint8_t kSubMotoringLimitMax = 60; }; #endif /* BRUSHLESS_DRIVE_CLIENT_HPP_ */ diff --git a/inc/buzzer_control_client.hpp b/inc/buzzer_control_client.hpp index faff676..36abfa2 100644 --- a/inc/buzzer_control_client.hpp +++ b/inc/buzzer_control_client.hpp @@ -20,56 +20,57 @@ const uint8_t kTypeBuzzerControl = 61; -class BuzzerControlClient: public ClientAbstract{ - public: - BuzzerControlClient(uint8_t obj_idn): - ClientAbstract( kTypeBuzzerControl, obj_idn), - ctrl_mode_( kTypeBuzzerControl, obj_idn, kSubCtrlMode), - ctrl_brake_( kTypeBuzzerControl, obj_idn, kSubCtrlBrake), - ctrl_coast_( kTypeBuzzerControl, obj_idn, kSubCtrlCoast), - ctrl_note_( kTypeBuzzerControl, obj_idn, kSubCtrlNote), - volume_max_( kTypeBuzzerControl, obj_idn, kSubVolumeMax), - hz_( kTypeBuzzerControl, obj_idn, kSubHz), - volume_( kTypeBuzzerControl, obj_idn, kSubVolume), - duration_( kTypeBuzzerControl, obj_idn, kSubDuration) - {}; +class BuzzerControlClient: public ClientAbstract +{ +public: + BuzzerControlClient(uint8_t obj_idn): + ClientAbstract(kTypeBuzzerControl, obj_idn), + ctrl_mode_(kTypeBuzzerControl, obj_idn, kSubCtrlMode), + ctrl_brake_(kTypeBuzzerControl, obj_idn, kSubCtrlBrake), + ctrl_coast_(kTypeBuzzerControl, obj_idn, kSubCtrlCoast), + ctrl_note_(kTypeBuzzerControl, obj_idn, kSubCtrlNote), + volume_max_(kTypeBuzzerControl, obj_idn, kSubVolumeMax), + hz_(kTypeBuzzerControl, obj_idn, kSubHz), + volume_(kTypeBuzzerControl, obj_idn, kSubVolume), + duration_(kTypeBuzzerControl, obj_idn, kSubDuration) + {}; - // Client Entries - ClientEntryVoid ctrl_mode_; - ClientEntryVoid ctrl_brake_; - ClientEntryVoid ctrl_coast_; - ClientEntryVoid ctrl_note_; - ClientEntry volume_max_; - ClientEntry hz_; - ClientEntry volume_; - ClientEntry duration_; + // Client Entries + ClientEntryVoid ctrl_mode_; + ClientEntryVoid ctrl_brake_; + ClientEntryVoid ctrl_coast_; + ClientEntryVoid ctrl_note_; + ClientEntry volume_max_; + ClientEntry hz_; + ClientEntry volume_; + ClientEntry duration_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubDuration+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &ctrl_mode_, // 0 - &ctrl_brake_, // 1 - &ctrl_coast_, // 2 - &ctrl_note_, // 3 - &volume_max_, // 4 - &hz_, // 5 - &volume_, // 6 - &duration_, // 7 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubDuration + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &ctrl_mode_, // 0 + &ctrl_brake_, // 1 + &ctrl_coast_, // 2 + &ctrl_note_, // 3 + &volume_max_, // 4 + &hz_, // 5 + &volume_, // 6 + &duration_, // 7 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubCtrlMode = 0; - static const uint8_t kSubCtrlBrake = 1; - static const uint8_t kSubCtrlCoast = 2; - static const uint8_t kSubCtrlNote = 3; - static const uint8_t kSubVolumeMax = 4; - static const uint8_t kSubHz = 5; - static const uint8_t kSubVolume = 6; - static const uint8_t kSubDuration = 7; +private: + static const uint8_t kSubCtrlMode = 0; + static const uint8_t kSubCtrlBrake = 1; + static const uint8_t kSubCtrlCoast = 2; + static const uint8_t kSubCtrlNote = 3; + static const uint8_t kSubVolumeMax = 4; + static const uint8_t kSubHz = 5; + static const uint8_t kSubVolume = 6; + static const uint8_t kSubDuration = 7; }; #endif // BUZZER_CONTROL_CLIENT_H diff --git a/inc/byte_queue.h b/inc/byte_queue.h index 30e6946..2742917 100644 --- a/inc/byte_queue.h +++ b/inc/byte_queue.h @@ -13,11 +13,11 @@ Contributors: Matthew Piccoli, Raphael Van Hoffelen */ -/* A FIFO buffer for uint8 bytes implemented with a fixed size circular - * buffer. A ByteQueue struct maintains the buffer data and state for one - * buffer instance, and many simultaneous instances are supported. An operation - * on one buffer must not be interrupted by another operation on that same - * buffer (possibly via an interrupt). This restriction is not enforced and no +/* A FIFO buffer for uint8 bytes implemented with a fixed size circular + * buffer. A ByteQueue struct maintains the buffer data and state for one + * buffer instance, and many simultaneous instances are supported. An operation + * on one buffer must not be interrupted by another operation on that same + * buffer (possibly via an interrupt). This restriction is not enforced and no * error code is generated. */ @@ -32,7 +32,7 @@ extern "C" { /// ByteQueue instance state struct, voluntarily opaque. struct ByteQueue { - uint8_t* data; // pointer to array allocated for data + uint8_t *data; // pointer to array allocated for data uint8_t *start; // points to first data byte uint8_t *end; // points to empty byte past last data byte uint16_t data_size; // size allocated for data @@ -41,7 +41,7 @@ struct ByteQueue { // initialize buffer as empty // must provide pointer to allocated array for buffer to use -void InitBQ(struct ByteQueue *b, uint8_t* data, uint16_t data_size); +void InitBQ(struct ByteQueue *b, uint8_t *data, uint16_t data_size); // return 1 if buffer full, 0 else int8_t IsFullBQ(struct ByteQueue *b); diff --git a/inc/client_communication.cpp b/inc/client_communication.cpp index c24410a..6ee9df7 100644 --- a/inc/client_communication.cpp +++ b/inc/client_communication.cpp @@ -15,56 +15,52 @@ #include "client_communication.hpp" -int8_t ParseMsg(uint8_t* rx_data, uint8_t rx_length, - ClientEntryAbstract** entry_array, uint8_t entry_length) +int8_t ParseMsg(uint8_t *rx_data, uint8_t rx_length, + ClientEntryAbstract **entry_array, uint8_t entry_length) { - uint8_t type_idn = rx_data[0]; - uint8_t sub_idn = rx_data[1]; - uint8_t obj_idn = rx_data[2] >> 2; // high 6 bits are obj_idn - Access dir = static_cast(rx_data[2] & 0b00000011); // low two bits + uint8_t type_idn = rx_data[0]; + uint8_t sub_idn = rx_data[1]; + uint8_t obj_idn = rx_data[2] >> 2; // high 6 bits are obj_idn + Access dir = static_cast(rx_data[2] & 0b00000011); // low two bits - // if we have a reply (we only parse replies here) - if(dir == kReply) - { - // if sub_idn is within array range (safe to access array at this location) - if(sub_idn < entry_length) - { - // if there is a ClientEntry object at this sub_idn - if(entry_array[sub_idn] != nullptr) - { - // if the type and obj identifiers match - if(entry_array[sub_idn]->type_idn_ == type_idn && - entry_array[sub_idn]->obj_idn_ == obj_idn) - { - // ... then we have a valid message - entry_array[sub_idn]->Reply(&rx_data[3],rx_length-3); - return 1; // I parsed something - } - } - } - } - return 0; // I didn't parse anything + // if we have a reply (we only parse replies here) + if (dir == kReply) { + // if sub_idn is within array range (safe to access array at this location) + if (sub_idn < entry_length) { + // if there is a ClientEntry object at this sub_idn + if (entry_array[sub_idn] != nullptr) { + // if the type and obj identifiers match + if (entry_array[sub_idn]->type_idn_ == type_idn && + entry_array[sub_idn]->obj_idn_ == obj_idn) { + // ... then we have a valid message + entry_array[sub_idn]->Reply(&rx_data[3], rx_length - 3); + return 1; // I parsed something + } + } + } + } + + return 0; // I didn't parse anything } -int8_t ParseMsg(uint8_t* rx_data, uint8_t rx_length, - ClientEntryAbstract& entry) +int8_t ParseMsg(uint8_t *rx_data, uint8_t rx_length, + ClientEntryAbstract &entry) { - uint8_t type_idn = rx_data[0]; - uint8_t sub_idn = rx_data[1]; - uint8_t obj_idn = rx_data[2] >> 2; // high 6 bits are obj_idn - Access dir = static_cast(rx_data[2] & 0b00000011); // low two bits + uint8_t type_idn = rx_data[0]; + uint8_t sub_idn = rx_data[1]; + uint8_t obj_idn = rx_data[2] >> 2; // high 6 bits are obj_idn + Access dir = static_cast(rx_data[2] & 0b00000011); // low two bits + + // if we have a reply (we only parse replies here) + if (dir == kReply) { + // if the type and obj identifiers match + if (entry.type_idn_ == type_idn && + entry.obj_idn_ == obj_idn && entry.sub_idn_ == sub_idn) { + // ... then we have a valid message + entry.Reply(&rx_data[3], rx_length - 3); + return 1; // I parsed something + } + } - // if we have a reply (we only parse replies here) - if(dir == kReply) - { - // if the type and obj identifiers match - if(entry.type_idn_ == type_idn && - entry.obj_idn_ == obj_idn && entry.sub_idn_ == sub_idn) - { - // ... then we have a valid message - entry.Reply(&rx_data[3],rx_length-3); - return 1; // I parsed something - } - } - return 0; // I didn't parse anything + return 0; // I didn't parse anything } \ No newline at end of file diff --git a/inc/client_communication.hpp b/inc/client_communication.hpp index 4914deb..bd595ed 100644 --- a/inc/client_communication.hpp +++ b/inc/client_communication.hpp @@ -19,178 +19,197 @@ #include // for memcpy #include "communication_interface.h" -enum Access {kGet=0, kSet=1, kSave=2, kReply=3}; +enum Access {kGet = 0, kSet = 1, kSave = 2, kReply = 3}; -class ClientEntryAbstract { - public: - ClientEntryAbstract(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): - type_idn_(type_idn), - obj_idn_(obj_idn), - sub_idn_(sub_idn) {}; +class ClientEntryAbstract +{ +public: + ClientEntryAbstract(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): + type_idn_(type_idn), + obj_idn_(obj_idn), + sub_idn_(sub_idn) {}; - virtual ~ClientEntryAbstract(){}; + virtual ~ClientEntryAbstract() {}; - virtual void Reply(const uint8_t* data, uint8_t len) = 0; + virtual void Reply(const uint8_t *data, uint8_t len) = 0; - const uint8_t type_idn_; - const uint8_t obj_idn_; - const uint8_t sub_idn_; + const uint8_t type_idn_; + const uint8_t obj_idn_; + const uint8_t sub_idn_; }; -class ClientEntryVoid: public ClientEntryAbstract { - public: - ClientEntryVoid(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): - ClientEntryAbstract(type_idn, obj_idn, sub_idn), - is_fresh_(false) - {}; - - void get(CommunicationInterface &com) { - uint8_t tx_msg[2]; - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kGet; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - }; - - void set(CommunicationInterface &com) { - uint8_t tx_msg[2]; // must fit outgoing message - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kSet; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - } - - void save(CommunicationInterface &com) { - uint8_t tx_msg[2]; - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kSave; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - } - - void Reply(const uint8_t* data, uint8_t len) { - (void)data; - if(len == 0) { - is_fresh_ = true; - } - }; - - bool IsFresh() {return is_fresh_;}; - - private: - bool is_fresh_; +class ClientEntryVoid: public ClientEntryAbstract +{ +public: + ClientEntryVoid(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): + ClientEntryAbstract(type_idn, obj_idn, sub_idn), + is_fresh_(false) + {}; + + void get(CommunicationInterface &com) + { + uint8_t tx_msg[2]; + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kGet; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + }; + + void set(CommunicationInterface &com) + { + uint8_t tx_msg[2]; // must fit outgoing message + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kSet; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + } + + void save(CommunicationInterface &com) + { + uint8_t tx_msg[2]; + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kSave; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + } + + void Reply(const uint8_t *data, uint8_t len) + { + (void)data; + + if (len == 0) { + is_fresh_ = true; + } + }; + + bool IsFresh() {return is_fresh_;}; + +private: + bool is_fresh_; }; template -class ClientEntry: public ClientEntryAbstract { - public: - ClientEntry(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): - ClientEntryAbstract(type_idn, obj_idn, sub_idn), - is_fresh_(false), - value_() - {}; - - void get(CommunicationInterface &com) { - uint8_t tx_msg[2]; - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kGet; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - }; - - void set(CommunicationInterface &com, T value) { - uint8_t tx_msg[2+sizeof(T)]; // must fit outgoing message - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kSet; // high six | low two - memcpy(&tx_msg[2], &value, sizeof(T)); - com.SendPacket(type_idn_, tx_msg, 2+sizeof(T)); - } - - void save(CommunicationInterface &com) { - uint8_t tx_msg[2]; - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kSave; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - } - - void Reply(const uint8_t* data, uint8_t len) { - if(len == sizeof(T)) { - memcpy(&value_, data, sizeof(T)); - is_fresh_ = true; - } - }; - - T get_reply() { - is_fresh_ = false; - return value_; - }; - - bool IsFresh() {return is_fresh_;}; - - private: - bool is_fresh_; - T value_; +class ClientEntry: public ClientEntryAbstract +{ +public: + ClientEntry(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn): + ClientEntryAbstract(type_idn, obj_idn, sub_idn), + is_fresh_(false), + value_() + {}; + + void get(CommunicationInterface &com) + { + uint8_t tx_msg[2]; + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kGet; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + }; + + void set(CommunicationInterface &com, T value) + { + uint8_t tx_msg[2 + sizeof(T)]; // must fit outgoing message + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kSet; // high six | low two + memcpy(&tx_msg[2], &value, sizeof(T)); + com.SendPacket(type_idn_, tx_msg, 2 + sizeof(T)); + } + + void save(CommunicationInterface &com) + { + uint8_t tx_msg[2]; + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kSave; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + } + + void Reply(const uint8_t *data, uint8_t len) + { + if (len == sizeof(T)) { + memcpy(&value_, data, sizeof(T)); + is_fresh_ = true; + } + }; + + T get_reply() + { + is_fresh_ = false; + return value_; + }; + + bool IsFresh() {return is_fresh_;}; + +private: + bool is_fresh_; + T value_; }; -class PackedClientEntry : public ClientEntryAbstract { - public: - PackedClientEntry(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn, uint8_t * data_buf): - ClientEntryAbstract(type_idn, obj_idn, sub_idn), - data_buf_(data_buf) - {}; - - //Take in an array of bytes and a length, and ship the bytes out as an IQUART packet - void set(CommunicationInterface &com, uint8_t * buf, uint8_t data_length) { - uint8_t tx_msg[2+data_length]; // must fit outgoing message - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kSet; // high six | low two - memcpy(&tx_msg[2], buf, data_length); - com.SendPacket(type_idn_, tx_msg, 2+data_length); - } - - void get(CommunicationInterface &com) { - uint8_t tx_msg[2]; - tx_msg[0] = sub_idn_; - tx_msg[1] = (obj_idn_<<2) | kGet; // high six | low two - com.SendPacket(type_idn_, tx_msg, 2); - }; - - void Reply(const uint8_t* data, uint8_t len) { - if(data_buf_ != nullptr){ - memcpy(data_buf_, data, len); - is_fresh_ = true; - } - }; - - void get_reply(uint8_t * output_buf, uint8_t output_len) { - if(data_buf_ != nullptr){ - is_fresh_ = false; - memcpy(output_buf, data_buf_, output_len); - } - }; - - bool IsFresh() {return is_fresh_;}; - - private: - bool is_fresh_; - uint8_t * data_buf_; +class PackedClientEntry : public ClientEntryAbstract +{ +public: + PackedClientEntry(uint8_t type_idn, uint8_t obj_idn, uint8_t sub_idn, uint8_t *data_buf): + ClientEntryAbstract(type_idn, obj_idn, sub_idn), + data_buf_(data_buf) + {}; + + //Take in an array of bytes and a length, and ship the bytes out as an IQUART packet + void set(CommunicationInterface &com, uint8_t *buf, uint8_t data_length) + { + uint8_t tx_msg[2 + data_length]; // must fit outgoing message + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kSet; // high six | low two + memcpy(&tx_msg[2], buf, data_length); + com.SendPacket(type_idn_, tx_msg, 2 + data_length); + } + + void get(CommunicationInterface &com) + { + uint8_t tx_msg[2]; + tx_msg[0] = sub_idn_; + tx_msg[1] = (obj_idn_ << 2) | kGet; // high six | low two + com.SendPacket(type_idn_, tx_msg, 2); + }; + + void Reply(const uint8_t *data, uint8_t len) + { + if (data_buf_ != nullptr) { + memcpy(data_buf_, data, len); + is_fresh_ = true; + } + }; + + void get_reply(uint8_t *output_buf, uint8_t output_len) + { + if (data_buf_ != nullptr) { + is_fresh_ = false; + memcpy(output_buf, data_buf_, output_len); + } + }; + + bool IsFresh() {return is_fresh_;}; + +private: + bool is_fresh_; + uint8_t *data_buf_; }; -class ClientAbstract{ - public: - ClientAbstract(uint8_t type_idn, uint8_t obj_idn): - type_idn_(type_idn), - obj_idn_(obj_idn) {}; +class ClientAbstract +{ +public: + ClientAbstract(uint8_t type_idn, uint8_t obj_idn): + type_idn_(type_idn), + obj_idn_(obj_idn) {}; - virtual ~ClientAbstract(){}; + virtual ~ClientAbstract() {}; - virtual void ReadMsg(uint8_t* rx_data, uint8_t rx_length) = 0; + virtual void ReadMsg(uint8_t *rx_data, uint8_t rx_length) = 0; - const uint8_t type_idn_; - const uint8_t obj_idn_; + const uint8_t type_idn_; + const uint8_t obj_idn_; }; -int8_t ParseMsg(uint8_t* rx_data, uint8_t rx_length, - ClientEntryAbstract** entry_array, uint8_t entry_length); +int8_t ParseMsg(uint8_t *rx_data, uint8_t rx_length, + ClientEntryAbstract **entry_array, uint8_t entry_length); -int8_t ParseMsg(uint8_t* rx_data, uint8_t rx_length, - ClientEntryAbstract& entry); +int8_t ParseMsg(uint8_t *rx_data, uint8_t rx_length, + ClientEntryAbstract &entry); #endif // CLIENT_COMMUNICATION_H diff --git a/inc/coil_temperature_estimator_client.hpp b/inc/coil_temperature_estimator_client.hpp index 8ba745f..d5f6e1c 100644 --- a/inc/coil_temperature_estimator_client.hpp +++ b/inc/coil_temperature_estimator_client.hpp @@ -21,101 +21,103 @@ const uint8_t kTypeCoilTemperatureEstimator = 83; -class CoilTemperatureEstimatorClient : public ClientAbstract { - public: - CoilTemperatureEstimatorClient(uint8_t obj_idn) - : ClientAbstract(kTypeCoilTemperatureEstimator, obj_idn), - t_coil_(kTypeCoilTemperatureEstimator, obj_idn, kSubTCoil), - t_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubTAlu), - t_amb_(kTypeCoilTemperatureEstimator, obj_idn, kSubTAmb), - h_coil_amb_free_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbFreeConv), - h_coil_stator_cond_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilStatorCond), - h_coil_amb_forced_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbForcedConv), - c_coil_(kTypeCoilTemperatureEstimator, obj_idn, kSubCCoil), - h_coil_amb_forced_conv_coeff_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbForcedConvCoeff), - otw_(kTypeCoilTemperatureEstimator, obj_idn, kSubOtw), - otlo_(kTypeCoilTemperatureEstimator, obj_idn, kSubOtlo), - derate_(kTypeCoilTemperatureEstimator, obj_idn, kSubDerate), - q_coil_joule_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilJoule), - q_coil_amb_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilAmbConv), - q_coil_stator_cond_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilStatorCond), - h_lam_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubHLamAlu), - c_lam_(kTypeCoilTemperatureEstimator, obj_idn, kSubCLam), - k_lam_hist_coeff_(kTypeCoilTemperatureEstimator, obj_idn, kSubKLamHistCoeff), - q_lam_hist_(kTypeCoilTemperatureEstimator, obj_idn, kSubQLamHist), - q_lam_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubQLamAlu), - t_lam_(kTypeCoilTemperatureEstimator, obj_idn, kSubTLam){}; +class CoilTemperatureEstimatorClient : public ClientAbstract +{ +public: + CoilTemperatureEstimatorClient(uint8_t obj_idn) + : ClientAbstract(kTypeCoilTemperatureEstimator, obj_idn), + t_coil_(kTypeCoilTemperatureEstimator, obj_idn, kSubTCoil), + t_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubTAlu), + t_amb_(kTypeCoilTemperatureEstimator, obj_idn, kSubTAmb), + h_coil_amb_free_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbFreeConv), + h_coil_stator_cond_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilStatorCond), + h_coil_amb_forced_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbForcedConv), + c_coil_(kTypeCoilTemperatureEstimator, obj_idn, kSubCCoil), + h_coil_amb_forced_conv_coeff_(kTypeCoilTemperatureEstimator, obj_idn, kSubHCoilAmbForcedConvCoeff), + otw_(kTypeCoilTemperatureEstimator, obj_idn, kSubOtw), + otlo_(kTypeCoilTemperatureEstimator, obj_idn, kSubOtlo), + derate_(kTypeCoilTemperatureEstimator, obj_idn, kSubDerate), + q_coil_joule_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilJoule), + q_coil_amb_conv_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilAmbConv), + q_coil_stator_cond_(kTypeCoilTemperatureEstimator, obj_idn, kSubQCoilStatorCond), + h_lam_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubHLamAlu), + c_lam_(kTypeCoilTemperatureEstimator, obj_idn, kSubCLam), + k_lam_hist_coeff_(kTypeCoilTemperatureEstimator, obj_idn, kSubKLamHistCoeff), + q_lam_hist_(kTypeCoilTemperatureEstimator, obj_idn, kSubQLamHist), + q_lam_alu_(kTypeCoilTemperatureEstimator, obj_idn, kSubQLamAlu), + t_lam_(kTypeCoilTemperatureEstimator, obj_idn, kSubTLam) {}; - // Client Entries - ClientEntry t_coil_; - ClientEntry t_alu_; - ClientEntry t_amb_; - ClientEntry h_coil_amb_free_conv_; - ClientEntry h_coil_stator_cond_; - ClientEntry h_coil_amb_forced_conv_; - ClientEntry c_coil_; - ClientEntry h_coil_amb_forced_conv_coeff_; - ClientEntry otw_; - ClientEntry otlo_; - ClientEntry derate_; - ClientEntry q_coil_joule_; - ClientEntry q_coil_amb_conv_; - ClientEntry q_coil_stator_cond_; - ClientEntry h_lam_alu_; - ClientEntry c_lam_; - ClientEntry k_lam_hist_coeff_; - ClientEntry q_lam_hist_; - ClientEntry q_lam_alu_; - ClientEntry t_lam_; + // Client Entries + ClientEntry t_coil_; + ClientEntry t_alu_; + ClientEntry t_amb_; + ClientEntry h_coil_amb_free_conv_; + ClientEntry h_coil_stator_cond_; + ClientEntry h_coil_amb_forced_conv_; + ClientEntry c_coil_; + ClientEntry h_coil_amb_forced_conv_coeff_; + ClientEntry otw_; + ClientEntry otlo_; + ClientEntry derate_; + ClientEntry q_coil_joule_; + ClientEntry q_coil_amb_conv_; + ClientEntry q_coil_stator_cond_; + ClientEntry h_lam_alu_; + ClientEntry c_lam_; + ClientEntry k_lam_hist_coeff_; + ClientEntry q_lam_hist_; + ClientEntry q_lam_alu_; + ClientEntry t_lam_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubTLam + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &t_coil_, // 0 - &t_alu_, // 1 - &t_amb_, // 2 - &h_coil_amb_free_conv_, // 3 - &h_coil_stator_cond_, // 4 - &h_coil_amb_forced_conv_, // 5 - &c_coil_, // 6 - &h_coil_amb_forced_conv_coeff_, // 7 - &otw_, // 8 - &otlo_, // 9 - &derate_, // 10 - &q_coil_joule_, // 11 - &q_coil_amb_conv_, // 12 - &q_coil_stator_cond_, // 13 - &h_lam_alu_, // 14 - &c_lam_, // 15 - &k_lam_hist_coeff_, // 16 - &q_lam_hist_, // 17 - &q_lam_alu_, // 18 - &t_lam_ // 19 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubTLam + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &t_coil_, // 0 + &t_alu_, // 1 + &t_amb_, // 2 + &h_coil_amb_free_conv_, // 3 + &h_coil_stator_cond_, // 4 + &h_coil_amb_forced_conv_, // 5 + &c_coil_, // 6 + &h_coil_amb_forced_conv_coeff_, // 7 + &otw_, // 8 + &otlo_, // 9 + &derate_, // 10 + &q_coil_joule_, // 11 + &q_coil_amb_conv_, // 12 + &q_coil_stator_cond_, // 13 + &h_lam_alu_, // 14 + &c_lam_, // 15 + &k_lam_hist_coeff_, // 16 + &q_lam_hist_, // 17 + &q_lam_alu_, // 18 + &t_lam_ // 19 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubTCoil = 0; - static const uint8_t kSubTAlu = 1; - static const uint8_t kSubTAmb = 2; - static const uint8_t kSubHCoilAmbFreeConv = 3; - static const uint8_t kSubHCoilStatorCond = 4; - static const uint8_t kSubHCoilAmbForcedConv = 5; - static const uint8_t kSubCCoil = 6; - static const uint8_t kSubHCoilAmbForcedConvCoeff = 7; - static const uint8_t kSubOtw = 8; - static const uint8_t kSubOtlo = 9; - static const uint8_t kSubDerate = 10; - static const uint8_t kSubQCoilJoule = 11; - static const uint8_t kSubQCoilAmbConv = 12; - static const uint8_t kSubQCoilStatorCond = 13; - static const uint8_t kSubHLamAlu = 14; - static const uint8_t kSubCLam = 15; - static const uint8_t kSubKLamHistCoeff = 16; - static const uint8_t kSubQLamHist = 17; - static const uint8_t kSubQLamAlu = 18; - static const uint8_t kSubTLam = 19; +private: + static const uint8_t kSubTCoil = 0; + static const uint8_t kSubTAlu = 1; + static const uint8_t kSubTAmb = 2; + static const uint8_t kSubHCoilAmbFreeConv = 3; + static const uint8_t kSubHCoilStatorCond = 4; + static const uint8_t kSubHCoilAmbForcedConv = 5; + static const uint8_t kSubCCoil = 6; + static const uint8_t kSubHCoilAmbForcedConvCoeff = 7; + static const uint8_t kSubOtw = 8; + static const uint8_t kSubOtlo = 9; + static const uint8_t kSubDerate = 10; + static const uint8_t kSubQCoilJoule = 11; + static const uint8_t kSubQCoilAmbConv = 12; + static const uint8_t kSubQCoilStatorCond = 13; + static const uint8_t kSubHLamAlu = 14; + static const uint8_t kSubCLam = 15; + static const uint8_t kSubKLamHistCoeff = 16; + static const uint8_t kSubQLamHist = 17; + static const uint8_t kSubQLamAlu = 18; + static const uint8_t kSubTLam = 19; }; #endif /* COIL_TEMPERATURE_ESTIMATOR_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/communication_interface.h b/inc/communication_interface.h index df17660..5018cde 100644 --- a/inc/communication_interface.h +++ b/inc/communication_interface.h @@ -18,66 +18,67 @@ #include -class CommunicationInterface{ - private: - - public: - /******************************************************************************* - * Receive - ******************************************************************************/ - - /// Poll the hardware for new byte data. - /// Returns: 1 packet ready - /// 0 normal operation - /// -1 failure - /// - virtual int8_t GetBytes() = 0; - - /// Peek at the next available incoming packet. If a packet is ready, pointer - /// 'packet' will point to the first byte of type+data and 'length' will give - /// the length of packet type+data. Arguments 'packet' and 'length' are ignored - /// if no packet is ready. Repeated calls to Peek will return pointers to the - /// same packet data until Drop is used. - /// Returns: 1 packet peek available - /// 0 no packet peek available - /// -1 failure - /// - virtual int8_t PeekPacket(uint8_t **packet, uint8_t *length) = 0; - - /// Drop the next available packet from queue. Usually called after Peek. - /// Returns: 1 packet removed - /// 0 no packet ready to remove - /// -1 failure - /// - virtual int8_t DropPacket() = 0; - - - /******************************************************************************* - * Send - ******************************************************************************/ - - /// Add a packet to the outgoing USB queue with automatically generated header - /// and CRC. A hardware transmission is not immediately initiated unless the - /// endpoint is filled. To force a transmission, follow with SendNow(). This - /// operation is nonblocking. If the buffer fills, the most recent data is lost. - virtual int8_t SendPacket(uint8_t msg_type, uint8_t *data, uint16_t length) = 0; - - /// Add bytes to the outgoing USB queue. A hardware transmission is not - /// immediately initiated unless the endpoint is filled. To force a - /// transmission, follow with SendUsbNow(). This operation is - /// nonblocking. If the buffer fills, the most recent data is lost. - virtual int8_t SendBytes(uint8_t *bytes, uint16_t length) = 0; - - /// Initiate a hardware transmission, which will chain transmissions through - /// the endpoint IN interrupt until the buffer empties completely. - virtual void SendNow() = 0; - - /******************************************************************************* - * Parsing - ******************************************************************************/ - - /// Read a given message and act appropriately. - virtual void ReadMsg(CommunicationInterface& com, uint8_t* data, uint8_t length) = 0; +class CommunicationInterface +{ +private: + +public: + /******************************************************************************* + * Receive + ******************************************************************************/ + + /// Poll the hardware for new byte data. + /// Returns: 1 packet ready + /// 0 normal operation + /// -1 failure + /// + virtual int8_t GetBytes() = 0; + + /// Peek at the next available incoming packet. If a packet is ready, pointer + /// 'packet' will point to the first byte of type+data and 'length' will give + /// the length of packet type+data. Arguments 'packet' and 'length' are ignored + /// if no packet is ready. Repeated calls to Peek will return pointers to the + /// same packet data until Drop is used. + /// Returns: 1 packet peek available + /// 0 no packet peek available + /// -1 failure + /// + virtual int8_t PeekPacket(uint8_t **packet, uint8_t *length) = 0; + + /// Drop the next available packet from queue. Usually called after Peek. + /// Returns: 1 packet removed + /// 0 no packet ready to remove + /// -1 failure + /// + virtual int8_t DropPacket() = 0; + + + /******************************************************************************* + * Send + ******************************************************************************/ + + /// Add a packet to the outgoing USB queue with automatically generated header + /// and CRC. A hardware transmission is not immediately initiated unless the + /// endpoint is filled. To force a transmission, follow with SendNow(). This + /// operation is nonblocking. If the buffer fills, the most recent data is lost. + virtual int8_t SendPacket(uint8_t msg_type, uint8_t *data, uint16_t length) = 0; + + /// Add bytes to the outgoing USB queue. A hardware transmission is not + /// immediately initiated unless the endpoint is filled. To force a + /// transmission, follow with SendUsbNow(). This operation is + /// nonblocking. If the buffer fills, the most recent data is lost. + virtual int8_t SendBytes(uint8_t *bytes, uint16_t length) = 0; + + /// Initiate a hardware transmission, which will chain transmissions through + /// the endpoint IN interrupt until the buffer empties completely. + virtual void SendNow() = 0; + + /******************************************************************************* + * Parsing + ******************************************************************************/ + + /// Read a given message and act appropriately. + virtual void ReadMsg(CommunicationInterface &com, uint8_t *data, uint8_t length) = 0; }; // end class CommunicationInterface #endif // COMMUNICATION_INTERFACE_H \ No newline at end of file diff --git a/inc/esc_propeller_input_parser_client.hpp b/inc/esc_propeller_input_parser_client.hpp index 4d03378..a4b628b 100644 --- a/inc/esc_propeller_input_parser_client.hpp +++ b/inc/esc_propeller_input_parser_client.hpp @@ -20,64 +20,66 @@ const uint8_t kTypeEscPropellerInputParser = 60; -class EscPropellerInputParserClient : public ClientAbstract { - public: - EscPropellerInputParserClient(uint8_t obj_idn) - : ClientAbstract(kTypeEscPropellerInputParser, obj_idn), - mode_(kTypeEscPropellerInputParser, obj_idn, kSubMode), - raw_value_(kTypeEscPropellerInputParser, obj_idn, kSubRawValue), - sign_(kTypeEscPropellerInputParser, obj_idn, kSubSign), - volts_max_(kTypeEscPropellerInputParser, obj_idn, kSubVoltsMax), - velocity_max_(kTypeEscPropellerInputParser, obj_idn, kSubVelocityMax), - thrust_max_(kTypeEscPropellerInputParser, obj_idn, kSubThrustMax), - safe_factor_(kTypeEscPropellerInputParser, obj_idn, kSubSafeFactor), - flip_negative_(kTypeEscPropellerInputParser, obj_idn, kSubFlipNegative), - zero_spin_throttle_(kTypeEscPropellerInputParser, obj_idn, kSubZeroSpinThrottle), - zero_spin_tolerance_(kTypeEscPropellerInputParser, obj_idn, kSubZeroSpinTolerance){}; +class EscPropellerInputParserClient : public ClientAbstract +{ +public: + EscPropellerInputParserClient(uint8_t obj_idn) + : ClientAbstract(kTypeEscPropellerInputParser, obj_idn), + mode_(kTypeEscPropellerInputParser, obj_idn, kSubMode), + raw_value_(kTypeEscPropellerInputParser, obj_idn, kSubRawValue), + sign_(kTypeEscPropellerInputParser, obj_idn, kSubSign), + volts_max_(kTypeEscPropellerInputParser, obj_idn, kSubVoltsMax), + velocity_max_(kTypeEscPropellerInputParser, obj_idn, kSubVelocityMax), + thrust_max_(kTypeEscPropellerInputParser, obj_idn, kSubThrustMax), + safe_factor_(kTypeEscPropellerInputParser, obj_idn, kSubSafeFactor), + flip_negative_(kTypeEscPropellerInputParser, obj_idn, kSubFlipNegative), + zero_spin_throttle_(kTypeEscPropellerInputParser, obj_idn, kSubZeroSpinThrottle), + zero_spin_tolerance_(kTypeEscPropellerInputParser, obj_idn, kSubZeroSpinTolerance) {}; - // Client Entries - // Control commands - ClientEntry mode_; - ClientEntry raw_value_; - ClientEntry sign_; - ClientEntry volts_max_; - ClientEntry velocity_max_; - ClientEntry thrust_max_; - ClientEntry safe_factor_; - ClientEntry flip_negative_; - ClientEntry zero_spin_throttle_; - ClientEntry zero_spin_tolerance_; + // Client Entries + // Control commands + ClientEntry mode_; + ClientEntry raw_value_; + ClientEntry sign_; + ClientEntry volts_max_; + ClientEntry velocity_max_; + ClientEntry thrust_max_; + ClientEntry safe_factor_; + ClientEntry flip_negative_; + ClientEntry zero_spin_throttle_; + ClientEntry zero_spin_tolerance_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubZeroSpinTolerance + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &mode_, // 0 - &raw_value_, // 1 - nullptr, // 2 - &sign_, // 3 - &volts_max_, // 4 - &velocity_max_, // 5 - &thrust_max_, // 6 - &safe_factor_, // 7 - &flip_negative_, // 8 - &zero_spin_throttle_, // 9 - &zero_spin_tolerance_ // 10 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubZeroSpinTolerance + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &mode_, // 0 + &raw_value_, // 1 + nullptr, // 2 + &sign_, // 3 + &volts_max_, // 4 + &velocity_max_, // 5 + &thrust_max_, // 6 + &safe_factor_, // 7 + &flip_negative_, // 8 + &zero_spin_throttle_, // 9 + &zero_spin_tolerance_ // 10 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubMode = 0; - static const uint8_t kSubRawValue = 1; - static const uint8_t kSubSign = 3; - static const uint8_t kSubVoltsMax = 4; - static const uint8_t kSubVelocityMax = 5; - static const uint8_t kSubThrustMax = 6; - static const uint8_t kSubSafeFactor = 7; - static const uint8_t kSubFlipNegative = 8; - static const uint8_t kSubZeroSpinThrottle = 9; - static const uint8_t kSubZeroSpinTolerance = 10; +private: + static const uint8_t kSubMode = 0; + static const uint8_t kSubRawValue = 1; + static const uint8_t kSubSign = 3; + static const uint8_t kSubVoltsMax = 4; + static const uint8_t kSubVelocityMax = 5; + static const uint8_t kSubThrustMax = 6; + static const uint8_t kSubSafeFactor = 7; + static const uint8_t kSubFlipNegative = 8; + static const uint8_t kSubZeroSpinThrottle = 9; + static const uint8_t kSubZeroSpinTolerance = 10; }; #endif /* ESC_PROPELLER_INPUT_PARSER_CLIENT_HPP_ */ diff --git a/inc/generic_interface.hpp b/inc/generic_interface.hpp index 663e94c..172f148 100644 --- a/inc/generic_interface.hpp +++ b/inc/generic_interface.hpp @@ -24,82 +24,83 @@ #define GENERIC_PF_INDEX_DATA_SIZE 20 // size of index buffer in packet_finder #ifndef GENERIC_TX_BUFFER_SIZE - #define GENERIC_TX_BUFFER_SIZE 64 +#define GENERIC_TX_BUFFER_SIZE 64 #endif -class GenericInterface: public CommunicationInterface{ - private: - - public: - // Member Variables - struct PacketFinder pf; // packet_finder instance - struct ByteQueue index_queue; // needed by pf for storing indices - uint8_t pf_index_data[GENERIC_PF_INDEX_DATA_SIZE]; // data for index_queue used by pf - BipBuffer tx_bipbuf; // bipbuffer for transmissions - uint8_t tx_buffer[GENERIC_TX_BUFFER_SIZE]; // raw buffer for transmissions - - - // Default Constructor - GenericInterface(); - - /******************************************************************************* - * Receive - ******************************************************************************/ - - /// Poll the hardware for new byte data. - /// Returns: 1 packet ready - /// 0 normal operation - /// -1 failure - /// - int8_t GetBytes(); - int8_t SetRxBytes(uint8_t* data_in, uint16_t length_in); - - /// Peek at the next available incoming packet. If a packet is ready, pointer - /// 'packet' will point to the first byte of type+data and 'length' will give - /// the length of packet type+data. Arguments 'packet' and 'length' are ignored - /// if no packet is ready. Repeated calls to Peek will return pointers to the - /// same packet data until Drop is used. - /// Returns: 1 packet peek available - /// 0 no packet peek available - /// -1 failure - /// - int8_t PeekPacket(uint8_t **packet, uint8_t *length); - - /// Drop the next available packet from queue. Usually called after Peek. - /// Returns: 1 packet removed - /// 0 no packet ready to remove - /// -1 failure - /// - int8_t DropPacket(); - - /******************************************************************************* - * Send - ******************************************************************************/ - - /// Add a packet to the outgoing queue with automatically generated header - /// and CRC. If the buffer fills, the most recent data is lost. - int8_t SendPacket(uint8_t msg_type, uint8_t *data, uint16_t length); - - /// Add bytes to the outgoing queue. - /// If the buffer fills, the most recent data is lost. - int8_t SendBytes(uint8_t *bytes, uint16_t length); - - /// Does nothing in this interface - void SendNow(); - - /// Gets all outbound bytes - /// The data is copied into the user supplied data_out buffer. - /// The length of data transferred is copied into length_out. - /// Returns: 1 for data transferred - /// 0 for no data transferred (buffer empty) - int8_t GetTxBytes(uint8_t* data_out, uint8_t& length_out); - - /******************************************************************************* - * Parsing - ******************************************************************************/ - - /// Read a given message and act appropriately. - void ReadMsg(CommunicationInterface& com, uint8_t* data, uint8_t length); +class GenericInterface: public CommunicationInterface +{ +private: + +public: + // Member Variables + struct PacketFinder pf; // packet_finder instance + struct ByteQueue index_queue; // needed by pf for storing indices + uint8_t pf_index_data[GENERIC_PF_INDEX_DATA_SIZE]; // data for index_queue used by pf + BipBuffer tx_bipbuf; // bipbuffer for transmissions + uint8_t tx_buffer[GENERIC_TX_BUFFER_SIZE]; // raw buffer for transmissions + + + // Default Constructor + GenericInterface(); + + /******************************************************************************* + * Receive + ******************************************************************************/ + + /// Poll the hardware for new byte data. + /// Returns: 1 packet ready + /// 0 normal operation + /// -1 failure + /// + int8_t GetBytes(); + int8_t SetRxBytes(uint8_t *data_in, uint16_t length_in); + + /// Peek at the next available incoming packet. If a packet is ready, pointer + /// 'packet' will point to the first byte of type+data and 'length' will give + /// the length of packet type+data. Arguments 'packet' and 'length' are ignored + /// if no packet is ready. Repeated calls to Peek will return pointers to the + /// same packet data until Drop is used. + /// Returns: 1 packet peek available + /// 0 no packet peek available + /// -1 failure + /// + int8_t PeekPacket(uint8_t **packet, uint8_t *length); + + /// Drop the next available packet from queue. Usually called after Peek. + /// Returns: 1 packet removed + /// 0 no packet ready to remove + /// -1 failure + /// + int8_t DropPacket(); + + /******************************************************************************* + * Send + ******************************************************************************/ + + /// Add a packet to the outgoing queue with automatically generated header + /// and CRC. If the buffer fills, the most recent data is lost. + int8_t SendPacket(uint8_t msg_type, uint8_t *data, uint16_t length); + + /// Add bytes to the outgoing queue. + /// If the buffer fills, the most recent data is lost. + int8_t SendBytes(uint8_t *bytes, uint16_t length); + + /// Does nothing in this interface + void SendNow(); + + /// Gets all outbound bytes + /// The data is copied into the user supplied data_out buffer. + /// The length of data transferred is copied into length_out. + /// Returns: 1 for data transferred + /// 0 for no data transferred (buffer empty) + int8_t GetTxBytes(uint8_t *data_out, uint8_t &length_out); + + /******************************************************************************* + * Parsing + ******************************************************************************/ + + /// Read a given message and act appropriately. + void ReadMsg(CommunicationInterface &com, uint8_t *data, uint8_t length); }; // class GenericInterface #endif // GENERIC_INTERFACE_H \ No newline at end of file diff --git a/inc/gpio_controller_client.hpp b/inc/gpio_controller_client.hpp index 7c45050..5e11e26 100644 --- a/inc/gpio_controller_client.hpp +++ b/inc/gpio_controller_client.hpp @@ -20,65 +20,67 @@ const uint8_t kTypeGpioController = 90; -class GpioControllerClient : public ClientAbstract { - public: - GpioControllerClient(uint8_t obj_idn) - : ClientAbstract(kTypeGpioController, obj_idn), - mode_register_(kTypeGpioController, obj_idn, kSubModeRegister), - inputs_register_(kTypeGpioController, obj_idn, kSubInputsRegister), - outputs_register_(kTypeGpioController, obj_idn, kSubOutputsRegister), - use_pull_register_(kTypeGpioController, obj_idn, kSubUsePullRegister), - pull_type_register_(kTypeGpioController, obj_idn, kSubPullTypeRegister), - push_pull_open_drain_register_(kTypeGpioController, obj_idn, kSubPushPullOpenDrainRegister), - addressable_gpio_mode_(kTypeGpioController, obj_idn, kSubAddressableGpioMode), - addressable_outputs_(kTypeGpioController, obj_idn, kSubAddressableOutputs), - addressable_use_pull_(kTypeGpioController, obj_idn, kSubAddressableUsePull), - addressable_pull_type_(kTypeGpioController, obj_idn, kSubAddressablePullType), - addressable_push_pull_open_drain_(kTypeGpioController, obj_idn, kSubAddressablePushPullOpenDrain){}; +class GpioControllerClient : public ClientAbstract +{ +public: + GpioControllerClient(uint8_t obj_idn) + : ClientAbstract(kTypeGpioController, obj_idn), + mode_register_(kTypeGpioController, obj_idn, kSubModeRegister), + inputs_register_(kTypeGpioController, obj_idn, kSubInputsRegister), + outputs_register_(kTypeGpioController, obj_idn, kSubOutputsRegister), + use_pull_register_(kTypeGpioController, obj_idn, kSubUsePullRegister), + pull_type_register_(kTypeGpioController, obj_idn, kSubPullTypeRegister), + push_pull_open_drain_register_(kTypeGpioController, obj_idn, kSubPushPullOpenDrainRegister), + addressable_gpio_mode_(kTypeGpioController, obj_idn, kSubAddressableGpioMode), + addressable_outputs_(kTypeGpioController, obj_idn, kSubAddressableOutputs), + addressable_use_pull_(kTypeGpioController, obj_idn, kSubAddressableUsePull), + addressable_pull_type_(kTypeGpioController, obj_idn, kSubAddressablePullType), + addressable_push_pull_open_drain_(kTypeGpioController, obj_idn, kSubAddressablePushPullOpenDrain) {}; - // Client Entries - ClientEntry mode_register_; // 0 - ClientEntry inputs_register_; // 1 - ClientEntry outputs_register_; // 2 - ClientEntry use_pull_register_; // 3 - ClientEntry pull_type_register_; // 4 - ClientEntry push_pull_open_drain_register_; // 5 - ClientEntry addressable_gpio_mode_; // 6 - ClientEntry addressable_outputs_; // 7 - ClientEntry addressable_use_pull_; // 8 - ClientEntry addressable_pull_type_; // 9 - ClientEntry addressable_push_pull_open_drain_; // 10 + // Client Entries + ClientEntry mode_register_; // 0 + ClientEntry inputs_register_; // 1 + ClientEntry outputs_register_; // 2 + ClientEntry use_pull_register_; // 3 + ClientEntry pull_type_register_; // 4 + ClientEntry push_pull_open_drain_register_; // 5 + ClientEntry addressable_gpio_mode_; // 6 + ClientEntry addressable_outputs_; // 7 + ClientEntry addressable_use_pull_; // 8 + ClientEntry addressable_pull_type_; // 9 + ClientEntry addressable_push_pull_open_drain_; // 10 - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubAddressablePushPullOpenDrain + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &mode_register_, // 0 - &inputs_register_, // 1 - &outputs_register_, // 2 - &use_pull_register_, // 3 - &pull_type_register_, // 4 - &push_pull_open_drain_register_, // 5 - &addressable_gpio_mode_, // 6 - &addressable_outputs_, // 7 - &addressable_use_pull_, // 8 - &addressable_pull_type_, // 9 - &addressable_push_pull_open_drain_ // 10 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubAddressablePushPullOpenDrain + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &mode_register_, // 0 + &inputs_register_, // 1 + &outputs_register_, // 2 + &use_pull_register_, // 3 + &pull_type_register_, // 4 + &push_pull_open_drain_register_, // 5 + &addressable_gpio_mode_, // 6 + &addressable_outputs_, // 7 + &addressable_use_pull_, // 8 + &addressable_pull_type_, // 9 + &addressable_push_pull_open_drain_ // 10 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubModeRegister = 0; - static const uint8_t kSubInputsRegister = 1; - static const uint8_t kSubOutputsRegister = 2; - static const uint8_t kSubUsePullRegister = 3; - static const uint8_t kSubPullTypeRegister = 4; - static const uint8_t kSubPushPullOpenDrainRegister = 5; - static const uint8_t kSubAddressableGpioMode = 6; - static const uint8_t kSubAddressableOutputs = 7; - static const uint8_t kSubAddressableUsePull = 8; - static const uint8_t kSubAddressablePullType = 9; - static const uint8_t kSubAddressablePushPullOpenDrain = 10; +private: + static const uint8_t kSubModeRegister = 0; + static const uint8_t kSubInputsRegister = 1; + static const uint8_t kSubOutputsRegister = 2; + static const uint8_t kSubUsePullRegister = 3; + static const uint8_t kSubPullTypeRegister = 4; + static const uint8_t kSubPushPullOpenDrainRegister = 5; + static const uint8_t kSubAddressableGpioMode = 6; + static const uint8_t kSubAddressableOutputs = 7; + static const uint8_t kSubAddressableUsePull = 8; + static const uint8_t kSubAddressablePullType = 9; + static const uint8_t kSubAddressablePushPullOpenDrain = 10; }; #endif /* GPIO_CONTROLLER_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/hobby_input_client.hpp b/inc/hobby_input_client.hpp index 2095ad3..bfaa206 100644 --- a/inc/hobby_input_client.hpp +++ b/inc/hobby_input_client.hpp @@ -20,49 +20,50 @@ const uint8_t kTypeHobbyInput = 76; -class HobbyInputClient: public ClientAbstract{ - public: - HobbyInputClient(uint8_t obj_idn): - ClientAbstract( kTypeHobbyInput, obj_idn), - allowed_protocols_( kTypeHobbyInput, obj_idn, kSubAllowedProtocols), - protocol_( kTypeHobbyInput, obj_idn, kSubProtocol), - calibrated_protocol_( kTypeHobbyInput, obj_idn, kSubCalibratedProtocol), - calibrated_high_ticks_us_( kTypeHobbyInput, obj_idn, kSubCalibratedHighTicksUs), - calibrated_low_ticks_us_( kTypeHobbyInput, obj_idn, kSubCalibratedLowTicksUs), - reset_calibration_( kTypeHobbyInput, obj_idn, kSubResetCalibration) - {}; +class HobbyInputClient: public ClientAbstract +{ +public: + HobbyInputClient(uint8_t obj_idn): + ClientAbstract(kTypeHobbyInput, obj_idn), + allowed_protocols_(kTypeHobbyInput, obj_idn, kSubAllowedProtocols), + protocol_(kTypeHobbyInput, obj_idn, kSubProtocol), + calibrated_protocol_(kTypeHobbyInput, obj_idn, kSubCalibratedProtocol), + calibrated_high_ticks_us_(kTypeHobbyInput, obj_idn, kSubCalibratedHighTicksUs), + calibrated_low_ticks_us_(kTypeHobbyInput, obj_idn, kSubCalibratedLowTicksUs), + reset_calibration_(kTypeHobbyInput, obj_idn, kSubResetCalibration) + {}; - // Client Entries - // Control commands - ClientEntry allowed_protocols_; - ClientEntry protocol_; - ClientEntry calibrated_protocol_; - ClientEntry calibrated_high_ticks_us_; - ClientEntry calibrated_low_ticks_us_; - ClientEntryVoid reset_calibration_; + // Client Entries + // Control commands + ClientEntry allowed_protocols_; + ClientEntry protocol_; + ClientEntry calibrated_protocol_; + ClientEntry calibrated_high_ticks_us_; + ClientEntry calibrated_low_ticks_us_; + ClientEntryVoid reset_calibration_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubResetCalibration+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &allowed_protocols_, // 0 - &protocol_, // 1 - &calibrated_protocol_, // 2 - &calibrated_high_ticks_us_, // 3 - &calibrated_low_ticks_us_, // 4 - &reset_calibration_ // 5 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubResetCalibration + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &allowed_protocols_, // 0 + &protocol_, // 1 + &calibrated_protocol_, // 2 + &calibrated_high_ticks_us_, // 3 + &calibrated_low_ticks_us_, // 4 + &reset_calibration_ // 5 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubAllowedProtocols = 0; - static const uint8_t kSubProtocol = 1; - static const uint8_t kSubCalibratedProtocol = 2; - static const uint8_t kSubCalibratedHighTicksUs = 3; - static const uint8_t kSubCalibratedLowTicksUs = 4; - static const uint8_t kSubResetCalibration = 5; +private: + static const uint8_t kSubAllowedProtocols = 0; + static const uint8_t kSubProtocol = 1; + static const uint8_t kSubCalibratedProtocol = 2; + static const uint8_t kSubCalibratedHighTicksUs = 3; + static const uint8_t kSubCalibratedLowTicksUs = 4; + static const uint8_t kSubResetCalibration = 5; }; #endif /* HOBBY_INPUT_CLIENT_HPP_ */ diff --git a/inc/iquart_flight_controller_interface_client.hpp b/inc/iquart_flight_controller_interface_client.hpp index 752f9b2..5564290 100644 --- a/inc/iquart_flight_controller_interface_client.hpp +++ b/inc/iquart_flight_controller_interface_client.hpp @@ -24,84 +24,88 @@ const uint8_t kTypeIQUartFlightControllerInterface = 88; /** * @brief A struct that holds the data from a received telemetry packet - * + * */ -struct __attribute__ ((__packed__)) IFCITelemetryData{ - int16_t mcu_temp; //centi ℃ - int16_t coil_temp; //centi ℃ - int16_t voltage; //cV - int16_t current; //cA - int16_t consumption; //mAh - int16_t speed; //rad/s - uint32_t uptime; //s +struct __attribute__((__packed__)) IFCITelemetryData { + int16_t mcu_temp; //centi ℃ + int16_t coil_temp; //centi ℃ + int16_t voltage; //cV + int16_t current; //cA + int16_t consumption; //mAh + int16_t speed; //rad/s + uint32_t uptime; //s }; /** * @brief A struct that can be used to more easily send an IFCI packed command message - * + * */ -struct __attribute__ ((__packed__)) IFCIPackedMessage{ - uint16_t commands[MAX_CONTROL_VALUES_PER_IFCI]; //An array to hold all control values - uint8_t telem_byte; //The module ID to send back its telemetry - uint8_t num_cvs; //The number of control values being sent in this command +struct __attribute__((__packed__)) IFCIPackedMessage { + uint16_t commands[MAX_CONTROL_VALUES_PER_IFCI]; //An array to hold all control values + uint8_t telem_byte; //The module ID to send back its telemetry + uint8_t num_cvs; //The number of control values being sent in this command }; -class IQUartFlightControllerInterfaceClient : public ClientAbstract { - public: - IQUartFlightControllerInterfaceClient(uint8_t obj_idn, uint8_t * data_buf = nullptr) - : ClientAbstract(kTypeIQUartFlightControllerInterface, obj_idn), - packed_command_(kTypeIQUartFlightControllerInterface, obj_idn, kSubPackedCommand, data_buf), - telemetry_(kTypeIQUartFlightControllerInterface, obj_idn, kSubTelemetry), - throttle_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubThrottleCvi), - x_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubXCvi), - y_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubYCvi){}; - - // Client Entries - PackedClientEntry packed_command_; - ClientEntry telemetry_; - ClientEntry throttle_cvi_; - ClientEntry x_cvi_; - ClientEntry y_cvi_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubYCvi + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &packed_command_, // 0 - &telemetry_, // 1 - &throttle_cvi_, // 2 - &x_cvi_, // 3 - &y_cvi_ // 4 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - /** - * @brief This function takes in an IFCIPackedMessage struct, and prepares the data as a series of bytes ready for transmission over IQUART - * As an example, assume you want to send 4 throttle commands, and get telemetry from Module ID 3. To do so, make an IFCIPackedMessage and set - * commands[0] through commands[3] to throttle values [0, 65535], telem_byte to 3, and num_cvs to 4. Then, create a byte array, and a byte to store the - * message length, and call this function. - * - * @param ifci_commands The IFCIPackedMessage struct that you want to send - * @param output_data A pointer to an array of bytes that will store the data - * @param output_data_length The length of the data stored in the output_data array - */ - void PackageIfciCommandsForTransmission(IFCIPackedMessage * ifci_commands, uint8_t * output_data, uint8_t * output_data_length){ - //Copy the CV bytes - memcpy(output_data, ifci_commands->commands, ifci_commands->num_cvs * 2); - - //Add the telem byte - output_data[ifci_commands->num_cvs * 2] = ifci_commands->telem_byte; - - //Set the output data length - *output_data_length = ifci_commands->num_cvs * 2 + 1; - } - - private: - static const uint8_t kSubPackedCommand = 0; - static const uint8_t kSubTelemetry = 1; - static const uint8_t kSubThrottleCvi = 2; - static const uint8_t kSubXCvi = 3; - static const uint8_t kSubYCvi = 4; +class IQUartFlightControllerInterfaceClient : public ClientAbstract +{ +public: + IQUartFlightControllerInterfaceClient(uint8_t obj_idn, uint8_t *data_buf = nullptr) + : ClientAbstract(kTypeIQUartFlightControllerInterface, obj_idn), + packed_command_(kTypeIQUartFlightControllerInterface, obj_idn, kSubPackedCommand, data_buf), + telemetry_(kTypeIQUartFlightControllerInterface, obj_idn, kSubTelemetry), + throttle_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubThrottleCvi), + x_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubXCvi), + y_cvi_(kTypeIQUartFlightControllerInterface, obj_idn, kSubYCvi) {}; + + // Client Entries + PackedClientEntry packed_command_; + ClientEntry telemetry_; + ClientEntry throttle_cvi_; + ClientEntry x_cvi_; + ClientEntry y_cvi_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubYCvi + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &packed_command_, // 0 + &telemetry_, // 1 + &throttle_cvi_, // 2 + &x_cvi_, // 3 + &y_cvi_ // 4 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + + /** + * @brief This function takes in an IFCIPackedMessage struct, and prepares the data as a series of bytes ready for transmission over IQUART + * As an example, assume you want to send 4 throttle commands, and get telemetry from Module ID 3. To do so, make an IFCIPackedMessage and set + * commands[0] through commands[3] to throttle values [0, 65535], telem_byte to 3, and num_cvs to 4. Then, create a byte array, and a byte to store the + * message length, and call this function. + * + * @param ifci_commands The IFCIPackedMessage struct that you want to send + * @param output_data A pointer to an array of bytes that will store the data + * @param output_data_length The length of the data stored in the output_data array + */ + void PackageIfciCommandsForTransmission(IFCIPackedMessage *ifci_commands, uint8_t *output_data, + uint8_t *output_data_length) + { + //Copy the CV bytes + memcpy(output_data, ifci_commands->commands, ifci_commands->num_cvs * 2); + + //Add the telem byte + output_data[ifci_commands->num_cvs * 2] = ifci_commands->telem_byte; + + //Set the output data length + *output_data_length = ifci_commands->num_cvs * 2 + 1; + } + +private: + static const uint8_t kSubPackedCommand = 0; + static const uint8_t kSubTelemetry = 1; + static const uint8_t kSubThrottleCvi = 2; + static const uint8_t kSubXCvi = 3; + static const uint8_t kSubYCvi = 4; }; #endif /* IQUART_FLIGHT_CONTROLLER_INTERFACE_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/mag_alpha_client.hpp b/inc/mag_alpha_client.hpp index 2544c69..17cc5f5 100644 --- a/inc/mag_alpha_client.hpp +++ b/inc/mag_alpha_client.hpp @@ -8,9 +8,9 @@ /* Name: mag_alpha_client.hpp - Last update: 2023/06/29 by Ben Quan - Author: Ben Quan - Contributors: + Last update: 2023/06/29 by Ben Quan + Author: Ben Quan + Contributors: */ #ifndef MAG_ALPHA_CLIENT_HPP @@ -20,56 +20,57 @@ const uint8_t kTypeMagAlpha = 75; -class MagAlphaClient: public ClientAbstract{ - public: - MagAlphaClient(uint8_t obj_idn): - ClientAbstract( kTypeMagAlpha, obj_idn), - angle_raw_(kTypeMagAlpha, obj_idn, kSubAngleRaw), - angle_rad_(kTypeMagAlpha, obj_idn, kSubAngleRad), - alarm_(kTypeMagAlpha, obj_idn, kSubAlarm), - mght_(kTypeMagAlpha, obj_idn, kSubMght), - mglt_(kTypeMagAlpha, obj_idn, kSubMglt), - reg_val_(kTypeMagAlpha, obj_idn, kSubRegVal), - reg_adr_(kTypeMagAlpha, obj_idn, kSubRegAdr), - reg_str_(kTypeMagAlpha, obj_idn, kSubRegStr) - {}; +class MagAlphaClient: public ClientAbstract +{ +public: + MagAlphaClient(uint8_t obj_idn): + ClientAbstract(kTypeMagAlpha, obj_idn), + angle_raw_(kTypeMagAlpha, obj_idn, kSubAngleRaw), + angle_rad_(kTypeMagAlpha, obj_idn, kSubAngleRad), + alarm_(kTypeMagAlpha, obj_idn, kSubAlarm), + mght_(kTypeMagAlpha, obj_idn, kSubMght), + mglt_(kTypeMagAlpha, obj_idn, kSubMglt), + reg_val_(kTypeMagAlpha, obj_idn, kSubRegVal), + reg_adr_(kTypeMagAlpha, obj_idn, kSubRegAdr), + reg_str_(kTypeMagAlpha, obj_idn, kSubRegStr) + {}; - // Client Entries - ClientEntry angle_raw_; - ClientEntry angle_rad_; - ClientEntry alarm_; - ClientEntry mght_; - ClientEntry mglt_; - ClientEntry reg_val_; - ClientEntry reg_adr_; - ClientEntryVoid reg_str_; + // Client Entries + ClientEntry angle_raw_; + ClientEntry angle_rad_; + ClientEntry alarm_; + ClientEntry mght_; + ClientEntry mglt_; + ClientEntry reg_val_; + ClientEntry reg_adr_; + ClientEntryVoid reg_str_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubRegStr+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &angle_raw_, // 0 - &angle_rad_, // 1 - &alarm_, // 2 - &mght_, // 3 - &mglt_, // 4 - ®_val_, // 5 - ®_adr_, // 6 - ®_str_ // 7 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubRegStr + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &angle_raw_, // 0 + &angle_rad_, // 1 + &alarm_, // 2 + &mght_, // 3 + &mglt_, // 4 + ®_val_, // 5 + ®_adr_, // 6 + ®_str_ // 7 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubAngleRaw = 0; - static const uint8_t kSubAngleRad = 1; - static const uint8_t kSubAlarm = 2; - static const uint8_t kSubMght = 3; - static const uint8_t kSubMglt = 4; - static const uint8_t kSubRegVal = 5; - static const uint8_t kSubRegAdr = 6; - static const uint8_t kSubRegStr = 7; +private: + static const uint8_t kSubAngleRaw = 0; + static const uint8_t kSubAngleRad = 1; + static const uint8_t kSubAlarm = 2; + static const uint8_t kSubMght = 3; + static const uint8_t kSubMglt = 4; + static const uint8_t kSubRegVal = 5; + static const uint8_t kSubRegAdr = 6; + static const uint8_t kSubRegStr = 7; }; #endif // MAG_ALPHA_CLIENT_H \ No newline at end of file diff --git a/inc/multi_turn_angle_control_client.hpp b/inc/multi_turn_angle_control_client.hpp index 9773c63..8318f11 100644 --- a/inc/multi_turn_angle_control_client.hpp +++ b/inc/multi_turn_angle_control_client.hpp @@ -20,156 +20,158 @@ const uint8_t kTypeAngleMotorControl = 59; -class MultiTurnAngleControlClient : public ClientAbstract { - public: - MultiTurnAngleControlClient(uint8_t obj_idn) - : ClientAbstract(kTypeAngleMotorControl, obj_idn), - ctrl_mode_(kTypeAngleMotorControl, obj_idn, kSubCtrlMode), - ctrl_brake_(kTypeAngleMotorControl, obj_idn, kSubCtrlBrake), - ctrl_coast_(kTypeAngleMotorControl, obj_idn, kSubCtrlCoast), - ctrl_pwm_(kTypeAngleMotorControl, obj_idn, kSubCtrlPwm), - ctrl_volts_(kTypeAngleMotorControl, obj_idn, kSubCtrlVolts), - ctrl_angle_(kTypeAngleMotorControl, obj_idn, kSubCtrlAngle), - ctrl_velocity_(kTypeAngleMotorControl, obj_idn, kSubCtrlVelocity), - angle_Kp_(kTypeAngleMotorControl, obj_idn, kSubAngleKp), - angle_Ki_(kTypeAngleMotorControl, obj_idn, kSubAngleKi), - angle_Kd_(kTypeAngleMotorControl, obj_idn, kSubAngleKd), - timeout_(kTypeAngleMotorControl, obj_idn, kSubTimeout), - obs_angular_displacement_(kTypeAngleMotorControl, obj_idn, kSubObsAngularDisplacement), - obs_angular_velocity_(kTypeAngleMotorControl, obj_idn, kSubObsAngularVelocity), - meter_per_rad_(kTypeAngleMotorControl, obj_idn, kSubMeterPerRad), - ctrl_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubCtrlLinearDisplacement), - ctrl_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubCtrlLinearVelocity), - obs_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubObsLinearDisplacement), - obs_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubObsLinearVelocity), - angular_speed_max_(kTypeAngleMotorControl, obj_idn, kSubAngularSpeedMax), - trajectory_angular_displacement_(kTypeAngleMotorControl, obj_idn, - kSubTrajectoryAngularDisplacement), - trajectory_angular_velocity_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryAngularVelocity), - trajectory_angular_acceleration_(kTypeAngleMotorControl, obj_idn, - kSubTrajectoryAngularAcceleration), - trajectory_duration_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryDuration), - trajectory_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearDisplacement), - trajectory_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearVelocity), - trajectory_linear_acceleration_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearAcceleration), - trajectory_average_speed_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryAverageSpeed), - trajectory_queue_mode_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryQueueMode), - ff_(kTypeAngleMotorControl, obj_idn, kSubFF), - sample_zero_angle_(kTypeAngleMotorControl, obj_idn, kSubSampleZeroAngle), - zero_angle_(kTypeAngleMotorControl, obj_idn, kSubZeroAngle){}; +class MultiTurnAngleControlClient : public ClientAbstract +{ +public: + MultiTurnAngleControlClient(uint8_t obj_idn) + : ClientAbstract(kTypeAngleMotorControl, obj_idn), + ctrl_mode_(kTypeAngleMotorControl, obj_idn, kSubCtrlMode), + ctrl_brake_(kTypeAngleMotorControl, obj_idn, kSubCtrlBrake), + ctrl_coast_(kTypeAngleMotorControl, obj_idn, kSubCtrlCoast), + ctrl_pwm_(kTypeAngleMotorControl, obj_idn, kSubCtrlPwm), + ctrl_volts_(kTypeAngleMotorControl, obj_idn, kSubCtrlVolts), + ctrl_angle_(kTypeAngleMotorControl, obj_idn, kSubCtrlAngle), + ctrl_velocity_(kTypeAngleMotorControl, obj_idn, kSubCtrlVelocity), + angle_Kp_(kTypeAngleMotorControl, obj_idn, kSubAngleKp), + angle_Ki_(kTypeAngleMotorControl, obj_idn, kSubAngleKi), + angle_Kd_(kTypeAngleMotorControl, obj_idn, kSubAngleKd), + timeout_(kTypeAngleMotorControl, obj_idn, kSubTimeout), + obs_angular_displacement_(kTypeAngleMotorControl, obj_idn, kSubObsAngularDisplacement), + obs_angular_velocity_(kTypeAngleMotorControl, obj_idn, kSubObsAngularVelocity), + meter_per_rad_(kTypeAngleMotorControl, obj_idn, kSubMeterPerRad), + ctrl_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubCtrlLinearDisplacement), + ctrl_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubCtrlLinearVelocity), + obs_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubObsLinearDisplacement), + obs_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubObsLinearVelocity), + angular_speed_max_(kTypeAngleMotorControl, obj_idn, kSubAngularSpeedMax), + trajectory_angular_displacement_(kTypeAngleMotorControl, obj_idn, + kSubTrajectoryAngularDisplacement), + trajectory_angular_velocity_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryAngularVelocity), + trajectory_angular_acceleration_(kTypeAngleMotorControl, obj_idn, + kSubTrajectoryAngularAcceleration), + trajectory_duration_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryDuration), + trajectory_linear_displacement_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearDisplacement), + trajectory_linear_velocity_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearVelocity), + trajectory_linear_acceleration_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryLinearAcceleration), + trajectory_average_speed_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryAverageSpeed), + trajectory_queue_mode_(kTypeAngleMotorControl, obj_idn, kSubTrajectoryQueueMode), + ff_(kTypeAngleMotorControl, obj_idn, kSubFF), + sample_zero_angle_(kTypeAngleMotorControl, obj_idn, kSubSampleZeroAngle), + zero_angle_(kTypeAngleMotorControl, obj_idn, kSubZeroAngle) {}; - // Client Entries - // Control commands - ClientEntry ctrl_mode_; - ClientEntryVoid ctrl_brake_; - ClientEntryVoid ctrl_coast_; - ClientEntry ctrl_pwm_; - ClientEntry ctrl_volts_; - ClientEntry ctrl_angle_; - ClientEntry ctrl_velocity_; - // Angle control - ClientEntry angle_Kp_; - ClientEntry angle_Ki_; - ClientEntry angle_Kd_; - // Timeout - ClientEntry timeout_; - // Angular values - ClientEntry obs_angular_displacement_; - ClientEntry obs_angular_velocity_; - // Linear conversion - ClientEntry meter_per_rad_; - ClientEntry ctrl_linear_displacement_; - ClientEntry ctrl_linear_velocity_; - ClientEntry obs_linear_displacement_; - ClientEntry obs_linear_velocity_; - // Limits - ClientEntry angular_speed_max_; - // Trajectory - ClientEntry trajectory_angular_displacement_; - ClientEntry trajectory_angular_velocity_; - ClientEntry trajectory_angular_acceleration_; - ClientEntry trajectory_duration_; - ClientEntry trajectory_linear_displacement_; - ClientEntry trajectory_linear_velocity_; - ClientEntry trajectory_linear_acceleration_; - ClientEntry trajectory_average_speed_; - ClientEntry trajectory_queue_mode_; - ClientEntry ff_; - ClientEntryVoid sample_zero_angle_; - ClientEntry zero_angle_; + // Client Entries + // Control commands + ClientEntry ctrl_mode_; + ClientEntryVoid ctrl_brake_; + ClientEntryVoid ctrl_coast_; + ClientEntry ctrl_pwm_; + ClientEntry ctrl_volts_; + ClientEntry ctrl_angle_; + ClientEntry ctrl_velocity_; + // Angle control + ClientEntry angle_Kp_; + ClientEntry angle_Ki_; + ClientEntry angle_Kd_; + // Timeout + ClientEntry timeout_; + // Angular values + ClientEntry obs_angular_displacement_; + ClientEntry obs_angular_velocity_; + // Linear conversion + ClientEntry meter_per_rad_; + ClientEntry ctrl_linear_displacement_; + ClientEntry ctrl_linear_velocity_; + ClientEntry obs_linear_displacement_; + ClientEntry obs_linear_velocity_; + // Limits + ClientEntry angular_speed_max_; + // Trajectory + ClientEntry trajectory_angular_displacement_; + ClientEntry trajectory_angular_velocity_; + ClientEntry trajectory_angular_acceleration_; + ClientEntry trajectory_duration_; + ClientEntry trajectory_linear_displacement_; + ClientEntry trajectory_linear_velocity_; + ClientEntry trajectory_linear_acceleration_; + ClientEntry trajectory_average_speed_; + ClientEntry trajectory_queue_mode_; + ClientEntry ff_; + ClientEntryVoid sample_zero_angle_; + ClientEntry zero_angle_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubZeroAngle + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &ctrl_mode_, // 0 - &ctrl_brake_, // 1 - &ctrl_coast_, // 2 - &ctrl_angle_, // 3 - &ctrl_velocity_, // 4 - &angle_Kp_, // 5 - &angle_Ki_, // 6 - &angle_Kd_, // 7 - &timeout_, // 8 - &ctrl_pwm_, // 9 - &ctrl_volts_, // 10 - &obs_angular_displacement_, // 11 - &obs_angular_velocity_, // 12 - &meter_per_rad_, // 13 - &ctrl_linear_displacement_, // 14 - &ctrl_linear_velocity_, // 15 - &obs_linear_displacement_, // 16 - &obs_linear_velocity_, // 17 - &angular_speed_max_, // 18 - &trajectory_angular_displacement_, // 19 - &trajectory_angular_velocity_, // 20 - &trajectory_angular_acceleration_, // 21 - &trajectory_duration_, // 22 - &trajectory_linear_displacement_, // 23 - &trajectory_linear_velocity_, // 24 - &trajectory_linear_acceleration_, // 25 - &trajectory_average_speed_, // 26 - &trajectory_queue_mode_, // 27 - nullptr, // 28 - &ff_, // 29 - &sample_zero_angle_, // 30 - &zero_angle_ // 31 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubZeroAngle + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &ctrl_mode_, // 0 + &ctrl_brake_, // 1 + &ctrl_coast_, // 2 + &ctrl_angle_, // 3 + &ctrl_velocity_, // 4 + &angle_Kp_, // 5 + &angle_Ki_, // 6 + &angle_Kd_, // 7 + &timeout_, // 8 + &ctrl_pwm_, // 9 + &ctrl_volts_, // 10 + &obs_angular_displacement_, // 11 + &obs_angular_velocity_, // 12 + &meter_per_rad_, // 13 + &ctrl_linear_displacement_, // 14 + &ctrl_linear_velocity_, // 15 + &obs_linear_displacement_, // 16 + &obs_linear_velocity_, // 17 + &angular_speed_max_, // 18 + &trajectory_angular_displacement_, // 19 + &trajectory_angular_velocity_, // 20 + &trajectory_angular_acceleration_, // 21 + &trajectory_duration_, // 22 + &trajectory_linear_displacement_, // 23 + &trajectory_linear_velocity_, // 24 + &trajectory_linear_acceleration_, // 25 + &trajectory_average_speed_, // 26 + &trajectory_queue_mode_, // 27 + nullptr, // 28 + &ff_, // 29 + &sample_zero_angle_, // 30 + &zero_angle_ // 31 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubCtrlMode = 0; - static const uint8_t kSubCtrlBrake = 1; - static const uint8_t kSubCtrlCoast = 2; - static const uint8_t kSubCtrlAngle = 3; - static const uint8_t kSubCtrlVelocity = 4; - static const uint8_t kSubAngleKp = 5; - static const uint8_t kSubAngleKi = 6; - static const uint8_t kSubAngleKd = 7; - static const uint8_t kSubTimeout = 8; - static const uint8_t kSubCtrlPwm = 9; - static const uint8_t kSubCtrlVolts = 10; - static const uint8_t kSubObsAngularDisplacement = 11; - static const uint8_t kSubObsAngularVelocity = 12; - static const uint8_t kSubMeterPerRad = 13; - static const uint8_t kSubCtrlLinearDisplacement = 14; - static const uint8_t kSubCtrlLinearVelocity = 15; - static const uint8_t kSubObsLinearDisplacement = 16; - static const uint8_t kSubObsLinearVelocity = 17; - static const uint8_t kSubAngularSpeedMax = 18; - static const uint8_t kSubTrajectoryAngularDisplacement = 19; - static const uint8_t kSubTrajectoryAngularVelocity = 20; - static const uint8_t kSubTrajectoryAngularAcceleration = 21; - static const uint8_t kSubTrajectoryDuration = 22; - static const uint8_t kSubTrajectoryLinearDisplacement = 23; - static const uint8_t kSubTrajectoryLinearVelocity = 24; - static const uint8_t kSubTrajectoryLinearAcceleration = 25; - static const uint8_t kSubTrajectoryAverageSpeed = 26; - static const uint8_t kSubTrajectoryQueueMode = 27; - static const uint8_t kSubFF = 29; - static const uint8_t kSubSampleZeroAngle = 30; - static const uint8_t kSubZeroAngle = 31; +private: + static const uint8_t kSubCtrlMode = 0; + static const uint8_t kSubCtrlBrake = 1; + static const uint8_t kSubCtrlCoast = 2; + static const uint8_t kSubCtrlAngle = 3; + static const uint8_t kSubCtrlVelocity = 4; + static const uint8_t kSubAngleKp = 5; + static const uint8_t kSubAngleKi = 6; + static const uint8_t kSubAngleKd = 7; + static const uint8_t kSubTimeout = 8; + static const uint8_t kSubCtrlPwm = 9; + static const uint8_t kSubCtrlVolts = 10; + static const uint8_t kSubObsAngularDisplacement = 11; + static const uint8_t kSubObsAngularVelocity = 12; + static const uint8_t kSubMeterPerRad = 13; + static const uint8_t kSubCtrlLinearDisplacement = 14; + static const uint8_t kSubCtrlLinearVelocity = 15; + static const uint8_t kSubObsLinearDisplacement = 16; + static const uint8_t kSubObsLinearVelocity = 17; + static const uint8_t kSubAngularSpeedMax = 18; + static const uint8_t kSubTrajectoryAngularDisplacement = 19; + static const uint8_t kSubTrajectoryAngularVelocity = 20; + static const uint8_t kSubTrajectoryAngularAcceleration = 21; + static const uint8_t kSubTrajectoryDuration = 22; + static const uint8_t kSubTrajectoryLinearDisplacement = 23; + static const uint8_t kSubTrajectoryLinearVelocity = 24; + static const uint8_t kSubTrajectoryLinearAcceleration = 25; + static const uint8_t kSubTrajectoryAverageSpeed = 26; + static const uint8_t kSubTrajectoryQueueMode = 27; + static const uint8_t kSubFF = 29; + static const uint8_t kSubSampleZeroAngle = 30; + static const uint8_t kSubZeroAngle = 31; }; #endif /* MULTI_TURN_ANGLE_CONTROL_CLIENT_HPP_ */ diff --git a/inc/packet_finder.h b/inc/packet_finder.h index f03e4e7..6ed55c0 100644 --- a/inc/packet_finder.h +++ b/inc/packet_finder.h @@ -16,9 +16,9 @@ #ifndef PACKET_FINDER_H #define PACKET_FINDER_H -/// PacketFinder enables extraction of well formed, crc-verified packets from a -/// byte stream. It is a specialized queue/buffer which takes in raw bytes and -/// returns packet data. The returned packet data is a byte array consisting of +/// PacketFinder enables extraction of well formed, crc-verified packets from a +/// byte stream. It is a specialized queue/buffer which takes in raw bytes and +/// returns packet data. The returned packet data is a byte array consisting of /// a type byte followed by data bytes. /// /// General Packet Format: @@ -27,7 +27,7 @@ /// 'type' is the (uint8) message type /// 'data' is a series of (uint8) bytes, serialized Little-Endian /// 'crc' is the (uint16) CRC value for 'length'+'type'+'data', Little-Endian - + #include "byte_queue.h" #include @@ -37,9 +37,9 @@ extern "C" { //#define COMPUTER #ifdef COMPUTER - #define PACKET_BUFFER_SIZE 1023 /// size of input buffer +#define PACKET_BUFFER_SIZE 1023 /// size of input buffer #else - #define PACKET_BUFFER_SIZE 64 /// size of input buffer +#define PACKET_BUFFER_SIZE 64 /// size of input buffer #endif #define MAX_PACKET_SIZE 64 /// size packet (including header and CRC) #define MAX_PACKET_DATA_SIZE (MAX_PACKET_SIZE-5) /// size data (not type, etc) @@ -48,39 +48,39 @@ extern const uint8_t kStartByte; /// special start byte /// Internally used state machine states, voluntarily opaque. enum PacketFinderState { - kStart, - kLen, - kType, - kData, - kCRCL, - kCRCH, + kStart, + kLen, + kType, + kData, + kCRCL, + kCRCH, }; - + /// Interally used instance state struct, voluntarily opaque. struct PacketFinder { - uint8_t* start_data; /// pointer to first data byte in buffer - uint8_t* end_data; /// pointer to byte following last data byte in buffer - - enum PacketFinderState state; /// parser state machine state - - uint16_t parse_index; /// index of next parser operation - uint16_t packet_start_index; /// start index for current packet - uint16_t received_length; /// bytes in data reported for current packet - - uint16_t expected_crc; /// crc calculated for recieved data - uint16_t received_crc; /// crc received in packet footer - uint16_t data_bytes; /// bytes of type+data read in current packet - - uint8_t buffer[PACKET_BUFFER_SIZE]; /// buffer of recieved packet data - uint8_t out_buffer[MAX_PACKET_SIZE]; /// fixed output buffer for contiguous access - - struct ByteQueue* packet_indices; - // maintain queue of indices in buffer at which to find complete packets - // each index gives the location of a packet's length byte in buffer - // length bytes of type+data always follow contiguously after + uint8_t *start_data; /// pointer to first data byte in buffer + uint8_t *end_data; /// pointer to byte following last data byte in buffer + + enum PacketFinderState state; /// parser state machine state + + uint16_t parse_index; /// index of next parser operation + uint16_t packet_start_index; /// start index for current packet + uint16_t received_length; /// bytes in data reported for current packet + + uint16_t expected_crc; /// crc calculated for recieved data + uint16_t received_crc; /// crc received in packet footer + uint16_t data_bytes; /// bytes of type+data read in current packet + + uint8_t buffer[PACKET_BUFFER_SIZE]; /// buffer of recieved packet data + uint8_t out_buffer[MAX_PACKET_SIZE]; /// fixed output buffer for contiguous access + + struct ByteQueue *packet_indices; + // maintain queue of indices in buffer at which to find complete packets + // each index gives the location of a packet's length byte in buffer + // length bytes of type+data always follow contiguously after }; - + /// Initialize a PacketFinder state struct /// @param pf Pointer to an uninitialized PacketFinder struct. void InitPacketFinder(struct PacketFinder *pf, struct ByteQueue *bq); @@ -94,11 +94,11 @@ void InitPacketFinder(struct PacketFinder *pf, struct ByteQueue *bq); /// -2 for fatal error int8_t PutBytes(struct PacketFinder *pf, const uint8_t *data, uint16_t length); -/// Peek at the next available packet in queue. If a packet is ready, pointer -/// 'packet' will point to the first byte (the type) and 'length' will give the -/// length of packet type+data, which may be read as a contiguous array. -/// Arguments 'packet' and 'length' are ignored if no packet is ready. Repeated -/// calls to PeekPacket will return pointers to the same packet until DropPacket +/// Peek at the next available packet in queue. If a packet is ready, pointer +/// 'packet' will point to the first byte (the type) and 'length' will give the +/// length of packet type+data, which may be read as a contiguous array. +/// Arguments 'packet' and 'length' are ignored if no packet is ready. Repeated +/// calls to PeekPacket will return pointers to the same packet until DropPacket /// is used. /// @param pf Pointer to PacketFinder struct. /// @param packet Reference to a pointer to data, function redirects pointer. @@ -113,9 +113,9 @@ int8_t PeekPacket(struct PacketFinder *pf, uint8_t **packet, uint8_t *length); /// 0 if no packet ready to remove, int8_t DropPacket(struct PacketFinder *pf); -/// Copy the next available completed packet and remove from queue. Output -/// argument 'packet' must reserve enough space to copy the packet. Output -/// argument 'length' is of type+data. Use of the Peek/Drop functions will +/// Copy the next available completed packet and remove from queue. Output +/// argument 'packet' must reserve enough space to copy the packet. Output +/// argument 'length' is of type+data. Use of the Peek/Drop functions will /// typically yield better performance with less copying. /// @param pf Pointer to PacketFinder struct. /// @param packet Pointer to byte data, function changes data. @@ -124,8 +124,8 @@ int8_t DropPacket(struct PacketFinder *pf); /// 0 if no packet ready to pop int8_t GetPacketCopy(struct PacketFinder *pf, uint8_t *packet, uint8_t *length); -/// Embed a byte data string into a packet with start header, type, and CRC -/// footer. The generated packet copied to out_data is in_len+5 bytes long, and +/// Embed a byte data string into a packet with start header, type, and CRC +/// footer. The generated packet copied to out_data is in_len+5 bytes long, and /// sufficient space in out_data must be reserved! /// @param type Message type byte. /// @param in_data Pointer to data for packet. @@ -133,9 +133,9 @@ int8_t GetPacketCopy(struct PacketFinder *pf, uint8_t *packet, uint8_t *length); /// @param out_data Pointer to location for function to create packet. /// @param out_len Pointer to integer packet length, function changes value. int8_t FormPacket(uint8_t type, - const uint8_t *in_data, uint8_t in_len, - uint8_t *out_data, uint8_t *out_len); - + const uint8_t *in_data, uint8_t in_len, + uint8_t *out_data, uint8_t *out_len); + #ifdef __cplusplus } #endif // __cplusplus diff --git a/inc/persistent_memory_client.hpp b/inc/persistent_memory_client.hpp index af696bb..8b72fb4 100644 --- a/inc/persistent_memory_client.hpp +++ b/inc/persistent_memory_client.hpp @@ -8,7 +8,7 @@ /* Name: persistent_memory_client.hpp - Last update: 10/31/2022 by Ben Quan + Last update: 10/31/2022 by Ben Quan Author: Matthew Piccoli Contributors: Ben Quan, Raphael Van Hoffelen */ @@ -20,42 +20,43 @@ const uint8_t kTypePersistentMemory = 11; -class PersistentMemoryClient: public ClientAbstract{ - public: - PersistentMemoryClient(uint8_t obj_idn): - ClientAbstract( kTypePersistentMemory, obj_idn), - erase_( kTypePersistentMemory, obj_idn, kSubErase), - revert_to_default_( kTypePersistentMemory, obj_idn, kSubRevertToDefault), - format_key_1_( kTypePersistentMemory, obj_idn, kSubFormatKey1), - format_key_2_( kTypePersistentMemory, obj_idn, kSubFormatKey2) - {}; - - // Client Entries - // Control commands - ClientEntryVoid erase_; - ClientEntryVoid revert_to_default_; - ClientEntry format_key_1_; - ClientEntry format_key_2_; - - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubFormatKey2+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &erase_, // 0 - &revert_to_default_, // 1 - &format_key_1_, // 2 - &format_key_2_ // 3 - }; - - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubErase = 0; - static const uint8_t kSubRevertToDefault = 1; - static const uint8_t kSubFormatKey1 = 2; - static const uint8_t kSubFormatKey2 = 3; +class PersistentMemoryClient: public ClientAbstract +{ +public: + PersistentMemoryClient(uint8_t obj_idn): + ClientAbstract(kTypePersistentMemory, obj_idn), + erase_(kTypePersistentMemory, obj_idn, kSubErase), + revert_to_default_(kTypePersistentMemory, obj_idn, kSubRevertToDefault), + format_key_1_(kTypePersistentMemory, obj_idn, kSubFormatKey1), + format_key_2_(kTypePersistentMemory, obj_idn, kSubFormatKey2) + {}; + + // Client Entries + // Control commands + ClientEntryVoid erase_; + ClientEntryVoid revert_to_default_; + ClientEntry format_key_1_; + ClientEntry format_key_2_; + + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubFormatKey2 + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &erase_, // 0 + &revert_to_default_, // 1 + &format_key_1_, // 2 + &format_key_2_ // 3 + }; + + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubErase = 0; + static const uint8_t kSubRevertToDefault = 1; + static const uint8_t kSubFormatKey1 = 2; + static const uint8_t kSubFormatKey2 = 3; }; #endif /* PERSISTENT_MEMORY_CLIENT_HPP_ */ diff --git a/inc/power_monitor_client.hpp b/inc/power_monitor_client.hpp index e5fae52..1477959 100644 --- a/inc/power_monitor_client.hpp +++ b/inc/power_monitor_client.hpp @@ -20,73 +20,74 @@ const uint8_t kTypePowerMonitor = 69; -class PowerMonitorClient: public ClientAbstract{ - public: - PowerMonitorClient(uint8_t obj_idn): - ClientAbstract( kTypePowerMonitor, obj_idn), - volts_( kTypePowerMonitor, obj_idn, kSubVolts), - amps_( kTypePowerMonitor, obj_idn, kSubAmps), - watts_( kTypePowerMonitor, obj_idn, kSubWatts), - joules_( kTypePowerMonitor, obj_idn, kSubJoules), - reset_joules_( kTypePowerMonitor, obj_idn, kSubResetJoules), - filter_fs_( kTypePowerMonitor, obj_idn, kSubFilterFs), - filter_fc_( kTypePowerMonitor, obj_idn, kSubFilterFc), - volts_raw_( kTypePowerMonitor, obj_idn, kSubVoltsRaw), - amps_raw_( kTypePowerMonitor, obj_idn, kSubAmpsRaw), - volts_gain_( kTypePowerMonitor, obj_idn, kSubVoltsGain), - amps_gain_( kTypePowerMonitor, obj_idn, kSubAmpsGain), - amps_bias_( kTypePowerMonitor, obj_idn, kSubAmpsBias) - {}; +class PowerMonitorClient: public ClientAbstract +{ +public: + PowerMonitorClient(uint8_t obj_idn): + ClientAbstract(kTypePowerMonitor, obj_idn), + volts_(kTypePowerMonitor, obj_idn, kSubVolts), + amps_(kTypePowerMonitor, obj_idn, kSubAmps), + watts_(kTypePowerMonitor, obj_idn, kSubWatts), + joules_(kTypePowerMonitor, obj_idn, kSubJoules), + reset_joules_(kTypePowerMonitor, obj_idn, kSubResetJoules), + filter_fs_(kTypePowerMonitor, obj_idn, kSubFilterFs), + filter_fc_(kTypePowerMonitor, obj_idn, kSubFilterFc), + volts_raw_(kTypePowerMonitor, obj_idn, kSubVoltsRaw), + amps_raw_(kTypePowerMonitor, obj_idn, kSubAmpsRaw), + volts_gain_(kTypePowerMonitor, obj_idn, kSubVoltsGain), + amps_gain_(kTypePowerMonitor, obj_idn, kSubAmpsGain), + amps_bias_(kTypePowerMonitor, obj_idn, kSubAmpsBias) + {}; - // Client Entries - // Control commands - ClientEntry volts_; - ClientEntry amps_; - ClientEntry watts_; - ClientEntry joules_; - ClientEntryVoid reset_joules_; - ClientEntry filter_fs_; - ClientEntry filter_fc_; - ClientEntry volts_raw_; - ClientEntry amps_raw_; - ClientEntry volts_gain_; - ClientEntry amps_gain_; - ClientEntry amps_bias_; + // Client Entries + // Control commands + ClientEntry volts_; + ClientEntry amps_; + ClientEntry watts_; + ClientEntry joules_; + ClientEntryVoid reset_joules_; + ClientEntry filter_fs_; + ClientEntry filter_fc_; + ClientEntry volts_raw_; + ClientEntry amps_raw_; + ClientEntry volts_gain_; + ClientEntry amps_gain_; + ClientEntry amps_bias_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubAmpsBias+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &volts_, // 0 - &s_, // 1 - &watts_, // 2 - &joules_, // 3 - &reset_joules_, // 4 - &filter_fs_, // 5 - &filter_fc_, // 6 - &volts_raw_, // 7 - &s_raw_, // 8 - &volts_gain_, // 9 - &s_gain_, // 10 - &s_bias_ // 11 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubAmpsBias + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &volts_, // 0 + &s_, // 1 + &watts_, // 2 + &joules_, // 3 + &reset_joules_, // 4 + &filter_fs_, // 5 + &filter_fc_, // 6 + &volts_raw_, // 7 + &s_raw_, // 8 + &volts_gain_, // 9 + &s_gain_, // 10 + &s_bias_ // 11 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubVolts = 0; - static const uint8_t kSubAmps = 1; - static const uint8_t kSubWatts = 2; - static const uint8_t kSubJoules = 3; - static const uint8_t kSubResetJoules = 4; - static const uint8_t kSubFilterFs = 5; - static const uint8_t kSubFilterFc = 6; - static const uint8_t kSubVoltsRaw = 7; - static const uint8_t kSubAmpsRaw = 8; - static const uint8_t kSubVoltsGain = 9; - static const uint8_t kSubAmpsGain = 10; - static const uint8_t kSubAmpsBias = 11; +private: + static const uint8_t kSubVolts = 0; + static const uint8_t kSubAmps = 1; + static const uint8_t kSubWatts = 2; + static const uint8_t kSubJoules = 3; + static const uint8_t kSubResetJoules = 4; + static const uint8_t kSubFilterFs = 5; + static const uint8_t kSubFilterFc = 6; + static const uint8_t kSubVoltsRaw = 7; + static const uint8_t kSubAmpsRaw = 8; + static const uint8_t kSubVoltsGain = 9; + static const uint8_t kSubAmpsGain = 10; + static const uint8_t kSubAmpsBias = 11; }; #endif /* POWER_MONITOR_CLIENT_HPP_ */ diff --git a/inc/power_safety_client.hpp b/inc/power_safety_client.hpp index 77af37e..8c9f9d5 100644 --- a/inc/power_safety_client.hpp +++ b/inc/power_safety_client.hpp @@ -20,81 +20,83 @@ const uint8_t kTypePowerSafety = 84; -class PowerSafetyClient : public ClientAbstract { - public: - PowerSafetyClient(uint8_t obj_idn) - : ClientAbstract(kTypePowerSafety, obj_idn), - fault_now_(kTypePowerSafety, obj_idn, kSubFaultNow), - fault_ever_(kTypePowerSafety, obj_idn, kSubFaultEver), - fault_latching_(kTypePowerSafety, obj_idn, kSubFaultLatching), - volt_input_low_(kTypePowerSafety, obj_idn, kSubVoltInputLow), - volt_input_high_(kTypePowerSafety, obj_idn, kSubVoltInputHigh), - vref_int_low_(kTypePowerSafety, obj_idn, kSubVrefIntLow), - vref_int_high_(kTypePowerSafety, obj_idn, kSubVrefIntHigh), - current_input_low_(kTypePowerSafety, obj_idn, kSubCurrentInputLow), - current_input_high_(kTypePowerSafety, obj_idn, kSubCurrentInputHigh), - motor_current_low_(kTypePowerSafety, obj_idn, kSubMotorCurrentLow), - motor_current_high_(kTypePowerSafety, obj_idn, kSubMotorCurrentHigh), - temperature_uc_low_(kTypePowerSafety, obj_idn, kSubTemperatureUcLow), - temperature_uc_high_(kTypePowerSafety, obj_idn, kSubTemperatureUcHigh), - temperature_coil_low_(kTypePowerSafety, obj_idn, kSubTemperatureCoilLow), - temperature_coil_high_(kTypePowerSafety, obj_idn, kSubTemperatureCoilHigh){}; +class PowerSafetyClient : public ClientAbstract +{ +public: + PowerSafetyClient(uint8_t obj_idn) + : ClientAbstract(kTypePowerSafety, obj_idn), + fault_now_(kTypePowerSafety, obj_idn, kSubFaultNow), + fault_ever_(kTypePowerSafety, obj_idn, kSubFaultEver), + fault_latching_(kTypePowerSafety, obj_idn, kSubFaultLatching), + volt_input_low_(kTypePowerSafety, obj_idn, kSubVoltInputLow), + volt_input_high_(kTypePowerSafety, obj_idn, kSubVoltInputHigh), + vref_int_low_(kTypePowerSafety, obj_idn, kSubVrefIntLow), + vref_int_high_(kTypePowerSafety, obj_idn, kSubVrefIntHigh), + current_input_low_(kTypePowerSafety, obj_idn, kSubCurrentInputLow), + current_input_high_(kTypePowerSafety, obj_idn, kSubCurrentInputHigh), + motor_current_low_(kTypePowerSafety, obj_idn, kSubMotorCurrentLow), + motor_current_high_(kTypePowerSafety, obj_idn, kSubMotorCurrentHigh), + temperature_uc_low_(kTypePowerSafety, obj_idn, kSubTemperatureUcLow), + temperature_uc_high_(kTypePowerSafety, obj_idn, kSubTemperatureUcHigh), + temperature_coil_low_(kTypePowerSafety, obj_idn, kSubTemperatureCoilLow), + temperature_coil_high_(kTypePowerSafety, obj_idn, kSubTemperatureCoilHigh) {}; - // Client Entries - ClientEntry fault_now_; - ClientEntry fault_ever_; - ClientEntry fault_latching_; - ClientEntry volt_input_low_; - ClientEntry volt_input_high_; - ClientEntry vref_int_low_; - ClientEntry vref_int_high_; - ClientEntry current_input_low_; - ClientEntry current_input_high_; - ClientEntry motor_current_low_; - ClientEntry motor_current_high_; - ClientEntry temperature_uc_low_; - ClientEntry temperature_uc_high_; - ClientEntry temperature_coil_low_; - ClientEntry temperature_coil_high_; + // Client Entries + ClientEntry fault_now_; + ClientEntry fault_ever_; + ClientEntry fault_latching_; + ClientEntry volt_input_low_; + ClientEntry volt_input_high_; + ClientEntry vref_int_low_; + ClientEntry vref_int_high_; + ClientEntry current_input_low_; + ClientEntry current_input_high_; + ClientEntry motor_current_low_; + ClientEntry motor_current_high_; + ClientEntry temperature_uc_low_; + ClientEntry temperature_uc_high_; + ClientEntry temperature_coil_low_; + ClientEntry temperature_coil_high_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubTemperatureCoilHigh + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &fault_now_, // 0 - &fault_ever_, // 1 - &fault_latching_, // 2 - &volt_input_low_, // 3 - &volt_input_high_, // 4 - &vref_int_low_, // 5 - &vref_int_high_, // 6 - ¤t_input_low_, // 7 - ¤t_input_high_, // 8 - &motor_current_low_, // 9 - &motor_current_high_, // 10 - &temperature_uc_low_, // 11 - &temperature_uc_high_, // 12 - &temperature_coil_low_, // 13 - &temperature_coil_high_, // 14 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubTemperatureCoilHigh + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &fault_now_, // 0 + &fault_ever_, // 1 + &fault_latching_, // 2 + &volt_input_low_, // 3 + &volt_input_high_, // 4 + &vref_int_low_, // 5 + &vref_int_high_, // 6 + ¤t_input_low_, // 7 + ¤t_input_high_, // 8 + &motor_current_low_, // 9 + &motor_current_high_, // 10 + &temperature_uc_low_, // 11 + &temperature_uc_high_, // 12 + &temperature_coil_low_, // 13 + &temperature_coil_high_, // 14 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubFaultNow = 0; - static const uint8_t kSubFaultEver = 1; - static const uint8_t kSubFaultLatching = 2; - static const uint8_t kSubVoltInputLow = 3; - static const uint8_t kSubVoltInputHigh = 4; - static const uint8_t kSubVrefIntLow = 5; - static const uint8_t kSubVrefIntHigh = 6; - static const uint8_t kSubCurrentInputLow = 7; - static const uint8_t kSubCurrentInputHigh = 8; - static const uint8_t kSubMotorCurrentLow = 9; - static const uint8_t kSubMotorCurrentHigh = 10; - static const uint8_t kSubTemperatureUcLow = 11; - static const uint8_t kSubTemperatureUcHigh = 12; - static const uint8_t kSubTemperatureCoilLow = 13; - static const uint8_t kSubTemperatureCoilHigh = 14; +private: + static const uint8_t kSubFaultNow = 0; + static const uint8_t kSubFaultEver = 1; + static const uint8_t kSubFaultLatching = 2; + static const uint8_t kSubVoltInputLow = 3; + static const uint8_t kSubVoltInputHigh = 4; + static const uint8_t kSubVrefIntLow = 5; + static const uint8_t kSubVrefIntHigh = 6; + static const uint8_t kSubCurrentInputLow = 7; + static const uint8_t kSubCurrentInputHigh = 8; + static const uint8_t kSubMotorCurrentLow = 9; + static const uint8_t kSubMotorCurrentHigh = 10; + static const uint8_t kSubTemperatureUcLow = 11; + static const uint8_t kSubTemperatureUcHigh = 12; + static const uint8_t kSubTemperatureCoilLow = 13; + static const uint8_t kSubTemperatureCoilHigh = 14; }; #endif /* POWER_SAFETY_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/propeller_motor_control_client.hpp b/inc/propeller_motor_control_client.hpp index a0793a9..c39f0a7 100644 --- a/inc/propeller_motor_control_client.hpp +++ b/inc/propeller_motor_control_client.hpp @@ -20,107 +20,109 @@ const uint8_t kTypePropellerMotorControl = 52; -class PropellerMotorControlClient : public ClientAbstract { - public: - PropellerMotorControlClient(uint8_t obj_idn) - : ClientAbstract(kTypePropellerMotorControl, obj_idn), - ctrl_mode_(kTypePropellerMotorControl, obj_idn, kSubCtrlMode), - ctrl_brake_(kTypePropellerMotorControl, obj_idn, kSubCtrlBrake), - ctrl_coast_(kTypePropellerMotorControl, obj_idn, kSubCtrlCoast), - ctrl_pwm_(kTypePropellerMotorControl, obj_idn, kSubCtrlPwm), - ctrl_volts_(kTypePropellerMotorControl, obj_idn, kSubCtrlVolts), - ctrl_velocity_(kTypePropellerMotorControl, obj_idn, kSubCtrlVelocity), - ctrl_thrust_(kTypePropellerMotorControl, obj_idn, kSubCtrlThrust), - velocity_kp_(kTypePropellerMotorControl, obj_idn, kSubVelocityKp), - velocity_ki_(kTypePropellerMotorControl, obj_idn, kSubVelocityKi), - velocity_kd_(kTypePropellerMotorControl, obj_idn, kSubVelocityKd), - velocity_ff0_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF0), - velocity_ff1_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF1), - velocity_ff2_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF2), - propeller_kt_pos_(kTypePropellerMotorControl, obj_idn, kSubPropellerKtPos), - propeller_kt_neg_(kTypePropellerMotorControl, obj_idn, kSubPropellerKtNeg), - timeout_(kTypePropellerMotorControl, obj_idn, kSubTimeout), - input_filter_fc_(kTypePropellerMotorControl, obj_idn, kSubInputFilterFc), - timeout_meaning_(kTypePropellerMotorControl, obj_idn, kSubTimeoutMeaning), - timeout_behavior_(kTypePropellerMotorControl, obj_idn, kSubTimeoutBehavior), - timeout_song_option_(kTypePropellerMotorControl, obj_idn, kSubTimeoutSongOption){}; +class PropellerMotorControlClient : public ClientAbstract +{ +public: + PropellerMotorControlClient(uint8_t obj_idn) + : ClientAbstract(kTypePropellerMotorControl, obj_idn), + ctrl_mode_(kTypePropellerMotorControl, obj_idn, kSubCtrlMode), + ctrl_brake_(kTypePropellerMotorControl, obj_idn, kSubCtrlBrake), + ctrl_coast_(kTypePropellerMotorControl, obj_idn, kSubCtrlCoast), + ctrl_pwm_(kTypePropellerMotorControl, obj_idn, kSubCtrlPwm), + ctrl_volts_(kTypePropellerMotorControl, obj_idn, kSubCtrlVolts), + ctrl_velocity_(kTypePropellerMotorControl, obj_idn, kSubCtrlVelocity), + ctrl_thrust_(kTypePropellerMotorControl, obj_idn, kSubCtrlThrust), + velocity_kp_(kTypePropellerMotorControl, obj_idn, kSubVelocityKp), + velocity_ki_(kTypePropellerMotorControl, obj_idn, kSubVelocityKi), + velocity_kd_(kTypePropellerMotorControl, obj_idn, kSubVelocityKd), + velocity_ff0_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF0), + velocity_ff1_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF1), + velocity_ff2_(kTypePropellerMotorControl, obj_idn, kSubVelocityFF2), + propeller_kt_pos_(kTypePropellerMotorControl, obj_idn, kSubPropellerKtPos), + propeller_kt_neg_(kTypePropellerMotorControl, obj_idn, kSubPropellerKtNeg), + timeout_(kTypePropellerMotorControl, obj_idn, kSubTimeout), + input_filter_fc_(kTypePropellerMotorControl, obj_idn, kSubInputFilterFc), + timeout_meaning_(kTypePropellerMotorControl, obj_idn, kSubTimeoutMeaning), + timeout_behavior_(kTypePropellerMotorControl, obj_idn, kSubTimeoutBehavior), + timeout_song_option_(kTypePropellerMotorControl, obj_idn, kSubTimeoutSongOption) {}; - // Client Entries - // Control commands - ClientEntry ctrl_mode_; - ClientEntryVoid ctrl_brake_; - ClientEntryVoid ctrl_coast_; - ClientEntry ctrl_pwm_; - ClientEntry ctrl_volts_; - ClientEntry ctrl_velocity_; - ClientEntry ctrl_thrust_; - // Velocity control - ClientEntry velocity_kp_; - ClientEntry velocity_ki_; - ClientEntry velocity_kd_; - ClientEntry velocity_ff0_; - ClientEntry velocity_ff1_; - ClientEntry velocity_ff2_; - // Propeller values - ClientEntry propeller_kt_pos_; - ClientEntry propeller_kt_neg_; - // Timeout - ClientEntry timeout_; - // Filter - ClientEntry input_filter_fc_; - ClientEntry timeout_meaning_; - ClientEntry timeout_behavior_; - ClientEntry timeout_song_option_; + // Client Entries + // Control commands + ClientEntry ctrl_mode_; + ClientEntryVoid ctrl_brake_; + ClientEntryVoid ctrl_coast_; + ClientEntry ctrl_pwm_; + ClientEntry ctrl_volts_; + ClientEntry ctrl_velocity_; + ClientEntry ctrl_thrust_; + // Velocity control + ClientEntry velocity_kp_; + ClientEntry velocity_ki_; + ClientEntry velocity_kd_; + ClientEntry velocity_ff0_; + ClientEntry velocity_ff1_; + ClientEntry velocity_ff2_; + // Propeller values + ClientEntry propeller_kt_pos_; + ClientEntry propeller_kt_neg_; + // Timeout + ClientEntry timeout_; + // Filter + ClientEntry input_filter_fc_; + ClientEntry timeout_meaning_; + ClientEntry timeout_behavior_; + ClientEntry timeout_song_option_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubTimeoutSongOption + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &ctrl_mode_, // 0 - &ctrl_brake_, // 1 - &ctrl_coast_, // 2 - &ctrl_pwm_, // 3 - &ctrl_volts_, // 4 - &ctrl_velocity_, // 5 - &ctrl_thrust_, // 6 - &velocity_kp_, // 7 - &velocity_ki_, // 8 - &velocity_kd_, // 9 - &velocity_ff0_, // 10 - &velocity_ff1_, // 11 - &velocity_ff2_, // 12 - &propeller_kt_pos_, // 13 - &propeller_kt_neg_, // 14 - &timeout_, // 15 - &input_filter_fc_, // 16 - &timeout_meaning_, // 17 - &timeout_behavior_, // 18 - &timeout_song_option_ // 19 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubTimeoutSongOption + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &ctrl_mode_, // 0 + &ctrl_brake_, // 1 + &ctrl_coast_, // 2 + &ctrl_pwm_, // 3 + &ctrl_volts_, // 4 + &ctrl_velocity_, // 5 + &ctrl_thrust_, // 6 + &velocity_kp_, // 7 + &velocity_ki_, // 8 + &velocity_kd_, // 9 + &velocity_ff0_, // 10 + &velocity_ff1_, // 11 + &velocity_ff2_, // 12 + &propeller_kt_pos_, // 13 + &propeller_kt_neg_, // 14 + &timeout_, // 15 + &input_filter_fc_, // 16 + &timeout_meaning_, // 17 + &timeout_behavior_, // 18 + &timeout_song_option_ // 19 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubCtrlMode = 0; - static const uint8_t kSubCtrlBrake = 1; - static const uint8_t kSubCtrlCoast = 2; - static const uint8_t kSubCtrlPwm = 3; - static const uint8_t kSubCtrlVolts = 4; - static const uint8_t kSubCtrlVelocity = 5; - static const uint8_t kSubCtrlThrust = 6; - static const uint8_t kSubVelocityKp = 7; - static const uint8_t kSubVelocityKi = 8; - static const uint8_t kSubVelocityKd = 9; - static const uint8_t kSubVelocityFF0 = 10; - static const uint8_t kSubVelocityFF1 = 11; - static const uint8_t kSubVelocityFF2 = 12; - static const uint8_t kSubPropellerKtPos = 13; - static const uint8_t kSubPropellerKtNeg = 14; - static const uint8_t kSubTimeout = 15; - static const uint8_t kSubInputFilterFc = 16; - static const uint8_t kSubTimeoutMeaning = 17; - static const uint8_t kSubTimeoutBehavior = 18; - static const uint8_t kSubTimeoutSongOption = 19; +private: + static const uint8_t kSubCtrlMode = 0; + static const uint8_t kSubCtrlBrake = 1; + static const uint8_t kSubCtrlCoast = 2; + static const uint8_t kSubCtrlPwm = 3; + static const uint8_t kSubCtrlVolts = 4; + static const uint8_t kSubCtrlVelocity = 5; + static const uint8_t kSubCtrlThrust = 6; + static const uint8_t kSubVelocityKp = 7; + static const uint8_t kSubVelocityKi = 8; + static const uint8_t kSubVelocityKd = 9; + static const uint8_t kSubVelocityFF0 = 10; + static const uint8_t kSubVelocityFF1 = 11; + static const uint8_t kSubVelocityFF2 = 12; + static const uint8_t kSubPropellerKtPos = 13; + static const uint8_t kSubPropellerKtNeg = 14; + static const uint8_t kSubTimeout = 15; + static const uint8_t kSubInputFilterFc = 16; + static const uint8_t kSubTimeoutMeaning = 17; + static const uint8_t kSubTimeoutBehavior = 18; + static const uint8_t kSubTimeoutSongOption = 19; }; #endif /* PROPELLER_MOTOR_CONTROL_CLIENT_HPP_ */ diff --git a/inc/pulsing_rectangular_input_parser_client.hpp b/inc/pulsing_rectangular_input_parser_client.hpp index f30876d..cf30000 100644 --- a/inc/pulsing_rectangular_input_parser_client.hpp +++ b/inc/pulsing_rectangular_input_parser_client.hpp @@ -20,29 +20,31 @@ const uint8_t kTypePulsingRectangularInputParser = 89; -class PulsingRectangularInputParserClient : public ClientAbstract { - public: - PulsingRectangularInputParserClient(uint8_t obj_idn) - : ClientAbstract(kTypePulsingRectangularInputParser, obj_idn), - pulsing_voltage_mode_(kTypePulsingRectangularInputParser, obj_idn, kSubPulsingVoltageMode), - pulsing_voltage_limit_(kTypePulsingRectangularInputParser, obj_idn, kSubPulsingVoltageLimit){}; - - // Client Entries - ClientEntry pulsing_voltage_mode_; - ClientEntry pulsing_voltage_limit_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubPulsingVoltageLimit + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &pulsing_voltage_mode_, // 0 - &pulsing_voltage_limit_ // 1 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubPulsingVoltageMode = 0; - static const uint8_t kSubPulsingVoltageLimit = 1; +class PulsingRectangularInputParserClient : public ClientAbstract +{ +public: + PulsingRectangularInputParserClient(uint8_t obj_idn) + : ClientAbstract(kTypePulsingRectangularInputParser, obj_idn), + pulsing_voltage_mode_(kTypePulsingRectangularInputParser, obj_idn, kSubPulsingVoltageMode), + pulsing_voltage_limit_(kTypePulsingRectangularInputParser, obj_idn, kSubPulsingVoltageLimit) {}; + + // Client Entries + ClientEntry pulsing_voltage_mode_; + ClientEntry pulsing_voltage_limit_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubPulsingVoltageLimit + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &pulsing_voltage_mode_, // 0 + &pulsing_voltage_limit_ // 1 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubPulsingVoltageMode = 0; + static const uint8_t kSubPulsingVoltageLimit = 1; }; #endif /* PULSING_RECTANGULAR_INPUT_PARSER_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/pwm_interface_client.hpp b/inc/pwm_interface_client.hpp index 4310335..478c26b 100644 --- a/inc/pwm_interface_client.hpp +++ b/inc/pwm_interface_client.hpp @@ -20,33 +20,35 @@ const uint8_t kTypePwmInterface = 92; -class PwmInterfaceClient : public ClientAbstract { - public: - PwmInterfaceClient(uint8_t obj_idn) - : ClientAbstract(kTypePwmInterface, obj_idn), - pwm_frequency_(kTypePwmInterface, obj_idn, kSubPwmFrequency), - duty_cycle_(kTypePwmInterface, obj_idn, kSubDutyCycle), - pwm_mode_(kTypePwmInterface, obj_idn, kSubPwmMode){}; - - // Client Entries - ClientEntry pwm_frequency_; - ClientEntry duty_cycle_; - ClientEntry pwm_mode_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubPwmMode + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &pwm_frequency_, // 0 - &duty_cycle_, // 1 - &pwm_mode_ // 2 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubPwmFrequency = 0; - static const uint8_t kSubDutyCycle = 1; - static const uint8_t kSubPwmMode = 2; +class PwmInterfaceClient : public ClientAbstract +{ +public: + PwmInterfaceClient(uint8_t obj_idn) + : ClientAbstract(kTypePwmInterface, obj_idn), + pwm_frequency_(kTypePwmInterface, obj_idn, kSubPwmFrequency), + duty_cycle_(kTypePwmInterface, obj_idn, kSubDutyCycle), + pwm_mode_(kTypePwmInterface, obj_idn, kSubPwmMode) {}; + + // Client Entries + ClientEntry pwm_frequency_; + ClientEntry duty_cycle_; + ClientEntry pwm_mode_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubPwmMode + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &pwm_frequency_, // 0 + &duty_cycle_, // 1 + &pwm_mode_ // 2 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubPwmFrequency = 0; + static const uint8_t kSubDutyCycle = 1; + static const uint8_t kSubPwmMode = 2; }; #endif /* PWM_INTERFACE_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/serial_interface_client.hpp b/inc/serial_interface_client.hpp index 7fa6401..4eaa39b 100644 --- a/inc/serial_interface_client.hpp +++ b/inc/serial_interface_client.hpp @@ -20,28 +20,29 @@ const uint8_t kTypeSerialInterface = 16; -class SerialInterfaceClient: public ClientAbstract{ - public: - SerialInterfaceClient(uint8_t obj_idn): - ClientAbstract(kTypeSerialInterface, obj_idn), - baud_rate_( kTypeSerialInterface, obj_idn, kSubBaudRate) - {}; - - // Client Entries - ClientEntry baud_rate_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubBaudRate+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &baud_rate_, // 0 - }; - - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubBaudRate = 0; +class SerialInterfaceClient: public ClientAbstract +{ +public: + SerialInterfaceClient(uint8_t obj_idn): + ClientAbstract(kTypeSerialInterface, obj_idn), + baud_rate_(kTypeSerialInterface, obj_idn, kSubBaudRate) + {}; + + // Client Entries + ClientEntry baud_rate_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubBaudRate + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &baud_rate_, // 0 + }; + + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubBaudRate = 0; }; #endif // SERIAL_INTERFACE_CLIENT_H diff --git a/inc/servo_input_parser_client.hpp b/inc/servo_input_parser_client.hpp index e139ed4..9cd8180 100644 --- a/inc/servo_input_parser_client.hpp +++ b/inc/servo_input_parser_client.hpp @@ -20,37 +20,38 @@ const uint8_t kTypeServoInputParser = 78; -class ServoInputParserClient: public ClientAbstract{ - public: - ServoInputParserClient(uint8_t obj_idn): - ClientAbstract( kTypeServoInputParser, obj_idn), - mode_( kTypeServoInputParser, obj_idn, kSubMode), - unit_min_( kTypeServoInputParser, obj_idn, kSubUnitMin), - unit_max_( kTypeServoInputParser, obj_idn, kSubUnitMax) - {}; - - // Client Entries - // Control commands - ClientEntry mode_; - ClientEntry unit_min_; - ClientEntry unit_max_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubUnitMax+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &mode_, // 0 - &unit_min_, // 1 - &unit_max_ // 2 - }; - - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubMode = 0; - static const uint8_t kSubUnitMin = 1; - static const uint8_t kSubUnitMax = 2; +class ServoInputParserClient: public ClientAbstract +{ +public: + ServoInputParserClient(uint8_t obj_idn): + ClientAbstract(kTypeServoInputParser, obj_idn), + mode_(kTypeServoInputParser, obj_idn, kSubMode), + unit_min_(kTypeServoInputParser, obj_idn, kSubUnitMin), + unit_max_(kTypeServoInputParser, obj_idn, kSubUnitMax) + {}; + + // Client Entries + // Control commands + ClientEntry mode_; + ClientEntry unit_min_; + ClientEntry unit_max_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubUnitMax + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &mode_, // 0 + &unit_min_, // 1 + &unit_max_ // 2 + }; + + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubMode = 0; + static const uint8_t kSubUnitMin = 1; + static const uint8_t kSubUnitMax = 2; }; #endif /* SERVO_INPUT_PARSER_CLIENT_HPP_ */ diff --git a/inc/step_direction_input_client.hpp b/inc/step_direction_input_client.hpp index 22f3df3..749757b 100644 --- a/inc/step_direction_input_client.hpp +++ b/inc/step_direction_input_client.hpp @@ -21,32 +21,33 @@ const uint8_t kTypeStepDirInput = 58; -class StepDirectionInputClient: public ClientAbstract{ - public: - StepDirectionInputClient(uint8_t obj_idn): - ClientAbstract( kTypeStepDirInput, obj_idn), - angle_( kTypeStepDirInput, obj_idn, kSubAngle), - angle_step_( kTypeStepDirInput, obj_idn, kSubAngleStep) - {}; - - // Client Entries - ClientEntry angle_; - ClientEntry angle_step_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubAngleStep+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &angle_, // 0 - &angle_step_ // 1 - }; - - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubAngle = 0; - static const uint8_t kSubAngleStep = 1; +class StepDirectionInputClient: public ClientAbstract +{ +public: + StepDirectionInputClient(uint8_t obj_idn): + ClientAbstract(kTypeStepDirInput, obj_idn), + angle_(kTypeStepDirInput, obj_idn, kSubAngle), + angle_step_(kTypeStepDirInput, obj_idn, kSubAngleStep) + {}; + + // Client Entries + ClientEntry angle_; + ClientEntry angle_step_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubAngleStep + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &angle_, // 0 + &angle_step_ // 1 + }; + + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubAngle = 0; + static const uint8_t kSubAngleStep = 1; }; #endif /* STEP_DIRECTION_INPUT_CLIENT_HPP_ */ diff --git a/inc/stopping_handler_client.hpp b/inc/stopping_handler_client.hpp index 13d4bcc..ee9aeb1 100644 --- a/inc/stopping_handler_client.hpp +++ b/inc/stopping_handler_client.hpp @@ -20,29 +20,31 @@ const uint8_t kTypeStoppingHandler = 87; -class StoppingHandlerClient : public ClientAbstract { - public: - StoppingHandlerClient(uint8_t obj_idn) - : ClientAbstract(kTypeStoppingHandler, obj_idn), - stopped_speed_(kTypeStoppingHandler, obj_idn, kSubStoppedSpeed), - stopped_time_(kTypeStoppingHandler, obj_idn, kSubStoppedTime){}; - - // Client Entries - ClientEntry stopped_speed_; - ClientEntry stopped_time_; - - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubStoppedTime + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &stopped_speed_, // 0 - &stopped_time_ // 1 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } - - private: - static const uint8_t kSubStoppedSpeed = 0; - static const uint8_t kSubStoppedTime = 1; +class StoppingHandlerClient : public ClientAbstract +{ +public: + StoppingHandlerClient(uint8_t obj_idn) + : ClientAbstract(kTypeStoppingHandler, obj_idn), + stopped_speed_(kTypeStoppingHandler, obj_idn, kSubStoppedSpeed), + stopped_time_(kTypeStoppingHandler, obj_idn, kSubStoppedTime) {}; + + // Client Entries + ClientEntry stopped_speed_; + ClientEntry stopped_time_; + + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubStoppedTime + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &stopped_speed_, // 0 + &stopped_time_ // 1 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } + +private: + static const uint8_t kSubStoppedSpeed = 0; + static const uint8_t kSubStoppedTime = 1; }; #endif /* STOPPING_HANDLER_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/stow_user_interface_client.hpp b/inc/stow_user_interface_client.hpp index d12ffd6..a887baf 100644 --- a/inc/stow_user_interface_client.hpp +++ b/inc/stow_user_interface_client.hpp @@ -20,65 +20,67 @@ const uint8_t kTypeStowUserInterface = 85; -class StowUserInterfaceClient : public ClientAbstract { - public: - StowUserInterfaceClient(uint8_t obj_idn) - : ClientAbstract(kTypeStowUserInterface, obj_idn), - zero_angle_(kTypeStowUserInterface, obj_idn, kSubZeroAngle), - target_angle_(kTypeStowUserInterface, obj_idn, kSubTargetAngle), - target_acceleration_(kTypeStowUserInterface, obj_idn, kSubTargetAcceleration), - sample_zero_(kTypeStowUserInterface, obj_idn, kSubSampleZero), - user_stow_request_(kTypeStowUserInterface, obj_idn, kSubUserStowRequest), - stow_kp_(kTypeStowUserInterface, obj_idn, kSubStowKp), - stow_ki_(kTypeStowUserInterface, obj_idn, kSubStowKi), - stow_kd_(kTypeStowUserInterface, obj_idn, kSubStowKd), - hold_stow_(kTypeStowUserInterface, obj_idn, kSubHoldStow), - stow_status_(kTypeStowUserInterface, obj_idn, kSubStowStatus), - stow_result_(kTypeStowUserInterface, obj_idn, kSubStowResult){}; +class StowUserInterfaceClient : public ClientAbstract +{ +public: + StowUserInterfaceClient(uint8_t obj_idn) + : ClientAbstract(kTypeStowUserInterface, obj_idn), + zero_angle_(kTypeStowUserInterface, obj_idn, kSubZeroAngle), + target_angle_(kTypeStowUserInterface, obj_idn, kSubTargetAngle), + target_acceleration_(kTypeStowUserInterface, obj_idn, kSubTargetAcceleration), + sample_zero_(kTypeStowUserInterface, obj_idn, kSubSampleZero), + user_stow_request_(kTypeStowUserInterface, obj_idn, kSubUserStowRequest), + stow_kp_(kTypeStowUserInterface, obj_idn, kSubStowKp), + stow_ki_(kTypeStowUserInterface, obj_idn, kSubStowKi), + stow_kd_(kTypeStowUserInterface, obj_idn, kSubStowKd), + hold_stow_(kTypeStowUserInterface, obj_idn, kSubHoldStow), + stow_status_(kTypeStowUserInterface, obj_idn, kSubStowStatus), + stow_result_(kTypeStowUserInterface, obj_idn, kSubStowResult) {}; - // Client Entries - ClientEntry zero_angle_; - ClientEntry target_angle_; - ClientEntry target_acceleration_; - ClientEntryVoid sample_zero_; - ClientEntryVoid user_stow_request_; - ClientEntry stow_kp_; - ClientEntry stow_ki_; - ClientEntry stow_kd_; - ClientEntry hold_stow_; - ClientEntry stow_status_; - ClientEntry stow_result_; + // Client Entries + ClientEntry zero_angle_; + ClientEntry target_angle_; + ClientEntry target_acceleration_; + ClientEntryVoid sample_zero_; + ClientEntryVoid user_stow_request_; + ClientEntry stow_kp_; + ClientEntry stow_ki_; + ClientEntry stow_kd_; + ClientEntry hold_stow_; + ClientEntry stow_status_; + ClientEntry stow_result_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubStowResult + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &zero_angle_, // 0 - &target_angle_, // 1 - &target_acceleration_, // 2 - &sample_zero_, // 3 - &user_stow_request_, // 4 - &stow_kp_, // 5 - &stow_ki_, // 6 - &stow_kd_, // 7 - &hold_stow_, // 8 - &stow_status_, // 9 - &stow_result_ // 10 - }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubStowResult + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &zero_angle_, // 0 + &target_angle_, // 1 + &target_acceleration_, // 2 + &sample_zero_, // 3 + &user_stow_request_, // 4 + &stow_kp_, // 5 + &stow_ki_, // 6 + &stow_kd_, // 7 + &hold_stow_, // 8 + &stow_status_, // 9 + &stow_result_ // 10 + }; + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubZeroAngle = 0; - static const uint8_t kSubTargetAngle = 1; - static const uint8_t kSubTargetAcceleration = 2; - static const uint8_t kSubSampleZero = 3; - static const uint8_t kSubUserStowRequest = 4; - static const uint8_t kSubStowKp = 5; - static const uint8_t kSubStowKi = 6; - static const uint8_t kSubStowKd = 7; - static const uint8_t kSubHoldStow = 8; - static const uint8_t kSubStowStatus = 9; - static const uint8_t kSubStowResult = 10; +private: + static const uint8_t kSubZeroAngle = 0; + static const uint8_t kSubTargetAngle = 1; + static const uint8_t kSubTargetAcceleration = 2; + static const uint8_t kSubSampleZero = 3; + static const uint8_t kSubUserStowRequest = 4; + static const uint8_t kSubStowKp = 5; + static const uint8_t kSubStowKi = 6; + static const uint8_t kSubStowKd = 7; + static const uint8_t kSubHoldStow = 8; + static const uint8_t kSubStowStatus = 9; + static const uint8_t kSubStowResult = 10; }; #endif /* STOW_USER_INTERFACE_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/system_control_client.hpp b/inc/system_control_client.hpp index 1c28db0..2bf5ee7 100644 --- a/inc/system_control_client.hpp +++ b/inc/system_control_client.hpp @@ -20,126 +20,128 @@ const uint8_t kTypeSystemControl = 5; -class SystemControlClient : public ClientAbstract { - public: - SystemControlClient(uint8_t obj_idn) - : ClientAbstract(kTypeSystemControl, obj_idn), - reboot_program_(kTypeSystemControl, obj_idn, kSubRebootProgram), - reboot_boot_loader_(kTypeSystemControl, obj_idn, kSubRebootBootLoader), - dev_id_(kTypeSystemControl, obj_idn, kSubDevId), - rev_id_(kTypeSystemControl, obj_idn, kSubRevId), - uid1_(kTypeSystemControl, obj_idn, kSubUid1), - uid2_(kTypeSystemControl, obj_idn, kSubUid2), - uid3_(kTypeSystemControl, obj_idn, kSubUid3), - mem_size_(kTypeSystemControl, obj_idn, kSubMemSize), - build_year_(kTypeSystemControl, obj_idn, kSubBuildYear), - build_month_(kTypeSystemControl, obj_idn, kSubBuildMonth), - build_day_(kTypeSystemControl, obj_idn, kSubBuildDay), - build_hour_(kTypeSystemControl, obj_idn, kSubBuildHour), - build_minute_(kTypeSystemControl, obj_idn, kSubBuildMinuite), - build_second_(kTypeSystemControl, obj_idn, kSubBuildSecond), - module_id_(kTypeSystemControl, obj_idn, kSubModuleId), - time_(kTypeSystemControl, obj_idn, kSubTime), - firmware_version_(kTypeSystemControl, obj_idn, kSubFirmwareVersion), - hardware_version_(kTypeSystemControl, obj_idn, kSubHardwareVersion), - electronics_version_(kTypeSystemControl, obj_idn, kSubElectronicsVersion), - firmware_valid_(kTypeSystemControl, obj_idn, kSubFirmwareValid), - applications_present_(kTypeSystemControl, obj_idn_, kSubApplicationsPresent), - bootloader_version_(kTypeSystemControl, obj_idn_, kSubApplicationsPresent), - upgrade_version_(kTypeSystemControl, obj_idn_, kSubUpgradeVersion), - system_clock_(kTypeSystemControl, obj_idn_, kSubSystemClock), - control_flags_(kTypeSystemControl, obj_idn_, kSubControlFlags), - pcb_version_(kTypeSystemControl, obj_idn_, kSubPcbVersion){}; +class SystemControlClient : public ClientAbstract +{ +public: + SystemControlClient(uint8_t obj_idn) + : ClientAbstract(kTypeSystemControl, obj_idn), + reboot_program_(kTypeSystemControl, obj_idn, kSubRebootProgram), + reboot_boot_loader_(kTypeSystemControl, obj_idn, kSubRebootBootLoader), + dev_id_(kTypeSystemControl, obj_idn, kSubDevId), + rev_id_(kTypeSystemControl, obj_idn, kSubRevId), + uid1_(kTypeSystemControl, obj_idn, kSubUid1), + uid2_(kTypeSystemControl, obj_idn, kSubUid2), + uid3_(kTypeSystemControl, obj_idn, kSubUid3), + mem_size_(kTypeSystemControl, obj_idn, kSubMemSize), + build_year_(kTypeSystemControl, obj_idn, kSubBuildYear), + build_month_(kTypeSystemControl, obj_idn, kSubBuildMonth), + build_day_(kTypeSystemControl, obj_idn, kSubBuildDay), + build_hour_(kTypeSystemControl, obj_idn, kSubBuildHour), + build_minute_(kTypeSystemControl, obj_idn, kSubBuildMinuite), + build_second_(kTypeSystemControl, obj_idn, kSubBuildSecond), + module_id_(kTypeSystemControl, obj_idn, kSubModuleId), + time_(kTypeSystemControl, obj_idn, kSubTime), + firmware_version_(kTypeSystemControl, obj_idn, kSubFirmwareVersion), + hardware_version_(kTypeSystemControl, obj_idn, kSubHardwareVersion), + electronics_version_(kTypeSystemControl, obj_idn, kSubElectronicsVersion), + firmware_valid_(kTypeSystemControl, obj_idn, kSubFirmwareValid), + applications_present_(kTypeSystemControl, obj_idn_, kSubApplicationsPresent), + bootloader_version_(kTypeSystemControl, obj_idn_, kSubApplicationsPresent), + upgrade_version_(kTypeSystemControl, obj_idn_, kSubUpgradeVersion), + system_clock_(kTypeSystemControl, obj_idn_, kSubSystemClock), + control_flags_(kTypeSystemControl, obj_idn_, kSubControlFlags), + pcb_version_(kTypeSystemControl, obj_idn_, kSubPcbVersion) {}; - // Client Entries - ClientEntryVoid reboot_program_; - ClientEntryVoid reboot_boot_loader_; - ClientEntry dev_id_; - ClientEntry rev_id_; - ClientEntry uid1_; - ClientEntry uid2_; - ClientEntry uid3_; - ClientEntry mem_size_; - ClientEntry build_year_; - ClientEntry build_month_; - ClientEntry build_day_; - ClientEntry build_hour_; - ClientEntry build_minute_; - ClientEntry build_second_; - ClientEntry module_id_; - ClientEntry time_; - ClientEntry firmware_version_; - ClientEntry hardware_version_; - ClientEntry electronics_version_; - ClientEntry firmware_valid_; - ClientEntry applications_present_; - ClientEntry bootloader_version_; - ClientEntry upgrade_version_; - ClientEntry system_clock_; - ClientEntry control_flags_; - ClientEntry pcb_version_; + // Client Entries + ClientEntryVoid reboot_program_; + ClientEntryVoid reboot_boot_loader_; + ClientEntry dev_id_; + ClientEntry rev_id_; + ClientEntry uid1_; + ClientEntry uid2_; + ClientEntry uid3_; + ClientEntry mem_size_; + ClientEntry build_year_; + ClientEntry build_month_; + ClientEntry build_day_; + ClientEntry build_hour_; + ClientEntry build_minute_; + ClientEntry build_second_; + ClientEntry module_id_; + ClientEntry time_; + ClientEntry firmware_version_; + ClientEntry hardware_version_; + ClientEntry electronics_version_; + ClientEntry firmware_valid_; + ClientEntry applications_present_; + ClientEntry bootloader_version_; + ClientEntry upgrade_version_; + ClientEntry system_clock_; + ClientEntry control_flags_; + ClientEntry pcb_version_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubPcbVersion + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &reboot_program_, // 0 - &reboot_boot_loader_, // 1 - &dev_id_, // 2 - &rev_id_, // 3 - &uid1_, // 4 - &uid2_, // 5 - &uid3_, // 6 - &mem_size_, // 7 - &build_year_, // 8 - &build_month_, // 9 - &build_day_, // 10 - &build_hour_, // 11 - &build_minute_, // 12 - &build_second_, // 13 - &module_id_, // 14 - &time_, // 15 - &firmware_version_, // 16 - &hardware_version_, // 17 - &electronics_version_, // 18 - &firmware_valid_, // 19 - &applications_present_, // 20 - &bootloader_version_, // 21 - &upgrade_version_, // 22 - &system_clock_, // 23 - &control_flags_, // 24 - &pcb_version_ // 25 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubPcbVersion + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &reboot_program_, // 0 + &reboot_boot_loader_, // 1 + &dev_id_, // 2 + &rev_id_, // 3 + &uid1_, // 4 + &uid2_, // 5 + &uid3_, // 6 + &mem_size_, // 7 + &build_year_, // 8 + &build_month_, // 9 + &build_day_, // 10 + &build_hour_, // 11 + &build_minute_, // 12 + &build_second_, // 13 + &module_id_, // 14 + &time_, // 15 + &firmware_version_, // 16 + &hardware_version_, // 17 + &electronics_version_, // 18 + &firmware_valid_, // 19 + &applications_present_, // 20 + &bootloader_version_, // 21 + &upgrade_version_, // 22 + &system_clock_, // 23 + &control_flags_, // 24 + &pcb_version_ // 25 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubRebootProgram = 0; - static const uint8_t kSubRebootBootLoader = 1; - static const uint8_t kSubDevId = 2; - static const uint8_t kSubRevId = 3; - static const uint8_t kSubUid1 = 4; - static const uint8_t kSubUid2 = 5; - static const uint8_t kSubUid3 = 6; - static const uint8_t kSubMemSize = 7; - static const uint8_t kSubBuildYear = 8; - static const uint8_t kSubBuildMonth = 9; - static const uint8_t kSubBuildDay = 10; - static const uint8_t kSubBuildHour = 11; - static const uint8_t kSubBuildMinuite = 12; - static const uint8_t kSubBuildSecond = 13; - static const uint8_t kSubModuleId = 14; - static const uint8_t kSubTime = 15; - static const uint8_t kSubFirmwareVersion = 16; - static const uint8_t kSubHardwareVersion = 17; - static const uint8_t kSubElectronicsVersion = 18; - static const uint8_t kSubFirmwareValid = 19; - static const uint8_t kSubApplicationsPresent = 20; - static const uint8_t kSubBootloaderVersion = 21; - static const uint8_t kSubUpgradeVersion = 22; - static const uint8_t kSubSystemClock = 23; - static const uint8_t kSubControlFlags = 24; - static const uint8_t kSubPcbVersion = 25; +private: + static const uint8_t kSubRebootProgram = 0; + static const uint8_t kSubRebootBootLoader = 1; + static const uint8_t kSubDevId = 2; + static const uint8_t kSubRevId = 3; + static const uint8_t kSubUid1 = 4; + static const uint8_t kSubUid2 = 5; + static const uint8_t kSubUid3 = 6; + static const uint8_t kSubMemSize = 7; + static const uint8_t kSubBuildYear = 8; + static const uint8_t kSubBuildMonth = 9; + static const uint8_t kSubBuildDay = 10; + static const uint8_t kSubBuildHour = 11; + static const uint8_t kSubBuildMinuite = 12; + static const uint8_t kSubBuildSecond = 13; + static const uint8_t kSubModuleId = 14; + static const uint8_t kSubTime = 15; + static const uint8_t kSubFirmwareVersion = 16; + static const uint8_t kSubHardwareVersion = 17; + static const uint8_t kSubElectronicsVersion = 18; + static const uint8_t kSubFirmwareValid = 19; + static const uint8_t kSubApplicationsPresent = 20; + static const uint8_t kSubBootloaderVersion = 21; + static const uint8_t kSubUpgradeVersion = 22; + static const uint8_t kSubSystemClock = 23; + static const uint8_t kSubControlFlags = 24; + static const uint8_t kSubPcbVersion = 25; }; #endif // SYSTEM_CONTROL_CLIENT_H diff --git a/inc/temperature_estimator_client.hpp b/inc/temperature_estimator_client.hpp index bd95472..3bde5f9 100644 --- a/inc/temperature_estimator_client.hpp +++ b/inc/temperature_estimator_client.hpp @@ -21,49 +21,50 @@ const uint8_t kTypeTemperatureEstimatorClient = 77; -class TemperatureEstimatorClient: public ClientAbstract{ - public: - TemperatureEstimatorClient(uint8_t obj_idn): - ClientAbstract( kTypeTemperatureEstimatorClient, obj_idn), - temp_( kTypeTemperatureEstimatorClient, obj_idn, kSubTemp), - otw_( kTypeTemperatureEstimatorClient, obj_idn, kSubOtw), - otlo_( kTypeTemperatureEstimatorClient, obj_idn, kSubOtlo), - thermal_resistance_( kTypeTemperatureEstimatorClient, obj_idn, kSubThermalResistance), - thermal_capacitance_( kTypeTemperatureEstimatorClient, obj_idn, kSubThermalCapacitance), - derate_( kTypeTemperatureEstimatorClient, obj_idn, kSubDerate) - {}; +class TemperatureEstimatorClient: public ClientAbstract +{ +public: + TemperatureEstimatorClient(uint8_t obj_idn): + ClientAbstract(kTypeTemperatureEstimatorClient, obj_idn), + temp_(kTypeTemperatureEstimatorClient, obj_idn, kSubTemp), + otw_(kTypeTemperatureEstimatorClient, obj_idn, kSubOtw), + otlo_(kTypeTemperatureEstimatorClient, obj_idn, kSubOtlo), + thermal_resistance_(kTypeTemperatureEstimatorClient, obj_idn, kSubThermalResistance), + thermal_capacitance_(kTypeTemperatureEstimatorClient, obj_idn, kSubThermalCapacitance), + derate_(kTypeTemperatureEstimatorClient, obj_idn, kSubDerate) + {}; - // Client Entries - // Control commands - ClientEntry temp_; - ClientEntry otw_; - ClientEntry otlo_; - ClientEntry thermal_resistance_; - ClientEntry thermal_capacitance_; - ClientEntry derate_; + // Client Entries + // Control commands + ClientEntry temp_; + ClientEntry otw_; + ClientEntry otlo_; + ClientEntry thermal_resistance_; + ClientEntry thermal_capacitance_; + ClientEntry derate_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubDerate+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &temp_, // 0 - &otw_, // 1 - &otlo_, // 2 - &thermal_resistance_, // 3 - &thermal_capacitance_,// 4 - &derate_ // 5 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubDerate + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &temp_, // 0 + &otw_, // 1 + &otlo_, // 2 + &thermal_resistance_, // 3 + &thermal_capacitance_,// 4 + &derate_ // 5 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubTemp = 0; - static const uint8_t kSubOtw = 1; - static const uint8_t kSubOtlo = 2; - static const uint8_t kSubThermalResistance = 3; - static const uint8_t kSubThermalCapacitance = 4; - static const uint8_t kSubDerate = 5; +private: + static const uint8_t kSubTemp = 0; + static const uint8_t kSubOtw = 1; + static const uint8_t kSubOtlo = 2; + static const uint8_t kSubThermalResistance = 3; + static const uint8_t kSubThermalCapacitance = 4; + static const uint8_t kSubDerate = 5; }; #endif /* TEMPERATURE_ESTIMATOR_CLIENT_HPP_ */ diff --git a/inc/temperature_monitor_uc_client.hpp b/inc/temperature_monitor_uc_client.hpp index 050fbf4..eb271d3 100644 --- a/inc/temperature_monitor_uc_client.hpp +++ b/inc/temperature_monitor_uc_client.hpp @@ -20,49 +20,50 @@ const uint8_t kTypeTemperatureMonitorUcClient = 73; -class TemperatureMonitorUcClient: public ClientAbstract{ - public: - TemperatureMonitorUcClient(uint8_t obj_idn): - ClientAbstract( kTypeTemperatureMonitorUcClient, obj_idn), - uc_temp_( kTypeTemperatureMonitorUcClient, obj_idn, kSubUcTemp), - filter_fs_( kTypeTemperatureMonitorUcClient, obj_idn, kSubFilterFs), - filter_fc_( kTypeTemperatureMonitorUcClient, obj_idn, kSubFilterFc), - otw_( kTypeTemperatureMonitorUcClient, obj_idn, kSubOtw), - otlo_( kTypeTemperatureMonitorUcClient, obj_idn, kSubOtlo), - derate_( kTypeTemperatureMonitorUcClient, obj_idn, kSubDerate) - {}; +class TemperatureMonitorUcClient: public ClientAbstract +{ +public: + TemperatureMonitorUcClient(uint8_t obj_idn): + ClientAbstract(kTypeTemperatureMonitorUcClient, obj_idn), + uc_temp_(kTypeTemperatureMonitorUcClient, obj_idn, kSubUcTemp), + filter_fs_(kTypeTemperatureMonitorUcClient, obj_idn, kSubFilterFs), + filter_fc_(kTypeTemperatureMonitorUcClient, obj_idn, kSubFilterFc), + otw_(kTypeTemperatureMonitorUcClient, obj_idn, kSubOtw), + otlo_(kTypeTemperatureMonitorUcClient, obj_idn, kSubOtlo), + derate_(kTypeTemperatureMonitorUcClient, obj_idn, kSubDerate) + {}; - // Client Entries - // Control commands - ClientEntry uc_temp_; - ClientEntry filter_fs_; - ClientEntry filter_fc_; - ClientEntry otw_; - ClientEntry otlo_; - ClientEntry derate_; + // Client Entries + // Control commands + ClientEntry uc_temp_; + ClientEntry filter_fs_; + ClientEntry filter_fc_; + ClientEntry otw_; + ClientEntry otlo_; + ClientEntry derate_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) - { - static const uint8_t kEntryLength = kSubDerate+1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &uc_temp_, // 0 - &filter_fs_, // 1 - &filter_fc_, // 2 - &otw_, // 3 - &otlo_, // 4 - &derate_ // 5 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubDerate + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &uc_temp_, // 0 + &filter_fs_, // 1 + &filter_fc_, // 2 + &otw_, // 3 + &otlo_, // 4 + &derate_ // 5 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubUcTemp = 0; - static const uint8_t kSubFilterFs = 1; - static const uint8_t kSubFilterFc = 2; - static const uint8_t kSubOtw = 3; - static const uint8_t kSubOtlo = 4; - static const uint8_t kSubDerate = 5; +private: + static const uint8_t kSubUcTemp = 0; + static const uint8_t kSubFilterFs = 1; + static const uint8_t kSubFilterFc = 2; + static const uint8_t kSubOtw = 3; + static const uint8_t kSubOtlo = 4; + static const uint8_t kSubDerate = 5; }; #endif /* TEMPERATURE_MONITOR_UC_CLIENT_HPP_ */ diff --git a/inc/uavcan_node_client.hpp b/inc/uavcan_node_client.hpp index dd662f0..a28e44b 100644 --- a/inc/uavcan_node_client.hpp +++ b/inc/uavcan_node_client.hpp @@ -21,70 +21,72 @@ const uint8_t kTypeUavcanNode = 80; -class UavcanNodeClient : public ClientAbstract { - public: - UavcanNodeClient(uint8_t obj_idn) - : ClientAbstract(kTypeUavcanNode, obj_idn), - uavcan_node_id_(kTypeUavcanNode, obj_idn, kSubUavcanNodeId), - uavcan_esc_index_(kTypeUavcanNode, obj_idn, kSubUavcanEscIndex), - zero_behavior_(kTypeUavcanNode, obj_idn, kSubZeroBehavior), - last_error_code_(kTypeUavcanNode, obj_idn, kSubLastErrorCode), - receive_error_counter_(kTypeUavcanNode, obj_idn, kSubReceiveErrorCounter), - transmit_error_counter_(kTypeUavcanNode, obj_idn, kSubTransmitErrorCounter), - bus_off_flag_(kTypeUavcanNode, obj_idn, kSubBusOffFlag), - error_passive_flag_(kTypeUavcanNode, obj_idn, kSubErrorPassiveFlag), - error_warning_flag_(kTypeUavcanNode, obj_idn, kSubErrorWarningFlag), - telemetry_frequency_(kTypeUavcanNode, obj_idn, kSubTelemetryFrequency), - bit_rate_(kTypeUavcanNode, obj_idn, kSubBitRate), - bypass_arming_(kTypeUavcanNode, obj_idn, kSubBypassArming){}; +class UavcanNodeClient : public ClientAbstract +{ +public: + UavcanNodeClient(uint8_t obj_idn) + : ClientAbstract(kTypeUavcanNode, obj_idn), + uavcan_node_id_(kTypeUavcanNode, obj_idn, kSubUavcanNodeId), + uavcan_esc_index_(kTypeUavcanNode, obj_idn, kSubUavcanEscIndex), + zero_behavior_(kTypeUavcanNode, obj_idn, kSubZeroBehavior), + last_error_code_(kTypeUavcanNode, obj_idn, kSubLastErrorCode), + receive_error_counter_(kTypeUavcanNode, obj_idn, kSubReceiveErrorCounter), + transmit_error_counter_(kTypeUavcanNode, obj_idn, kSubTransmitErrorCounter), + bus_off_flag_(kTypeUavcanNode, obj_idn, kSubBusOffFlag), + error_passive_flag_(kTypeUavcanNode, obj_idn, kSubErrorPassiveFlag), + error_warning_flag_(kTypeUavcanNode, obj_idn, kSubErrorWarningFlag), + telemetry_frequency_(kTypeUavcanNode, obj_idn, kSubTelemetryFrequency), + bit_rate_(kTypeUavcanNode, obj_idn, kSubBitRate), + bypass_arming_(kTypeUavcanNode, obj_idn, kSubBypassArming) {}; - // Client Entries - ClientEntry uavcan_node_id_; - ClientEntry uavcan_esc_index_; - ClientEntry zero_behavior_; - ClientEntry last_error_code_; - ClientEntry receive_error_counter_; - ClientEntry transmit_error_counter_; - ClientEntry bus_off_flag_; - ClientEntry error_passive_flag_; - ClientEntry error_warning_flag_; - ClientEntry telemetry_frequency_; - ClientEntry bit_rate_; - ClientEntry bypass_arming_; + // Client Entries + ClientEntry uavcan_node_id_; + ClientEntry uavcan_esc_index_; + ClientEntry zero_behavior_; + ClientEntry last_error_code_; + ClientEntry receive_error_counter_; + ClientEntry transmit_error_counter_; + ClientEntry bus_off_flag_; + ClientEntry error_passive_flag_; + ClientEntry error_warning_flag_; + ClientEntry telemetry_frequency_; + ClientEntry bit_rate_; + ClientEntry bypass_arming_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) { - static const uint8_t kEntryLength = kSubBypassArming + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &uavcan_node_id_, // 0 - &uavcan_esc_index_, // 1 - &zero_behavior_, // 2 - &last_error_code_, // 3 - &receive_error_counter_, // 4 - &transmit_error_counter_, // 5 - &bus_off_flag_, // 6 - &error_passive_flag_, // 7 - &error_warning_flag_, // 8 - &telemetry_frequency_, // 9 - &bit_rate_, // 10 - &bypass_arming_ // 11 - }; + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) + { + static const uint8_t kEntryLength = kSubBypassArming + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &uavcan_node_id_, // 0 + &uavcan_esc_index_, // 1 + &zero_behavior_, // 2 + &last_error_code_, // 3 + &receive_error_counter_, // 4 + &transmit_error_counter_, // 5 + &bus_off_flag_, // 6 + &error_passive_flag_, // 7 + &error_warning_flag_, // 8 + &telemetry_frequency_, // 9 + &bit_rate_, // 10 + &bypass_arming_ // 11 + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubUavcanNodeId = 0; - static const uint8_t kSubUavcanEscIndex = 1; - static const uint8_t kSubZeroBehavior = 2; - static const uint8_t kSubLastErrorCode = 3; - static const uint8_t kSubReceiveErrorCounter = 4; - static const uint8_t kSubTransmitErrorCounter = 5; - static const uint8_t kSubBusOffFlag = 6; - static const uint8_t kSubErrorPassiveFlag = 7; - static const uint8_t kSubErrorWarningFlag = 8; - static const uint8_t kSubTelemetryFrequency = 9; - static const uint8_t kSubBitRate = 10; - static const uint8_t kSubBypassArming = 11; +private: + static const uint8_t kSubUavcanNodeId = 0; + static const uint8_t kSubUavcanEscIndex = 1; + static const uint8_t kSubZeroBehavior = 2; + static const uint8_t kSubLastErrorCode = 3; + static const uint8_t kSubReceiveErrorCounter = 4; + static const uint8_t kSubTransmitErrorCounter = 5; + static const uint8_t kSubBusOffFlag = 6; + static const uint8_t kSubErrorPassiveFlag = 7; + static const uint8_t kSubErrorWarningFlag = 8; + static const uint8_t kSubTelemetryFrequency = 9; + static const uint8_t kSubBitRate = 10; + static const uint8_t kSubBypassArming = 11; }; #endif /* UAVCAN_NODE_CLIENT_HPP_ */ \ No newline at end of file diff --git a/inc/voltage_superposition_client.hpp b/inc/voltage_superposition_client.hpp index f7a8eb0..0e40350 100644 --- a/inc/voltage_superposition_client.hpp +++ b/inc/voltage_superposition_client.hpp @@ -21,92 +21,94 @@ const uint8_t kTypeVoltageSuperposition = 74; -class VoltageSuperPositionClient : public ClientAbstract { - public: - VoltageSuperPositionClient(uint8_t obj_idn) - : ClientAbstract(kTypeVoltageSuperposition, obj_idn), - zero_angle_(kTypeVoltageSuperposition, obj_idn, kSubZeroAngle), - frequency_(kTypeVoltageSuperposition, obj_idn, kSubFrequency), - phase_(kTypeVoltageSuperposition, obj_idn, kSubPhase), - amplitude_(kTypeVoltageSuperposition, obj_idn, kSubAmplitude), - voltage_(kTypeVoltageSuperposition, obj_idn, kSubVoltage), - max_allowed_amplitude_(kTypeVoltageSuperposition, obj_idn, kSubMaxAllowedAmplitude), - velocity_cutoff_(kTypeVoltageSuperposition, obj_idn, kSubVelocityCutoff), - poly_limit_zero_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitZero), - poly_limit_one_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitOne), - poly_limit_two_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitTwo), - poly_limit_three_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitThree), - phase_lead_time_(kTypeVoltageSuperposition, obj_idn, kSubPhaseLeadTime), - phase_lead_angle_(kTypeVoltageSuperposition, obj_idn, kSubPhaseLeadAngle), - phase_act_(kTypeVoltageSuperposition, obj_idn, kSubPhaseActual), - amplitude_act_(kTypeVoltageSuperposition, obj_idn, kSubAmplitudeActual), - sample_mechanical_zero_(kTypeVoltageSuperposition, obj_idn, kSubSampleMechanicalZero), - propeller_torque_offset_angle_(kTypeVoltageSuperposition, obj_idn, - kSubPropellerTorqueOffsetAngle){}; +class VoltageSuperPositionClient : public ClientAbstract +{ +public: + VoltageSuperPositionClient(uint8_t obj_idn) + : ClientAbstract(kTypeVoltageSuperposition, obj_idn), + zero_angle_(kTypeVoltageSuperposition, obj_idn, kSubZeroAngle), + frequency_(kTypeVoltageSuperposition, obj_idn, kSubFrequency), + phase_(kTypeVoltageSuperposition, obj_idn, kSubPhase), + amplitude_(kTypeVoltageSuperposition, obj_idn, kSubAmplitude), + voltage_(kTypeVoltageSuperposition, obj_idn, kSubVoltage), + max_allowed_amplitude_(kTypeVoltageSuperposition, obj_idn, kSubMaxAllowedAmplitude), + velocity_cutoff_(kTypeVoltageSuperposition, obj_idn, kSubVelocityCutoff), + poly_limit_zero_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitZero), + poly_limit_one_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitOne), + poly_limit_two_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitTwo), + poly_limit_three_(kTypeVoltageSuperposition, obj_idn, kSubPolyLimitThree), + phase_lead_time_(kTypeVoltageSuperposition, obj_idn, kSubPhaseLeadTime), + phase_lead_angle_(kTypeVoltageSuperposition, obj_idn, kSubPhaseLeadAngle), + phase_act_(kTypeVoltageSuperposition, obj_idn, kSubPhaseActual), + amplitude_act_(kTypeVoltageSuperposition, obj_idn, kSubAmplitudeActual), + sample_mechanical_zero_(kTypeVoltageSuperposition, obj_idn, kSubSampleMechanicalZero), + propeller_torque_offset_angle_(kTypeVoltageSuperposition, obj_idn, + kSubPropellerTorqueOffsetAngle) {}; - // Client Entries - ClientEntry zero_angle_; - ClientEntry frequency_; - ClientEntry phase_; - ClientEntry amplitude_; - ClientEntry voltage_; - ClientEntry max_allowed_amplitude_; - ClientEntry velocity_cutoff_; - ClientEntry poly_limit_zero_; - ClientEntry poly_limit_one_; - ClientEntry poly_limit_two_; - ClientEntry poly_limit_three_; - ClientEntry phase_lead_time_; - ClientEntry phase_lead_angle_; - ClientEntry phase_act_; - ClientEntry amplitude_act_; - ClientEntryVoid sample_mechanical_zero_; - ClientEntry propeller_torque_offset_angle_; + // Client Entries + ClientEntry zero_angle_; + ClientEntry frequency_; + ClientEntry phase_; + ClientEntry amplitude_; + ClientEntry voltage_; + ClientEntry max_allowed_amplitude_; + ClientEntry velocity_cutoff_; + ClientEntry poly_limit_zero_; + ClientEntry poly_limit_one_; + ClientEntry poly_limit_two_; + ClientEntry poly_limit_three_; + ClientEntry phase_lead_time_; + ClientEntry phase_lead_angle_; + ClientEntry phase_act_; + ClientEntry amplitude_act_; + ClientEntryVoid sample_mechanical_zero_; + ClientEntry propeller_torque_offset_angle_; - void ReadMsg(uint8_t* rx_data, uint8_t rx_length) override { - static const uint8_t kEntryLength = kSubPropellerTorqueOffsetAngle + 1; - ClientEntryAbstract* entry_array[kEntryLength] = { - &zero_angle_, // 0 - &frequency_, // 1 - &phase_, // 2 - &litude_, // 3 - &voltage_, // 4 - &max_allowed_amplitude_, // 5 - &velocity_cutoff_, // 6 - &poly_limit_zero_, // 7 - &poly_limit_one_, // 8 - &poly_limit_two_, // 9 - &poly_limit_three_, // 10 - &phase_lead_time_, // 11 - &phase_lead_angle_, // 12 - &phase_act_, // 13 - &litude_act_, // 14 - &sample_mechanical_zero_, // 15 - &propeller_torque_offset_angle_ // 16 + void ReadMsg(uint8_t *rx_data, uint8_t rx_length) override + { + static const uint8_t kEntryLength = kSubPropellerTorqueOffsetAngle + 1; + ClientEntryAbstract *entry_array[kEntryLength] = { + &zero_angle_, // 0 + &frequency_, // 1 + &phase_, // 2 + &litude_, // 3 + &voltage_, // 4 + &max_allowed_amplitude_, // 5 + &velocity_cutoff_, // 6 + &poly_limit_zero_, // 7 + &poly_limit_one_, // 8 + &poly_limit_two_, // 9 + &poly_limit_three_, // 10 + &phase_lead_time_, // 11 + &phase_lead_angle_, // 12 + &phase_act_, // 13 + &litude_act_, // 14 + &sample_mechanical_zero_, // 15 + &propeller_torque_offset_angle_ // 16 - }; + }; - ParseMsg(rx_data, rx_length, entry_array, kEntryLength); - } + ParseMsg(rx_data, rx_length, entry_array, kEntryLength); + } - private: - static const uint8_t kSubZeroAngle = 0; - static const uint8_t kSubFrequency = 1; - static const uint8_t kSubPhase = 2; - static const uint8_t kSubAmplitude = 3; - static const uint8_t kSubVoltage = 4; - static const uint8_t kSubMaxAllowedAmplitude = 5; - static const uint8_t kSubVelocityCutoff = 6; - static const uint8_t kSubPolyLimitZero = 7; - static const uint8_t kSubPolyLimitOne = 8; - static const uint8_t kSubPolyLimitTwo = 9; - static const uint8_t kSubPolyLimitThree = 10; - static const uint8_t kSubPhaseLeadTime = 11; - static const uint8_t kSubPhaseLeadAngle = 12; - static const uint8_t kSubPhaseActual = 13; - static const uint8_t kSubAmplitudeActual = 14; - static const uint8_t kSubSampleMechanicalZero = 15; - static const uint8_t kSubPropellerTorqueOffsetAngle = 16; +private: + static const uint8_t kSubZeroAngle = 0; + static const uint8_t kSubFrequency = 1; + static const uint8_t kSubPhase = 2; + static const uint8_t kSubAmplitude = 3; + static const uint8_t kSubVoltage = 4; + static const uint8_t kSubMaxAllowedAmplitude = 5; + static const uint8_t kSubVelocityCutoff = 6; + static const uint8_t kSubPolyLimitZero = 7; + static const uint8_t kSubPolyLimitOne = 8; + static const uint8_t kSubPolyLimitTwo = 9; + static const uint8_t kSubPolyLimitThree = 10; + static const uint8_t kSubPhaseLeadTime = 11; + static const uint8_t kSubPhaseLeadAngle = 12; + static const uint8_t kSubPhaseActual = 13; + static const uint8_t kSubAmplitudeActual = 14; + static const uint8_t kSubSampleMechanicalZero = 15; + static const uint8_t kSubPropellerTorqueOffsetAngle = 16; }; #endif \ No newline at end of file diff --git a/src/byte_queue.c b/src/byte_queue.c index 39d67d4..0f8a808 100644 --- a/src/byte_queue.c +++ b/src/byte_queue.c @@ -19,109 +19,127 @@ #include "byte_queue.h" -void InitBQ(struct ByteQueue *p, uint8_t* data, uint16_t data_size) { - p->data = data; - p->data_size = data_size; - p->start = p->data; - p->end = p->data; - p->count = 0; +void InitBQ(struct ByteQueue *p, uint8_t *data, uint16_t data_size) +{ + p->data = data; + p->data_size = data_size; + p->start = p->data; + p->end = p->data; + p->count = 0; } -int8_t IsFullBQ(struct ByteQueue *p) { - // if p->end at end of fixed array - if (p->end == p->data + p->data_size - 1) { - if (p->start == p->data) { - return 1; // buffer full if p->start is at beginning of fixed array - } else { - return 0; // buffer not full - } - } - // otherwise p->end not at end of fixed array - else { - if (p->end == p->start - 1) { - return 1; // buffer full if p->end preceeds p->start by one - } else { - return 0; // buffer not full - } - } +int8_t IsFullBQ(struct ByteQueue *p) +{ + // if p->end at end of fixed array + if (p->end == p->data + p->data_size - 1) { + if (p->start == p->data) { + return 1; // buffer full if p->start is at beginning of fixed array + + } else { + return 0; // buffer not full + } + } + + // otherwise p->end not at end of fixed array + else { + if (p->end == p->start - 1) { + return 1; // buffer full if p->end preceeds p->start by one + + } else { + return 0; // buffer not full + } + } } -int8_t IsEmptyBQ(struct ByteQueue *p) { - if(p->start == p->end) - return 1; - else - return 0; +int8_t IsEmptyBQ(struct ByteQueue *p) +{ + if (p->start == p->end) { + return 1; + + } else { + return 0; + } } -uint16_t CountBQ(struct ByteQueue *p) { - return(p->count); +uint16_t CountBQ(struct ByteQueue *p) +{ + return (p->count); } -uint8_t GetByteBQ(struct ByteQueue *p) { - - // silent failure if empty - if(IsEmptyBQ(p)) { - return(0); - } - else { - // otherwise buffer contains data - // get data at start - uint8_t temp_char = *(p->start); - // if start is not at the end of the array, advance start - if (p->start != p->data + p->data_size - 1) { - p->start = p->start + 1; - } - // otherwise wrap start to the beginning of the array - else { - p->start = p->data; - } - p->count--; - return temp_char; - } +uint8_t GetByteBQ(struct ByteQueue *p) +{ + + // silent failure if empty + if (IsEmptyBQ(p)) { + return (0); + + } else { + // otherwise buffer contains data + // get data at start + uint8_t temp_char = *(p->start); + + // if start is not at the end of the array, advance start + if (p->start != p->data + p->data_size - 1) { + p->start = p->start + 1; + } + + // otherwise wrap start to the beginning of the array + else { + p->start = p->data; + } + + p->count--; + return temp_char; + } } -uint8_t PeekByteBQ(struct ByteQueue *p) { - // silent failure if empty - if(IsEmptyBQ(p)) { - return(0); - } - else { - return(*(p->start)); - } +uint8_t PeekByteBQ(struct ByteQueue *p) +{ + // silent failure if empty + if (IsEmptyBQ(p)) { + return (0); + + } else { + return (*(p->start)); + } } -int8_t PutByteBQ(struct ByteQueue *p, uint8_t item) { - // if p->end points to last element of array - if (p->end == p->data + p->data_size - 1) - { - // if start is at the beginning of the array, the buffer is full - if (p->start == p->data) { - return 0; // failure, buffer is full - } - // otherwise stuff item at p->end - else { - *(p->end) = item; // stuff item at p->end - p->end = p->data; // move p->end to start of array - } - } - // otherwise p->end does not point to last element of array - else { - // if p->end preceeds p->start by one, buffer is full - if (p->end == (p->start - 1)) { - return 0; // failure, buffer is full - } - // otherwise stuff item at p->end - else { - *(p->end) = item; // stuff item at p->end - p->end = p->end + 1; // advance p->end one - } - } - p->count++; - return 1; // success +int8_t PutByteBQ(struct ByteQueue *p, uint8_t item) +{ + // if p->end points to last element of array + if (p->end == p->data + p->data_size - 1) { + // if start is at the beginning of the array, the buffer is full + if (p->start == p->data) { + return 0; // failure, buffer is full + } + + // otherwise stuff item at p->end + else { + *(p->end) = item; // stuff item at p->end + p->end = p->data; // move p->end to start of array + } + } + + // otherwise p->end does not point to last element of array + else { + // if p->end preceeds p->start by one, buffer is full + if (p->end == (p->start - 1)) { + return 0; // failure, buffer is full + } + + // otherwise stuff item at p->end + else { + *(p->end) = item; // stuff item at p->end + p->end = p->end + 1; // advance p->end one + } + } + + p->count++; + return 1; // success } diff --git a/src/crc_helper.c b/src/crc_helper.c index ed00088..bfabec5 100644 --- a/src/crc_helper.c +++ b/src/crc_helper.c @@ -15,33 +15,40 @@ #include "crc_helper.h" // Compute CRC word for a byte string. -uint16_t MakeCrc(const uint8_t *data, uint16_t count) { +uint16_t MakeCrc(const uint8_t *data, uint16_t count) +{ - uint16_t crc = 0xffff; + uint16_t crc = 0xffff; - uint16_t i; - for(i = 0; i < count; i++) { - crc = ByteUpdateCrc(crc, data[i]); - } - return crc; + uint16_t i; + + for (i = 0; i < count; i++) { + crc = ByteUpdateCrc(crc, data[i]); + } + + return crc; } // Update a CRC accumulation with one data byte. -uint16_t ByteUpdateCrc(uint16_t crc, uint8_t data) { +uint16_t ByteUpdateCrc(uint16_t crc, uint8_t data) +{ - uint16_t x = (crc >> 8) ^ data; - x ^= x >> 4; + uint16_t x = (crc >> 8) ^ data; + x ^= x >> 4; - crc = (crc << 8) ^ (x << 12) ^ (x <<5) ^ x; - return crc; + crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x; + return crc; } // Update a CRC accumulation with several data bytes. -uint16_t ArrayUpdateCrc(uint16_t crc, const uint8_t *data, uint16_t count) { +uint16_t ArrayUpdateCrc(uint16_t crc, const uint8_t *data, uint16_t count) +{ + + uint16_t i; + + for (i = 0; i < count; i++) { + crc = ByteUpdateCrc(crc, data[i]); + } - uint16_t i; - for(i = 0; i < count; i++) { - crc = ByteUpdateCrc(crc, data[i]); - } - return crc; + return crc; } diff --git a/src/generic_interface.cpp b/src/generic_interface.cpp index c31fcba..91c2ce4 100644 --- a/src/generic_interface.cpp +++ b/src/generic_interface.cpp @@ -19,121 +19,121 @@ GenericInterface::GenericInterface() { - InitBQ(&index_queue, pf_index_data, GENERIC_PF_INDEX_DATA_SIZE); - InitPacketFinder(&pf, &index_queue); - tx_bipbuf = BipBuffer(tx_buffer, GENERIC_TX_BUFFER_SIZE); + InitBQ(&index_queue, pf_index_data, GENERIC_PF_INDEX_DATA_SIZE); + InitPacketFinder(&pf, &index_queue); + tx_bipbuf = BipBuffer(tx_buffer, GENERIC_TX_BUFFER_SIZE); } int8_t GenericInterface::GetBytes() { - // I can't do anything on my own since I don't have hardware - // Use SetRxBytes(uint8_t* data_in, uint16_t length_in) - return 0; + // I can't do anything on my own since I don't have hardware + // Use SetRxBytes(uint8_t* data_in, uint16_t length_in) + return 0; } -int8_t GenericInterface::SetRxBytes(uint8_t* data_in, uint16_t length_in) +int8_t GenericInterface::SetRxBytes(uint8_t *data_in, uint16_t length_in) { - if(data_in == nullptr) - return -1; - - if(length_in) - { - //copy it over - PutBytes(&pf, data_in, length_in); - return 1; - } - else - return 0; + if (data_in == nullptr) { + return -1; + } + + if (length_in) { + //copy it over + PutBytes(&pf, data_in, length_in); + return 1; + + } else { + return 0; + } } int8_t GenericInterface::PeekPacket(uint8_t **packet, uint8_t *length) { - return(::PeekPacket(&pf, packet, length)); + return (::PeekPacket(&pf, packet, length)); } int8_t GenericInterface::DropPacket() { - return(::DropPacket(&pf)); + return (::DropPacket(&pf)); } int8_t GenericInterface::SendPacket(uint8_t msg_type, uint8_t *data, uint16_t length) { - // This function must not be interrupted by another call to SendBytes or - // SendPacket, or else the packet it builds will be spliced/corrupted. - - uint8_t header[3]; - header[0] = kStartByte; // const defined by packet_finder.c - header[1] = length; - header[2] = msg_type; - SendBytes(header, 3); - - SendBytes(data, length); - - uint8_t footer[2]; - uint16_t crc; - crc = MakeCrc(&(header[1]), 2); - crc = ArrayUpdateCrc(crc, data, length); - footer[0] = crc & 0x00FF; - footer[1] = crc >> 8; - SendBytes(footer, 2); - - return(1); + // This function must not be interrupted by another call to SendBytes or + // SendPacket, or else the packet it builds will be spliced/corrupted. + + uint8_t header[3]; + header[0] = kStartByte; // const defined by packet_finder.c + header[1] = length; + header[2] = msg_type; + SendBytes(header, 3); + + SendBytes(data, length); + + uint8_t footer[2]; + uint16_t crc; + crc = MakeCrc(&(header[1]), 2); + crc = ArrayUpdateCrc(crc, data, length); + footer[0] = crc & 0x00FF; + footer[1] = crc >> 8; + SendBytes(footer, 2); + + return (1); } int8_t GenericInterface::SendBytes(uint8_t *bytes, uint16_t length) { - uint16_t length_temp = 0; - uint8_t* location_temp; - int8_t ret = 0; - - // Reserve space in the buffer - location_temp = tx_bipbuf.Reserve(length, length_temp); - - // If there's room, do the copy - if(length == length_temp) - { - memcpy(location_temp, bytes, length_temp); // do copy - tx_bipbuf.Commit(length_temp); - ret = 1; - } - else - { - tx_bipbuf.Commit(0); // Call the restaurant, cancel the reservations - } - - return ret; + uint16_t length_temp = 0; + uint8_t *location_temp; + int8_t ret = 0; + + // Reserve space in the buffer + location_temp = tx_bipbuf.Reserve(length, length_temp); + + // If there's room, do the copy + if (length == length_temp) { + memcpy(location_temp, bytes, length_temp); // do copy + tx_bipbuf.Commit(length_temp); + ret = 1; + + } else { + tx_bipbuf.Commit(0); // Call the restaurant, cancel the reservations + } + + return ret; } -int8_t GenericInterface::GetTxBytes(uint8_t* data_out, uint8_t& length_out) +int8_t GenericInterface::GetTxBytes(uint8_t *data_out, uint8_t &length_out) { - uint16_t length_temp; - uint8_t* location_temp; - - location_temp = tx_bipbuf.GetContiguousBlock(length_temp); - if(length_temp) - { - memcpy(data_out, location_temp, length_temp); - length_out = length_temp; - tx_bipbuf.DecommitBlock(length_temp); - - location_temp = tx_bipbuf.GetContiguousBlock(length_temp); - memcpy(&data_out[length_out], location_temp, length_temp); - length_out = length_out + length_temp; - tx_bipbuf.DecommitBlock(length_temp); - return 1; - } - return 0; + uint16_t length_temp; + uint8_t *location_temp; + + location_temp = tx_bipbuf.GetContiguousBlock(length_temp); + + if (length_temp) { + memcpy(data_out, location_temp, length_temp); + length_out = length_temp; + tx_bipbuf.DecommitBlock(length_temp); + + location_temp = tx_bipbuf.GetContiguousBlock(length_temp); + memcpy(&data_out[length_out], location_temp, length_temp); + length_out = length_out + length_temp; + tx_bipbuf.DecommitBlock(length_temp); + return 1; + } + + return 0; } void GenericInterface::SendNow() { - // I'm useless. + // I'm useless. } -void GenericInterface::ReadMsg(CommunicationInterface& com, uint8_t* data, uint8_t length) +void GenericInterface::ReadMsg(CommunicationInterface &com, uint8_t *data, uint8_t length) { - // I currently don't support being talked to - (void)com; - (void)data; - (void)length; + // I currently don't support being talked to + (void)com; + (void)data; + (void)length; } \ No newline at end of file diff --git a/src/packet_finder.c b/src/packet_finder.c index 3fca982..c8e35e3 100644 --- a/src/packet_finder.c +++ b/src/packet_finder.c @@ -21,11 +21,11 @@ //#define DEBUG #ifdef DEBUG - #include - void PrintFromBuffer(struct PacketFinder *pf, uint16_t index, uint16_t length); - #define DEBUG_PRINT_TXT(arg) printf(arg); +#include +void PrintFromBuffer(struct PacketFinder *pf, uint16_t index, uint16_t length); +#define DEBUG_PRINT_TXT(arg) printf(arg); #else - #define DEBUG_PRINT_TXT(arg) +#define DEBUG_PRINT_TXT(arg) #endif @@ -34,341 +34,403 @@ const uint8_t kMaxPacketSize = MAX_PACKET_SIZE; /// max packet size const uint8_t kMaxPacketDataSize = MAX_PACKET_DATA_SIZE; /// max size data const uint8_t kStartByte = 0x55; /// special start byte -void IncrementParseIndex(struct PacketFinder* pf); -void FlushUnusedBytes(struct PacketFinder* pf); +void IncrementParseIndex(struct PacketFinder *pf); +void FlushUnusedBytes(struct PacketFinder *pf); #define SUCCESS (0) #define BUFFER_OVERFLOW (-1) #define FATAL_ERROR (-2) -void InitPacketFinder(struct PacketFinder *pf, struct ByteQueue *bq) { +void InitPacketFinder(struct PacketFinder *pf, struct ByteQueue *bq) +{ - // parser state machine - pf->state = kStart; - pf->parse_index = 0; - pf->packet_start_index = 0; // index of length byte in the buffer, type and data follow - pf->received_length = 0; - pf->packet_indices = bq; - - // circular buffer in storage array 'buffer', size 'PACKET_BUFFER_SIZE' - pf->start_data = pf->buffer; // pointer to first data byte - pf->end_data = pf->buffer; // pointer to byte following last data byte + // parser state machine + pf->state = kStart; + pf->parse_index = 0; + pf->packet_start_index = 0; // index of length byte in the buffer, type and data follow + pf->received_length = 0; + pf->packet_indices = bq; + + // circular buffer in storage array 'buffer', size 'PACKET_BUFFER_SIZE' + pf->start_data = pf->buffer; // pointer to first data byte + pf->end_data = pf->buffer; // pointer to byte following last data byte } -int8_t PutBytes(struct PacketFinder *pf, const uint8_t *bytes, uint16_t bytes_length) { - - // flush any bytes in parser that are no longer needed - FlushUnusedBytes(pf); - - /////////////////////////////////////////////////// - // copy new data to buffer - - int8_t buffer_status = BUFFER_OVERFLOW; // put failure, data loss - - // if end_data comes before start_data, the buffer is wrapped around - // and the available storage is guarenteed continuous - if(pf->end_data < pf->start_data) { - uint16_t space = pf->start_data - pf->end_data - 1; // space available - - uint16_t copy_size = space; // calculate copy_size - if(copy_size > bytes_length) - copy_size = bytes_length; - - memcpy(pf->end_data, bytes, copy_size); // do copy - pf->end_data += copy_size; - - if(copy_size == bytes_length) // check for data loss - buffer_status = SUCCESS; // success - } - - // otherwise available storage might be broken into two pieces - else { - // if first data byte is occupied, last data byte must not be filled - uint8_t* current_end = pf->buffer + PACKET_BUFFER_SIZE; - if(pf->start_data == pf->buffer) - current_end--; - - // calculate amount of space near end and near beginning - uint16_t start_space, end_space; - end_space = current_end - pf->end_data; - if(pf->start_data == pf->buffer) - start_space = 0; - else - start_space = pf->start_data - pf->buffer - 1; - - // calculate first_half and second_half copy sizes (near end and at beginning) - uint16_t first_half = end_space; - if(first_half > bytes_length) - first_half = bytes_length; - uint16_t second_half = start_space; - if(second_half > bytes_length - first_half) - second_half = bytes_length - first_half; - - // copy first half, then second half - memcpy(pf->end_data, bytes, first_half); - if(second_half == 0) { - pf->end_data += first_half; - if(pf->end_data > &(pf->buffer[PACKET_BUFFER_SIZE-1])) - pf->end_data = pf->buffer; - } - else { - memcpy(pf->buffer, &(bytes[first_half]), second_half); - pf->end_data = pf->buffer + second_half; - } - - // check for data loss - if(first_half + second_half == bytes_length) - buffer_status = SUCCESS; // success - } - - - /////////////////////////////////////////////////// - // run parser to exhaustion - - uint16_t end_data_index = pf->end_data - pf->buffer; - while( pf->parse_index != end_data_index) { // parser incrementing - - switch(pf->state) { - - case kStart: - // if start symbol, advance state - if(pf->buffer[pf->parse_index] == kStartByte) { - pf->state = kLen; - } - // always advance parse index and start index - IncrementParseIndex(pf); - pf->packet_start_index = pf->parse_index; - break; - - case kLen: - pf->packet_start_index = pf->parse_index; - - // if length is reasonable - if(pf->buffer[pf->parse_index] <= kMaxPacketDataSize) { - // store index location, store length, start CRC calculation - pf->received_length = pf->buffer[pf->parse_index]; - pf->expected_crc = MakeCrc(&pf->buffer[pf->parse_index], 1); - // increment parse index and parse state - pf->state = kType; - IncrementParseIndex(pf); - } - // else length unreasonable - else { - // start over at this index - pf->state = kStart; - } - break; - - case kType: - pf->expected_crc = ByteUpdateCrc(pf->expected_crc, pf->buffer[pf->parse_index]); - - if(pf->received_length > 0) - pf->state = kData; - else - pf->state = kCRCL; - - pf->data_bytes = 0; //? - IncrementParseIndex(pf); - break; - - case kData: - pf->expected_crc = ByteUpdateCrc(pf->expected_crc, pf->buffer[pf->parse_index]); - pf->data_bytes++; - if(pf->data_bytes >= pf->received_length) - pf->state = kCRCL; - IncrementParseIndex(pf); - break; - - case kCRCL: - pf->received_crc = pf->buffer[pf->parse_index]; - pf->state = kCRCH; - IncrementParseIndex(pf); - break; - - case kCRCH: - pf->received_crc = pf->received_crc + 256*pf->buffer[pf->parse_index]; - // if pass the CRC check - if(pf->expected_crc == pf->received_crc) { - // add packet to outgoing queue - PutByteBQ(pf->packet_indices, pf->packet_start_index); - - #ifdef DEBUG - printf("Found: "); - PrintFromBuffer(pf, pf->packet_start_index+1, pf->received_length+1); - printf("\n"); - #endif - - IncrementParseIndex(pf); - pf->state = kStart; - } - // else fail the CRC check - else { - // restart parser at assumed length byte - pf->parse_index = pf->packet_start_index; - pf->state = kStart; - } - break; - - default: - return(FATAL_ERROR); - } // switch(pf->state) - } // while( pf->parse_index...) - - return(buffer_status); +int8_t PutBytes(struct PacketFinder *pf, const uint8_t *bytes, uint16_t bytes_length) +{ + + // flush any bytes in parser that are no longer needed + FlushUnusedBytes(pf); + + /////////////////////////////////////////////////// + // copy new data to buffer + + int8_t buffer_status = BUFFER_OVERFLOW; // put failure, data loss + + // if end_data comes before start_data, the buffer is wrapped around + // and the available storage is guarenteed continuous + if (pf->end_data < pf->start_data) { + uint16_t space = pf->start_data - pf->end_data - 1; // space available + + uint16_t copy_size = space; // calculate copy_size + + if (copy_size > bytes_length) { + copy_size = bytes_length; + } + + memcpy(pf->end_data, bytes, copy_size); // do copy + pf->end_data += copy_size; + + if (copy_size == bytes_length) { // check for data loss + buffer_status = SUCCESS; // success + } + } + + // otherwise available storage might be broken into two pieces + else { + // if first data byte is occupied, last data byte must not be filled + uint8_t *current_end = pf->buffer + PACKET_BUFFER_SIZE; + + if (pf->start_data == pf->buffer) { + current_end--; + } + + // calculate amount of space near end and near beginning + uint16_t start_space, end_space; + end_space = current_end - pf->end_data; + + if (pf->start_data == pf->buffer) { + start_space = 0; + + } else { + start_space = pf->start_data - pf->buffer - 1; + } + + // calculate first_half and second_half copy sizes (near end and at beginning) + uint16_t first_half = end_space; + + if (first_half > bytes_length) { + first_half = bytes_length; + } + + uint16_t second_half = start_space; + + if (second_half > bytes_length - first_half) { + second_half = bytes_length - first_half; + } + + // copy first half, then second half + memcpy(pf->end_data, bytes, first_half); + + if (second_half == 0) { + pf->end_data += first_half; + + if (pf->end_data > & (pf->buffer[PACKET_BUFFER_SIZE - 1])) { + pf->end_data = pf->buffer; + } + + } else { + memcpy(pf->buffer, &(bytes[first_half]), second_half); + pf->end_data = pf->buffer + second_half; + } + + // check for data loss + if (first_half + second_half == bytes_length) { + buffer_status = SUCCESS; // success + } + } + + + /////////////////////////////////////////////////// + // run parser to exhaustion + + uint16_t end_data_index = pf->end_data - pf->buffer; + + while (pf->parse_index != end_data_index) { // parser incrementing + + switch (pf->state) { + + case kStart: + + // if start symbol, advance state + if (pf->buffer[pf->parse_index] == kStartByte) { + pf->state = kLen; + } + + // always advance parse index and start index + IncrementParseIndex(pf); + pf->packet_start_index = pf->parse_index; + break; + + case kLen: + pf->packet_start_index = pf->parse_index; + + // if length is reasonable + if (pf->buffer[pf->parse_index] <= kMaxPacketDataSize) { + // store index location, store length, start CRC calculation + pf->received_length = pf->buffer[pf->parse_index]; + pf->expected_crc = MakeCrc(&pf->buffer[pf->parse_index], 1); + // increment parse index and parse state + pf->state = kType; + IncrementParseIndex(pf); + } + + // else length unreasonable + else { + // start over at this index + pf->state = kStart; + } + + break; + + case kType: + pf->expected_crc = ByteUpdateCrc(pf->expected_crc, pf->buffer[pf->parse_index]); + + if (pf->received_length > 0) { + pf->state = kData; + + } else { + pf->state = kCRCL; + } + + pf->data_bytes = 0; //? + IncrementParseIndex(pf); + break; + + case kData: + pf->expected_crc = ByteUpdateCrc(pf->expected_crc, pf->buffer[pf->parse_index]); + pf->data_bytes++; + + if (pf->data_bytes >= pf->received_length) { + pf->state = kCRCL; + } + + IncrementParseIndex(pf); + break; + + case kCRCL: + pf->received_crc = pf->buffer[pf->parse_index]; + pf->state = kCRCH; + IncrementParseIndex(pf); + break; + + case kCRCH: + pf->received_crc = pf->received_crc + 256 * pf->buffer[pf->parse_index]; + + // if pass the CRC check + if (pf->expected_crc == pf->received_crc) { + // add packet to outgoing queue + PutByteBQ(pf->packet_indices, pf->packet_start_index); + +#ifdef DEBUG + printf("Found: "); + PrintFromBuffer(pf, pf->packet_start_index + 1, pf->received_length + 1); + printf("\n"); +#endif + + IncrementParseIndex(pf); + pf->state = kStart; + } + + // else fail the CRC check + else { + // restart parser at assumed length byte + pf->parse_index = pf->packet_start_index; + pf->state = kStart; + } + + break; + + default: + return (FATAL_ERROR); + } // switch(pf->state) + } // while( pf->parse_index...) + + return (buffer_status); } -int8_t PeekPacket(struct PacketFinder *pf, uint8_t **packet, uint8_t *length) { - - if(!IsEmptyBQ(pf->packet_indices)) { - - // get return index and length - uint16_t start_index = PeekByteBQ(pf->packet_indices); - uint16_t return_index = start_index + 1; - if(return_index >= PACKET_BUFFER_SIZE) - return_index = 0; - *length = pf->buffer[start_index] + 1; - - #ifdef DEBUG - printf("Peek: "); - PrintFromBuffer(pf, return_index, *length); - printf("\n"); - #endif - - // if packet is contiguous, return direct pointer into buffer - if(return_index + *length < PACKET_BUFFER_SIZE) { - *packet = &(pf->buffer[return_index]); - } - - // else copy to out_buffer and return pointer to that - else { - uint16_t length_at_end = PACKET_BUFFER_SIZE - return_index; - - memcpy( &(pf->out_buffer[0]), - &(pf->buffer[return_index]), - length_at_end); // overflow out would cause a segfault - - - memcpy( &(pf->out_buffer[length_at_end]), - pf->buffer, - *length - length_at_end); - - *packet = pf->out_buffer; - } - return(1); - } - return(0); +int8_t PeekPacket(struct PacketFinder *pf, uint8_t **packet, uint8_t *length) +{ + + if (!IsEmptyBQ(pf->packet_indices)) { + + // get return index and length + uint16_t start_index = PeekByteBQ(pf->packet_indices); + uint16_t return_index = start_index + 1; + + if (return_index >= PACKET_BUFFER_SIZE) { + return_index = 0; + } + + *length = pf->buffer[start_index] + 1; + +#ifdef DEBUG + printf("Peek: "); + PrintFromBuffer(pf, return_index, *length); + printf("\n"); +#endif + + // if packet is contiguous, return direct pointer into buffer + if (return_index + *length < PACKET_BUFFER_SIZE) { + *packet = &(pf->buffer[return_index]); + } + + // else copy to out_buffer and return pointer to that + else { + uint16_t length_at_end = PACKET_BUFFER_SIZE - return_index; + + memcpy(&(pf->out_buffer[0]), + &(pf->buffer[return_index]), + length_at_end); // overflow out would cause a segfault + + + memcpy(&(pf->out_buffer[length_at_end]), + pf->buffer, + *length - length_at_end); + + *packet = pf->out_buffer; + } + + return (1); + } + + return (0); } -int8_t DropPacket(struct PacketFinder *pf) { - - if(!IsEmptyBQ(pf->packet_indices)) { - - // get previous return index and length - uint16_t prev_start_index = GetByteBQ(pf->packet_indices); - uint16_t prev_return_index = prev_start_index + 1; - if(prev_return_index >= PACKET_BUFFER_SIZE) - prev_return_index = 0; - uint8_t prev_length = pf->buffer[prev_start_index] + 1; - - // if still more packets, dump up to next packet start - if(!IsEmptyBQ(pf->packet_indices)) { - pf->start_data = &(pf->buffer[PeekByteBQ(pf->packet_indices)]); - } - // otherwise dump only used up bytes - else { - - // if packet was contiguous - if(prev_return_index + prev_length <= PACKET_BUFFER_SIZE) { - - uint16_t new_start_index = prev_return_index + prev_length; - if(new_start_index >= PACKET_BUFFER_SIZE) - new_start_index = 0; - pf->start_data = &(pf->buffer[new_start_index]); - } - - // not contiguous - else { - uint16_t length_at_end = PACKET_BUFFER_SIZE - prev_return_index; - pf->start_data = &(pf->buffer[prev_length - length_at_end]); - } - } - - return(1); - } - return(0); +int8_t DropPacket(struct PacketFinder *pf) +{ + + if (!IsEmptyBQ(pf->packet_indices)) { + + // get previous return index and length + uint16_t prev_start_index = GetByteBQ(pf->packet_indices); + uint16_t prev_return_index = prev_start_index + 1; + + if (prev_return_index >= PACKET_BUFFER_SIZE) { + prev_return_index = 0; + } + + uint8_t prev_length = pf->buffer[prev_start_index] + 1; + + // if still more packets, dump up to next packet start + if (!IsEmptyBQ(pf->packet_indices)) { + pf->start_data = &(pf->buffer[PeekByteBQ(pf->packet_indices)]); + } + + // otherwise dump only used up bytes + else { + + // if packet was contiguous + if (prev_return_index + prev_length <= PACKET_BUFFER_SIZE) { + + uint16_t new_start_index = prev_return_index + prev_length; + + if (new_start_index >= PACKET_BUFFER_SIZE) { + new_start_index = 0; + } + + pf->start_data = &(pf->buffer[new_start_index]); + } + + // not contiguous + else { + uint16_t length_at_end = PACKET_BUFFER_SIZE - prev_return_index; + pf->start_data = &(pf->buffer[prev_length - length_at_end]); + } + } + + return (1); + } + + return (0); } -int8_t GetPacketCopy(struct PacketFinder *pf, uint8_t *packet_copy, uint8_t *length) { +int8_t GetPacketCopy(struct PacketFinder *pf, uint8_t *packet_copy, uint8_t *length) +{ - uint8_t *packet_original; - int8_t status = PeekPacket(pf, &packet_original, length); - if(1 == status) { - memcpy( packet_copy, packet_original, *length); - DropPacket(pf); - return(1); - } - return(0); + uint8_t *packet_original; + int8_t status = PeekPacket(pf, &packet_original, length); + + if (1 == status) { + memcpy(packet_copy, packet_original, *length); + DropPacket(pf); + return (1); + } + + return (0); } -void IncrementParseIndex(struct PacketFinder* pf) { - pf->parse_index++; - if(pf->parse_index >= PACKET_BUFFER_SIZE) - pf->parse_index = pf->parse_index - PACKET_BUFFER_SIZE; +void IncrementParseIndex(struct PacketFinder *pf) +{ + pf->parse_index++; + + if (pf->parse_index >= PACKET_BUFFER_SIZE) { + pf->parse_index = pf->parse_index - PACKET_BUFFER_SIZE; + } } -void FlushUnusedBytes(struct PacketFinder* pf) { - // if no out bytes are queued - if(IsEmptyBQ(pf->packet_indices)) { - // start of data is start of working packet - pf->start_data = &(pf->buffer[pf->packet_start_index]); - } - else { - // start of data is start of stored packet - pf->start_data = &(pf->buffer[PeekByteBQ(pf->packet_indices)]); - } +void FlushUnusedBytes(struct PacketFinder *pf) +{ + // if no out bytes are queued + if (IsEmptyBQ(pf->packet_indices)) { + // start of data is start of working packet + pf->start_data = &(pf->buffer[pf->packet_start_index]); + + } else { + // start of data is start of stored packet + pf->start_data = &(pf->buffer[PeekByteBQ(pf->packet_indices)]); + } } int8_t FormPacket(uint8_t type, - const uint8_t *in_data, uint8_t in_len, - uint8_t *out_data, uint8_t *out_len) { + const uint8_t *in_data, uint8_t in_len, + uint8_t *out_data, uint8_t *out_len) +{ + + out_data[0] = kStartByte; + out_data[1] = in_len; + out_data[2] = type; - out_data[0] = kStartByte; - out_data[1] = in_len; - out_data[2] = type; + // memcpy( &(out_data[i+3]), in_data, in_len); // untested, faster alternative + uint8_t i = 0; - // memcpy( &(out_data[i+3]), in_data, in_len); // untested, faster alternative - uint8_t i = 0; - for(i=0; i> 8; + out_data[in_len + 3] = crc & 0x00FF; + out_data[in_len + 4] = crc >> 8; - *out_len = in_len+5; - return(1); + *out_len = in_len + 5; + return (1); } #ifdef DEBUG -void PrintFromBuffer(struct PacketFinder *pf, uint16_t index, uint16_t length) { - printf("[ "); - uint8_t i = 0; - for(i=0; ibuffer[index]); - index++; - if(index >= PACKET_BUFFER_SIZE) - index = 0; - } - printf("]"); +void PrintFromBuffer(struct PacketFinder *pf, uint16_t index, uint16_t length) +{ + printf("[ "); + uint8_t i = 0; + + for (i = 0; i < length; i++) { + printf("%i ", pf->buffer[index]); + index++; + + if (index >= PACKET_BUFFER_SIZE) { + index = 0; + } + } + + printf("]"); } #endif diff --git a/tests/2306_pulsing_test.cpp b/tests/2306_pulsing_test.cpp index ff342cd..397f50c 100644 --- a/tests/2306_pulsing_test.cpp +++ b/tests/2306_pulsing_test.cpp @@ -40,13 +40,13 @@ BrushlessDriveClient brushlessDrive(0); // Initialize Brush CoilTemperatureEstimatorClient coilTemperatureEstimator(0); // Initialize Coil Temperature Estimator Client EscPropellerInputParserClient escPropellerInputParser(0); // Initialize Esc Propeller Input Parser Client IQUartFlightControllerInterfaceClient iquartFlightControllerInterface( - 0); // Initialize IQUart Flight Controller Interface Client + 0); // Initialize IQUart Flight Controller Interface Client PersistentMemoryClient persistentMemory(0); // Initialize Persistent Memory Client PowerMonitorClient powerMonitor(0); // Initialize Power Monitor Client PowerSafetyClient powerSafety(0); // Initialize Power Safety Client PropellerMotorControlClient propellerMotorControl(0); // Initialize Propeller Motor Control Client PulsingRectangularInputParserClient pulsingRectangularInputParser( - 0); // Initialize Pulsing Rectangulat Input Parser Client + 0); // Initialize Pulsing Rectangulat Input Parser Client SerialInterfaceClient serialInterface(0); // Initialize Serial Interface Client SystemControlClient systemControl(0); // Initialize System Control Client TemperatureEstimatorClient temperatureEstimator(0); // Initialize Temperature Estimator Client @@ -55,236 +55,260 @@ VoltageSuperPositionClient voltageSuperposition(0); // Initialize Voltage Super // Initialize clientList to make it easier to call ReadMsg for each client ClientAbstract *clientList[14] = {&brushlessDrive, - &coilTemperatureEstimator, - &escPropellerInputParser, - &iquartFlightControllerInterface, - &persistentMemory, - &powerMonitor, - &powerSafety, - &propellerMotorControl, - &pulsingRectangularInputParser, - &serialInterface, - &systemControl, - &temperatureEstimator, - &temperatureMonitorUc, - &voltageSuperposition}; + &coilTemperatureEstimator, + &escPropellerInputParser, + &iquartFlightControllerInterface, + &persistentMemory, + &powerMonitor, + &powerSafety, + &propellerMotorControl, + &pulsingRectangularInputParser, + &serialInterface, + &systemControl, + &temperatureEstimator, + &temperatureMonitorUc, + &voltageSuperposition + }; // Send out any message data we have over the serial interface -int handleComTx() { - uint8_t packetBuffer[64]; - uint8_t length = 0; - DWORD bytesWritten; - - // Get the packet from the com interface and place it into the packet buffer - if (com.GetTxBytes(packetBuffer, length)) { - WriteFile(comPort, packetBuffer, length, &bytesWritten, NULL); - } - - return bytesWritten; +int handleComTx() +{ + uint8_t packetBuffer[64]; + uint8_t length = 0; + DWORD bytesWritten; + + // Get the packet from the com interface and place it into the packet buffer + if (com.GetTxBytes(packetBuffer, length)) { + WriteFile(comPort, packetBuffer, length, &bytesWritten, NULL); + } + + return bytesWritten; } // Grab any received data on the serial interface -int handleComRx() { - uint8_t recvBytes[64]; - DWORD dwBytesReceived; +int handleComRx() +{ + uint8_t recvBytes[64]; + DWORD dwBytesReceived; - ReadFile(comPort, &recvBytes, 64, &dwBytesReceived, 0); - com.SetRxBytes(recvBytes, dwBytesReceived); + ReadFile(comPort, &recvBytes, 64, &dwBytesReceived, 0); + com.SetRxBytes(recvBytes, dwBytesReceived); - return dwBytesReceived; + return dwBytesReceived; } // Hand off any received data to each module so they can handle it -void updateModules() { - // Temporary Pointer to the packet data location - uint8_t *packetData; - uint8_t packetLength; - - // Loads the packet data buffer with data receieved from the motor - while (com.PeekPacket(&packetData, &packetLength)) { - for (auto &client : clientList) { - client->ReadMsg(packetData, packetLength); - }; - com.DropPacket(); - } +void updateModules() +{ + // Temporary Pointer to the packet data location + uint8_t *packetData; + uint8_t packetLength; + + // Loads the packet data buffer with data receieved from the motor + while (com.PeekPacket(&packetData, &packetLength)) { + for (auto &client : clientList) { + client->ReadMsg(packetData, packetLength); + }; + + com.DropPacket(); + } } // Handles communication with motor -void sendMessageAndProcessReply() { - handleComTx(); - handleComRx(); - updateModules(); +void sendMessageAndProcessReply() +{ + handleComTx(); + handleComRx(); + updateModules(); } -uint8_t getDriveMode() { - brushlessDrive.drive_mode_.get(com); - sendMessageAndProcessReply(); - return brushlessDrive.drive_mode_.get_reply(); +uint8_t getDriveMode() +{ + brushlessDrive.drive_mode_.get(com); + sendMessageAndProcessReply(); + return brushlessDrive.drive_mode_.get_reply(); } -float getTCoil() { - coilTemperatureEstimator.t_coil_.get(com); - sendMessageAndProcessReply(); - return coilTemperatureEstimator.t_coil_.get_reply(); +float getTCoil() +{ + coilTemperatureEstimator.t_coil_.get(com); + sendMessageAndProcessReply(); + return coilTemperatureEstimator.t_coil_.get_reply(); } -float getZeroSpinThrottle() { - escPropellerInputParser.zero_spin_throttle_.get(com); - sendMessageAndProcessReply(); - return escPropellerInputParser.zero_spin_throttle_.get_reply(); +float getZeroSpinThrottle() +{ + escPropellerInputParser.zero_spin_throttle_.get(com); + sendMessageAndProcessReply(); + return escPropellerInputParser.zero_spin_throttle_.get_reply(); } -IFCITelemetryData getTelemetry() { - iquartFlightControllerInterface.telemetry_.get(com); - sendMessageAndProcessReply(); - IFCITelemetryData tele = iquartFlightControllerInterface.telemetry_.get_reply(); - return tele; +IFCITelemetryData getTelemetry() +{ + iquartFlightControllerInterface.telemetry_.get(com); + sendMessageAndProcessReply(); + IFCITelemetryData tele = iquartFlightControllerInterface.telemetry_.get_reply(); + return tele; } -uint32_t getFormatKey1() { - persistentMemory.format_key_1_.get(com); - sendMessageAndProcessReply(); - return persistentMemory.format_key_1_.get_reply(); +uint32_t getFormatKey1() +{ + persistentMemory.format_key_1_.get(com); + sendMessageAndProcessReply(); + return persistentMemory.format_key_1_.get_reply(); } -float getVolts() { - powerMonitor.volts_.get(com); - sendMessageAndProcessReply(); - return powerMonitor.volts_.get_reply(); +float getVolts() +{ + powerMonitor.volts_.get(com); + sendMessageAndProcessReply(); + return powerMonitor.volts_.get_reply(); } -float getVoltInputLow() { - powerSafety.volt_input_low_.get(com); - sendMessageAndProcessReply(); - return powerSafety.volt_input_low_.get_reply(); +float getVoltInputLow() +{ + powerSafety.volt_input_low_.get(com); + sendMessageAndProcessReply(); + return powerSafety.volt_input_low_.get_reply(); } -uint8_t getTimeoutSongOption() { - propellerMotorControl.timeout_song_option_.get(com); - sendMessageAndProcessReply(); - return propellerMotorControl.timeout_song_option_.get_reply(); +uint8_t getTimeoutSongOption() +{ + propellerMotorControl.timeout_song_option_.get(com); + sendMessageAndProcessReply(); + return propellerMotorControl.timeout_song_option_.get_reply(); } -uint8_t getPulsingVoltageMode() { - pulsingRectangularInputParser.pulsing_voltage_mode_.get(com); - sendMessageAndProcessReply(); - return pulsingRectangularInputParser.pulsing_voltage_mode_.get_reply(); +uint8_t getPulsingVoltageMode() +{ + pulsingRectangularInputParser.pulsing_voltage_mode_.get(com); + sendMessageAndProcessReply(); + return pulsingRectangularInputParser.pulsing_voltage_mode_.get_reply(); } -uint32_t getBaudRate() { - serialInterface.baud_rate_.get(com); - sendMessageAndProcessReply(); - return serialInterface.baud_rate_.get_reply(); +uint32_t getBaudRate() +{ + serialInterface.baud_rate_.get(com); + sendMessageAndProcessReply(); + return serialInterface.baud_rate_.get_reply(); } -uint32_t getUid1() { - systemControl.uid1_.get(com); - sendMessageAndProcessReply(); - return systemControl.uid1_.get_reply(); +uint32_t getUid1() +{ + systemControl.uid1_.get(com); + sendMessageAndProcessReply(); + return systemControl.uid1_.get_reply(); } -uint32_t getControlFlags() { - systemControl.control_flags_.get(com); - sendMessageAndProcessReply(); - return systemControl.control_flags_.get_reply(); +uint32_t getControlFlags() +{ + systemControl.control_flags_.get(com); + sendMessageAndProcessReply(); + return systemControl.control_flags_.get_reply(); } -float getTemp() { - temperatureEstimator.temp_.get(com); - sendMessageAndProcessReply(); - return temperatureEstimator.temp_.get_reply(); +float getTemp() +{ + temperatureEstimator.temp_.get(com); + sendMessageAndProcessReply(); + return temperatureEstimator.temp_.get_reply(); } -float getUcTemp() { - temperatureMonitorUc.uc_temp_.get(com); - sendMessageAndProcessReply(); - return temperatureMonitorUc.uc_temp_.get_reply(); +float getUcTemp() +{ + temperatureMonitorUc.uc_temp_.get(com); + sendMessageAndProcessReply(); + return temperatureMonitorUc.uc_temp_.get_reply(); } -uint8_t getFrequency() { - voltageSuperposition.frequency_.get(com); - sendMessageAndProcessReply(); - return voltageSuperposition.frequency_.get_reply(); +uint8_t getFrequency() +{ + voltageSuperposition.frequency_.get(com); + sendMessageAndProcessReply(); + return voltageSuperposition.frequency_.get_reply(); } -int main() { - comPort = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, - 0, // must be opened with exclusive-access - NULL, // default security attributes - OPEN_EXISTING, // must use OPEN_EXISTING - 0, // not overlapped I/O - NULL); // hTemplate must be NULL for comm devices +int main() +{ + comPort = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, + 0, // must be opened with exclusive-access + NULL, // default security attributes + OPEN_EXISTING, // must use OPEN_EXISTING + 0, // not overlapped I/O + NULL); // hTemplate must be NULL for comm devices + + cout << comPort << endl; - cout << comPort << endl; + if (comPort == INVALID_HANDLE_VALUE) { + cout << "Error in opening serial port" << endl; - if (comPort == INVALID_HANDLE_VALUE) - cout << "Error in opening serial port" << endl; - else - cout << "opening serial port successful" << endl; + } else { + cout << "opening serial port successful" << endl; + } - DCB dcb = {0}; // Device-control block used to configure serial communications - dcb.DCBlength = sizeof(DCB); - GetCommState(comPort, &dcb); - dcb.BaudRate = CBR_115200; // Set baud rate to 115200 - dcb.ByteSize = 8; - SetCommState(comPort, &dcb); + DCB dcb = {0}; // Device-control block used to configure serial communications + dcb.DCBlength = sizeof(DCB); + GetCommState(comPort, &dcb); + dcb.BaudRate = CBR_115200; // Set baud rate to 115200 + dcb.ByteSize = 8; + SetCommState(comPort, &dcb); - // Set up a read timeout - COMMTIMEOUTS timeouts; - GetCommTimeouts(comPort, &timeouts); - timeouts.ReadIntervalTimeout = 5; - SetCommTimeouts(comPort, &timeouts); + // Set up a read timeout + COMMTIMEOUTS timeouts; + GetCommTimeouts(comPort, &timeouts); + timeouts.ReadIntervalTimeout = 5; + SetCommTimeouts(comPort, &timeouts); - uint8_t driveMode = getDriveMode(); - cout << "drive mode: " << to_string(driveMode) << endl; + uint8_t driveMode = getDriveMode(); + cout << "drive mode: " << to_string(driveMode) << endl; - float tCoil = getTCoil(); - cout << "t_coil: " << to_string(tCoil) << endl; + float tCoil = getTCoil(); + cout << "t_coil: " << to_string(tCoil) << endl; - float zeroSpinThrottle = getZeroSpinThrottle(); - cout << "zero spin throttle: " << to_string(zeroSpinThrottle) << endl; + float zeroSpinThrottle = getZeroSpinThrottle(); + cout << "zero spin throttle: " << to_string(zeroSpinThrottle) << endl; - IFCITelemetryData telemetry = getTelemetry(); - cout << "telemetry coil_temp: " << telemetry.coil_temp << endl; - cout << "telemetry consumption: " << telemetry.consumption << endl; - cout << "telemetry current: " << telemetry.current << endl; - cout << "telemetry mcu temp: " << telemetry.mcu_temp << endl; - cout << "telemetry speed: " << telemetry.speed << endl; - cout << "telemetry uptime: " << telemetry.uptime << endl; - cout << "telemetry voltage: " << telemetry.voltage << endl; + IFCITelemetryData telemetry = getTelemetry(); + cout << "telemetry coil_temp: " << telemetry.coil_temp << endl; + cout << "telemetry consumption: " << telemetry.consumption << endl; + cout << "telemetry current: " << telemetry.current << endl; + cout << "telemetry mcu temp: " << telemetry.mcu_temp << endl; + cout << "telemetry speed: " << telemetry.speed << endl; + cout << "telemetry uptime: " << telemetry.uptime << endl; + cout << "telemetry voltage: " << telemetry.voltage << endl; - uint32_t formatKey1 = getFormatKey1(); - cout << "format key 1: " << to_string(formatKey1) << endl; + uint32_t formatKey1 = getFormatKey1(); + cout << "format key 1: " << to_string(formatKey1) << endl; - float volts = getVolts(); - cout << "volts: " << to_string(volts) << endl; + float volts = getVolts(); + cout << "volts: " << to_string(volts) << endl; - float voltInputLow = getVoltInputLow(); - cout << "volt input low: " << to_string(voltInputLow) << endl; + float voltInputLow = getVoltInputLow(); + cout << "volt input low: " << to_string(voltInputLow) << endl; - uint8_t timeoutSongOption = getTimeoutSongOption(); - cout << "timeout song option: " << to_string(timeoutSongOption) << endl; + uint8_t timeoutSongOption = getTimeoutSongOption(); + cout << "timeout song option: " << to_string(timeoutSongOption) << endl; - uint8_t pulsingVoltageMode = getPulsingVoltageMode(); - cout << "pulsing voltage mode: " << to_string(pulsingVoltageMode) << endl; + uint8_t pulsingVoltageMode = getPulsingVoltageMode(); + cout << "pulsing voltage mode: " << to_string(pulsingVoltageMode) << endl; - uint32_t baudRate = getBaudRate(); - cout << "baud rate: " << to_string(baudRate) << endl; + uint32_t baudRate = getBaudRate(); + cout << "baud rate: " << to_string(baudRate) << endl; - uint32_t uid1 = getUid1(); - cout << "uid1: " << to_string(uid1) << endl; + uint32_t uid1 = getUid1(); + cout << "uid1: " << to_string(uid1) << endl; - uint32_t controlFlags = getControlFlags(); - cout << "control flags: " << to_string(controlFlags) << endl; + uint32_t controlFlags = getControlFlags(); + cout << "control flags: " << to_string(controlFlags) << endl; - // float temp = getTemp(); - // cout << "temp: " << to_string(temp) << endl; + // float temp = getTemp(); + // cout << "temp: " << to_string(temp) << endl; - float ucTemp = getUcTemp(); - cout << "uc temp: " << to_string(ucTemp) << endl; + float ucTemp = getUcTemp(); + cout << "uc temp: " << to_string(ucTemp) << endl; - uint8_t frequency = getFrequency(); - cout << "frequency: " << to_string(frequency) << endl; + uint8_t frequency = getFrequency(); + cout << "frequency: " << to_string(frequency) << endl; - return 0; + return 0; } \ No newline at end of file diff --git a/tests/8108_speed_v0_0_7_test.cpp b/tests/8108_speed_v0_0_7_test.cpp index 6e3b3ed..689486b 100644 --- a/tests/8108_speed_v0_0_7_test.cpp +++ b/tests/8108_speed_v0_0_7_test.cpp @@ -44,189 +44,209 @@ UavcanNodeClient uavcanNode(0); // Initialize UAVCA // Initialize clientList to make it easier to call ReadMsg for each client ClientAbstract *clientList[9] = {&brushlessDrive, - &armingHandler, - &stoppingHandler, - &stowUserInterface, - &multiTurnAngleControl, - &propellerMotorControl, - &escPropellerInputParser, - &coilTemperatureEstimator, - &uavcanNode}; + &armingHandler, + &stoppingHandler, + &stowUserInterface, + &multiTurnAngleControl, + &propellerMotorControl, + &escPropellerInputParser, + &coilTemperatureEstimator, + &uavcanNode + }; // Send out any message data we have over the serial interface -int handleComTx() { - uint8_t packetBuffer[64]; - uint8_t length = 0; - DWORD bytesWritten; - - // Get the packet from the com interface and place it into the packet buffer - if (com.GetTxBytes(packetBuffer, length)) { - WriteFile(comPort, packetBuffer, length, &bytesWritten, NULL); - } - - return bytesWritten; +int handleComTx() +{ + uint8_t packetBuffer[64]; + uint8_t length = 0; + DWORD bytesWritten; + + // Get the packet from the com interface and place it into the packet buffer + if (com.GetTxBytes(packetBuffer, length)) { + WriteFile(comPort, packetBuffer, length, &bytesWritten, NULL); + } + + return bytesWritten; } // Grab any received data on the serial interface -int handleComRx() { - uint8_t recvBytes[64]; - DWORD dwBytesReceived; +int handleComRx() +{ + uint8_t recvBytes[64]; + DWORD dwBytesReceived; - ReadFile(comPort, &recvBytes, 64, &dwBytesReceived, 0); - com.SetRxBytes(recvBytes, dwBytesReceived); + ReadFile(comPort, &recvBytes, 64, &dwBytesReceived, 0); + com.SetRxBytes(recvBytes, dwBytesReceived); - return dwBytesReceived; + return dwBytesReceived; } // Hand off any received data to each module so they can handle it -void updateModules() { - // Temporary Pointer to the packet data location - uint8_t *packetData; - uint8_t packetLength; - - // Loads the packet data buffer with data receieved from the motor - while (com.PeekPacket(&packetData, &packetLength)) { - for (auto &client : clientList) { - client->ReadMsg(packetData, packetLength); - }; - com.DropPacket(); - } +void updateModules() +{ + // Temporary Pointer to the packet data location + uint8_t *packetData; + uint8_t packetLength; + + // Loads the packet data buffer with data receieved from the motor + while (com.PeekPacket(&packetData, &packetLength)) { + for (auto &client : clientList) { + client->ReadMsg(packetData, packetLength); + }; + + com.DropPacket(); + } } // Handles communication with motor -void sendMessageAndProcessReply() { - handleComTx(); - handleComRx(); - updateModules(); +void sendMessageAndProcessReply() +{ + handleComTx(); + handleComRx(); + updateModules(); } -uint8_t getDriveMode() { - brushlessDrive.drive_mode_.get(com); - sendMessageAndProcessReply(); - return brushlessDrive.drive_mode_.get_reply(); +uint8_t getDriveMode() +{ + brushlessDrive.drive_mode_.get(com); + sendMessageAndProcessReply(); + return brushlessDrive.drive_mode_.get_reply(); } -float getVMaxStart() { - brushlessDrive.v_max_start_.get(com); - sendMessageAndProcessReply(); - return brushlessDrive.v_max_start_.get_reply(); +float getVMaxStart() +{ + brushlessDrive.v_max_start_.get(com); + sendMessageAndProcessReply(); + return brushlessDrive.v_max_start_.get_reply(); } // Sends the command to motor to get Voltage -float getArmThrottleUpperLimit() { - armingHandler.arm_throttle_upper_limit_.get(com); - sendMessageAndProcessReply(); - return armingHandler.arm_throttle_upper_limit_.get_reply(); +float getArmThrottleUpperLimit() +{ + armingHandler.arm_throttle_upper_limit_.get(com); + sendMessageAndProcessReply(); + return armingHandler.arm_throttle_upper_limit_.get_reply(); } -float getStoppedSpeed() { - stoppingHandler.stopped_speed_.get(com); - sendMessageAndProcessReply(); - return stoppingHandler.stopped_speed_.get_reply(); +float getStoppedSpeed() +{ + stoppingHandler.stopped_speed_.get(com); + sendMessageAndProcessReply(); + return stoppingHandler.stopped_speed_.get_reply(); } -float getStoppedTime() { - stoppingHandler.stopped_time_.get(com); - sendMessageAndProcessReply(); - return stoppingHandler.stopped_time_.get_reply(); +float getStoppedTime() +{ + stoppingHandler.stopped_time_.get(com); + sendMessageAndProcessReply(); + return stoppingHandler.stopped_time_.get_reply(); } -float getStowKp() { - stowUserInterface.stow_kp_.get(com); - sendMessageAndProcessReply(); - return stowUserInterface.stow_kp_.get_reply(); +float getStowKp() +{ + stowUserInterface.stow_kp_.get(com); + sendMessageAndProcessReply(); + return stowUserInterface.stow_kp_.get_reply(); } -float getAngleKd() { - multiTurnAngleControl.angle_Kd_.get(com); - sendMessageAndProcessReply(); - return multiTurnAngleControl.angle_Kd_.get_reply(); +float getAngleKd() +{ + multiTurnAngleControl.angle_Kd_.get(com); + sendMessageAndProcessReply(); + return multiTurnAngleControl.angle_Kd_.get_reply(); } -uint8_t getTimeoutSongOption() { - propellerMotorControl.timeout_song_option_.get(com); - sendMessageAndProcessReply(); - return propellerMotorControl.timeout_song_option_.get_reply(); +uint8_t getTimeoutSongOption() +{ + propellerMotorControl.timeout_song_option_.get(com); + sendMessageAndProcessReply(); + return propellerMotorControl.timeout_song_option_.get_reply(); } -float getZeroSpinThrottle() { - escPropellerInputParser.zero_spin_throttle_.get(com); - sendMessageAndProcessReply(); - return escPropellerInputParser.zero_spin_throttle_.get_reply(); +float getZeroSpinThrottle() +{ + escPropellerInputParser.zero_spin_throttle_.get(com); + sendMessageAndProcessReply(); + return escPropellerInputParser.zero_spin_throttle_.get_reply(); } -float getTCoil() { - coilTemperatureEstimator.t_coil_.get(com); - sendMessageAndProcessReply(); - return coilTemperatureEstimator.t_coil_.get_reply(); +float getTCoil() +{ + coilTemperatureEstimator.t_coil_.get(com); + sendMessageAndProcessReply(); + return coilTemperatureEstimator.t_coil_.get_reply(); } -uint32_t getBitRate() { - uavcanNode.bit_rate_.get(com); - sendMessageAndProcessReply(); - return uavcanNode.bit_rate_.get_reply(); +uint32_t getBitRate() +{ + uavcanNode.bit_rate_.get(com); + sendMessageAndProcessReply(); + return uavcanNode.bit_rate_.get_reply(); } -int main() { - comPort = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, - 0, // must be opened with exclusive-access - NULL, // default security attributes - OPEN_EXISTING, // must use OPEN_EXISTING - 0, // not overlapped I/O - NULL); // hTemplate must be NULL for comm devices +int main() +{ + comPort = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, + 0, // must be opened with exclusive-access + NULL, // default security attributes + OPEN_EXISTING, // must use OPEN_EXISTING + 0, // not overlapped I/O + NULL); // hTemplate must be NULL for comm devices + + cout << comPort << endl; - cout << comPort << endl; + if (comPort == INVALID_HANDLE_VALUE) { + cout << "Error in opening serial port" << endl; - if (comPort == INVALID_HANDLE_VALUE) - cout << "Error in opening serial port" << endl; - else - cout << "opening serial port successful" << endl; + } else { + cout << "opening serial port successful" << endl; + } - DCB dcb = {0}; // Device-control block used to configure serial communications - dcb.DCBlength = sizeof(DCB); - GetCommState(comPort, &dcb); - dcb.BaudRate = CBR_115200; // Set baud rate to 115200 - dcb.ByteSize = 8; - SetCommState(comPort, &dcb); + DCB dcb = {0}; // Device-control block used to configure serial communications + dcb.DCBlength = sizeof(DCB); + GetCommState(comPort, &dcb); + dcb.BaudRate = CBR_115200; // Set baud rate to 115200 + dcb.ByteSize = 8; + SetCommState(comPort, &dcb); - // Set up a read timeout - COMMTIMEOUTS timeouts; - GetCommTimeouts(comPort, &timeouts); - timeouts.ReadIntervalTimeout = 5; - SetCommTimeouts(comPort, &timeouts); + // Set up a read timeout + COMMTIMEOUTS timeouts; + GetCommTimeouts(comPort, &timeouts); + timeouts.ReadIntervalTimeout = 5; + SetCommTimeouts(comPort, &timeouts); - uint8_t driveMode = getDriveMode(); - cout << "drive mode: " << to_string(driveMode) << endl; + uint8_t driveMode = getDriveMode(); + cout << "drive mode: " << to_string(driveMode) << endl; - float vMaxStart = getVMaxStart(); - cout << "v max start: " << to_string(vMaxStart) << endl; + float vMaxStart = getVMaxStart(); + cout << "v max start: " << to_string(vMaxStart) << endl; - float armThrottleUpperLimit = getArmThrottleUpperLimit(); - cout << "arm throttle upper limit: " << to_string(armThrottleUpperLimit) << endl; + float armThrottleUpperLimit = getArmThrottleUpperLimit(); + cout << "arm throttle upper limit: " << to_string(armThrottleUpperLimit) << endl; - float stoppedSpeed = getStoppedSpeed(); - cout << "stopped speed: " << to_string(stoppedSpeed) << endl; + float stoppedSpeed = getStoppedSpeed(); + cout << "stopped speed: " << to_string(stoppedSpeed) << endl; - float stoppedTime = getStoppedTime(); - cout << "stopped time: " << to_string(stoppedTime) << endl; + float stoppedTime = getStoppedTime(); + cout << "stopped time: " << to_string(stoppedTime) << endl; - float stowKp = getStowKp(); - cout << "stow kp: " << to_string(stowKp) << endl; + float stowKp = getStowKp(); + cout << "stow kp: " << to_string(stowKp) << endl; - float angleKd = getAngleKd(); - cout << "angle kd: " << to_string(angleKd) << endl; + float angleKd = getAngleKd(); + cout << "angle kd: " << to_string(angleKd) << endl; - uint8_t timeoutSongOption = getTimeoutSongOption(); - cout << "timeout song option: " << to_string(timeoutSongOption) << endl; + uint8_t timeoutSongOption = getTimeoutSongOption(); + cout << "timeout song option: " << to_string(timeoutSongOption) << endl; - float zeroSpinThrottle = getZeroSpinThrottle(); - cout << "zero spin throttle: " << to_string(zeroSpinThrottle) << endl; + float zeroSpinThrottle = getZeroSpinThrottle(); + cout << "zero spin throttle: " << to_string(zeroSpinThrottle) << endl; - float tCoil = getTCoil(); - cout << "t_coil: " << to_string(tCoil) << endl; + float tCoil = getTCoil(); + cout << "t_coil: " << to_string(tCoil) << endl; - uint32_t bitRate = getBitRate(); - cout << "bit rate: " << to_string(bitRate) << endl; + uint32_t bitRate = getBitRate(); + cout << "bit rate: " << to_string(bitRate) << endl; - return 0; + return 0; } \ No newline at end of file diff --git a/tests/fortiq_test.cpp b/tests/fortiq_test.cpp index 97ffac5..301ab13 100644 --- a/tests/fortiq_test.cpp +++ b/tests/fortiq_test.cpp @@ -33,109 +33,117 @@ PwmInterfaceClient pwm(0); AdcInterfaceClient adc(0); // Send out any message data we have over the serial interface -int handle_com_tx() { - uint8_t packet_buf[64]; - uint8_t length = 0; - DWORD bytes_written; - - // Get the packet from the com interface and place it into the packet buffer - if (com.GetTxBytes(packet_buf, length)) { - WriteFile(com_port, packet_buf, length, &bytes_written, NULL); - } - - return bytes_written; +int handle_com_tx() +{ + uint8_t packet_buf[64]; + uint8_t length = 0; + DWORD bytes_written; + + // Get the packet from the com interface and place it into the packet buffer + if (com.GetTxBytes(packet_buf, length)) { + WriteFile(com_port, packet_buf, length, &bytes_written, NULL); + } + + return bytes_written; } // Grab any received data on the serial interface -int handle_com_rx() { - uint8_t recv_bytes[64]; - DWORD dwBytesReceived; +int handle_com_rx() +{ + uint8_t recv_bytes[64]; + DWORD dwBytesReceived; - ReadFile(com_port, &recv_bytes, 64, &dwBytesReceived, 0); - com.SetRxBytes(recv_bytes, dwBytesReceived); + ReadFile(com_port, &recv_bytes, 64, &dwBytesReceived, 0); + com.SetRxBytes(recv_bytes, dwBytesReceived); - return dwBytesReceived; + return dwBytesReceived; } // Hand off any received data to each module so they can handle it -void update_modules() { - // Temporary Pointer to the packet data location - uint8_t *packet_data; - uint8_t packet_length; - - // Loads the packet data buffer with data receieved from the motor - while (com.PeekPacket(&packet_data, &packet_length)) { - power.ReadMsg(packet_data, packet_length); - gpio.ReadMsg(packet_data, packet_length); - pwm.ReadMsg(packet_data, packet_length); - adc.ReadMsg(packet_data, packet_length); - com.DropPacket(); - } +void update_modules() +{ + // Temporary Pointer to the packet data location + uint8_t *packet_data; + uint8_t packet_length; + + // Loads the packet data buffer with data receieved from the motor + while (com.PeekPacket(&packet_data, &packet_length)) { + power.ReadMsg(packet_data, packet_length); + gpio.ReadMsg(packet_data, packet_length); + pwm.ReadMsg(packet_data, packet_length); + adc.ReadMsg(packet_data, packet_length); + com.DropPacket(); + } } // Handles communication with motor -void send_message_and_process_reply() { - handle_com_tx(); - handle_com_rx(); - update_modules(); +void send_message_and_process_reply() +{ + handle_com_tx(); + handle_com_rx(); + update_modules(); } // Sends the command to motor to get Voltage -float get_voltage() { - power.volts_.get(com); - send_message_and_process_reply(); - return power.volts_.get_reply(); +float get_voltage() +{ + power.volts_.get(com); + send_message_and_process_reply(); + return power.volts_.get_reply(); } void set_gpio_outputs() {} -int main() { - com_port = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, - 0, // must be opened with exclusive-access - NULL, // default security attributes - OPEN_EXISTING, // must use OPEN_EXISTING - 0, // not overlapped I/O - NULL); // hTemplate must be NULL for comm devices - - cout << com_port << endl; - - if (com_port == INVALID_HANDLE_VALUE) - cout << "Error in opening serial port" << endl; - else - cout << "opening serial port successful" << endl; - - DCB dcb = {0}; // Device-control block used to configure serial communications - dcb.DCBlength = sizeof(DCB); - GetCommState(com_port, &dcb); - dcb.BaudRate = CBR_115200; // Set baud rate to 115200 - dcb.ByteSize = 8; - SetCommState(com_port, &dcb); - - // Set up a read timeout - COMMTIMEOUTS timeouts; - GetCommTimeouts(com_port, &timeouts); - timeouts.ReadIntervalTimeout = 5; - SetCommTimeouts(com_port, &timeouts); - - // Get and print the current Voltage reading of the motor - float current_voltage = get_voltage(); - cout << "voltage: " << to_string(current_voltage) << endl; - - gpio.outputs_register_.set(com, 2); - gpio.outputs_register_.get(com); - send_message_and_process_reply(); - cout << "gpio outputs: " << to_string(gpio.outputs_register_.get_reply()) << endl; - - pwm.duty_cycle_.set(com, 50); - pwm.duty_cycle_.get(com); - send_message_and_process_reply(); - cout << "duty cycle: " << to_string(pwm.duty_cycle_.get_reply()) << endl; - - gpio.addressable_outputs_.set(com, 131); - - adc.adc_voltage_.get(com); - send_message_and_process_reply(); - cout << "adc volts: " << to_string(adc.adc_voltage_.get_reply()) << endl; - - return 0; +int main() +{ + com_port = CreateFile(pcCommPort, GENERIC_READ | GENERIC_WRITE, + 0, // must be opened with exclusive-access + NULL, // default security attributes + OPEN_EXISTING, // must use OPEN_EXISTING + 0, // not overlapped I/O + NULL); // hTemplate must be NULL for comm devices + + cout << com_port << endl; + + if (com_port == INVALID_HANDLE_VALUE) { + cout << "Error in opening serial port" << endl; + + } else { + cout << "opening serial port successful" << endl; + } + + DCB dcb = {0}; // Device-control block used to configure serial communications + dcb.DCBlength = sizeof(DCB); + GetCommState(com_port, &dcb); + dcb.BaudRate = CBR_115200; // Set baud rate to 115200 + dcb.ByteSize = 8; + SetCommState(com_port, &dcb); + + // Set up a read timeout + COMMTIMEOUTS timeouts; + GetCommTimeouts(com_port, &timeouts); + timeouts.ReadIntervalTimeout = 5; + SetCommTimeouts(com_port, &timeouts); + + // Get and print the current Voltage reading of the motor + float current_voltage = get_voltage(); + cout << "voltage: " << to_string(current_voltage) << endl; + + gpio.outputs_register_.set(com, 2); + gpio.outputs_register_.get(com); + send_message_and_process_reply(); + cout << "gpio outputs: " << to_string(gpio.outputs_register_.get_reply()) << endl; + + pwm.duty_cycle_.set(com, 50); + pwm.duty_cycle_.get(com); + send_message_and_process_reply(); + cout << "duty cycle: " << to_string(pwm.duty_cycle_.get_reply()) << endl; + + gpio.addressable_outputs_.set(com, 131); + + adc.adc_voltage_.get(com); + send_message_and_process_reply(); + cout << "adc volts: " << to_string(adc.adc_voltage_.get_reply()) << endl; + + return 0; } \ No newline at end of file