diff --git a/CentralComputing/CANManager.cpp b/CentralComputing/CANManager.cpp index fd4e2a87..363ee742 100644 --- a/CentralComputing/CANManager.cpp +++ b/CentralComputing/CANManager.cpp @@ -217,9 +217,9 @@ std::shared_ptr CANManager::refresh() { if (r_frame.data[0] < 30) { int cell_id = r_frame.data[0]; // If its "1" indexed instead of 0, incremnt this private_cell_data.cell_data[cell_id].cell_id = r_frame.data[0]; - private_cell_data.cell_data[cell_id].instant_voltage = cast_to_u32(1, 2, r_frame.data); - private_cell_data.cell_data[cell_id].internal_resistance = cast_to_u32(3, 2, r_frame.data); - private_cell_data.cell_data[cell_id].open_voltage = cast_to_u32(5, 2, r_frame.data); + private_cell_data.cell_data[cell_id].instant_voltage = (r_frame.data[1]<<8) | (r_frame.data[2]); + private_cell_data.cell_data[cell_id].internal_resistance = (r_frame.data[3]<<8) | (r_frame.data[4]); + private_cell_data.cell_data[cell_id].open_voltage = (r_frame.data[5]<<8) | (r_frame.data[6]); private_cell_data.cell_data[cell_id].checksum = r_frame.data[7]; } else { print(LogLevel::LOG_ERROR, "Cell Data CAN frame has bad ID ??? %d", r_frame.data[0] ); @@ -241,7 +241,7 @@ std::shared_ptr CANManager::refresh() { } } else { - print(LogLevel::LOG_DEBUG, "CAN Frame UNKNOWN msg: id: %d, len: %d, \n", r_frame.can_id, r_frame.len); + print(LogLevel::LOG_DEBUG, "CAN Frame UNKNOWN msg: id: 0x%x, len: %d, \n", r_frame.can_id, r_frame.len); } // Print the contents of r_frame (assumes len <= 8) diff --git a/CentralComputing/PRUManager.cpp b/CentralComputing/PRUManager.cpp index 0bd2645a..9db6d24c 100644 --- a/CentralComputing/PRUManager.cpp +++ b/CentralComputing/PRUManager.cpp @@ -10,7 +10,6 @@ std::string PRUManager::name() { bool PRUManager::initialize_source() { /* Open the rpmsg_pru character device file */ - /** pollfds[0].fd = open(DEVICE_NAME, O_RDWR); orange_diff_counter = 0; @@ -30,6 +29,7 @@ bool PRUManager::initialize_source() { return false; } + /* Poll until we receive a message from the PRU and then print it */ result = read(pollfds[0].fd, readBuf, MAX_BUFFER_SIZE); if (result == 0) { print(LogLevel::LOG_ERROR, "PRU Unable to read during init: %s\n", DEVICE_NAME); @@ -38,38 +38,17 @@ bool PRUManager::initialize_source() { } print(LogLevel::LOG_INFO, "PRU Manager setup successful\n"); - reset_pru(); - **/ - old_data.wheel_distance[0] = 0; - old_data.wheel_distance[1] = 0; - old_data.wheel_velocity[0] = 0; - old_data.wheel_velocity[1] = 0; - old_data.orange_distance[0] = 0; - old_data.orange_distance[1] = 0; - old_data.orange_velocity[0] = 0; - old_data.orange_velocity[1] = 0; - old_data.watchdog_hz = 200; return true; } -int PRUManager::check_GPIO(int GPIONumber) { - std::string start = "/sys/class/gpio/gpio"; - std::string integer = std::to_string(GPIONumber); - std::string end = "/value"; - std::string path = start + integer + end; - std::fstream myfile(path, std::ios_base::in); - int val; - myfile >> val; - return val; -} - void PRUManager::stop_source() { - //close(pollfds[0].fd); + close(pollfds[0].fd); print(LogLevel::LOG_INFO, "PRU Manager stopped\n"); } std::shared_ptr PRUManager::refresh() { - /**reset_mutex.lock(); + + reset_mutex.lock(); if (do_reset) { int result = write(pollfds[0].fd, "RESETING\n", 9); if (result == 0) { @@ -105,7 +84,8 @@ std::shared_ptr PRUManager::refresh() { if (result == 0) { print(LogLevel::LOG_ERROR, "Unable to write during operation %s\n", DEVICE_NAME); set_error_flag(Command::Network_Command_ID::SET_PRU_ERROR, PRUErrors::PRU_WRITE_ERROR); - // Error. return garbage return empty_data(); + // Error. return garbage + return empty_data(); } // Read from the PRU @@ -125,7 +105,6 @@ std::shared_ptr PRUManager::refresh() { // Copy Raw Data into Buffer RawPRUData raw_data; - print(LogLevel::LOG_ERROR,"PRU RAW DATA %d, %d, %d",raw_data.counts[0],raw_data.counts[1],raw_data.counts[2]); memcpy(&raw_data, readBuf, sizeof(RawPRUData)); //print(LogLevel::LOG_ERROR,"PRU RAW DATA %d, %d, %d %d %d %d %d %d %d %d %d\n" // ,raw_data.counts[0],raw_data.counts[1],raw_data.counts[2], @@ -150,48 +129,21 @@ std::shared_ptr PRUManager::refresh() { raw_data.deltas[wheel_idx[i]], wheel_map[i]); } - **/ + // Get WATCHDOG - PRUData new_data; - if (check_GPIO(WHEEL_GPIO_ONE) == 0) { - old_data.wheel_distance[0] = old_data.wheel_distance[0] + WHEEL_CIRCUMFERENCE_IN_MM; - int64_t time = Utils::microseconds() - wheel_one_last_time; - wheel_one_last_time = Utils::microseconds(); - old_data.wheel_velocity[0] = (int32_t) ((WHEEL_CIRCUMFERENCE_IN_MM / time) * 1000000); - } - if (check_GPIO(WHEEL_GPIO_TWO) == 0) { - old_data.wheel_distance[1] = old_data.wheel_distance[1] + WHEEL_CIRCUMFERENCE_IN_MM; - int64_t time = Utils::microseconds() - wheel_two_last_time; - wheel_two_last_time = Utils::microseconds(); - old_data.wheel_velocity[1] = (int32_t) ((WHEEL_CIRCUMFERENCE_IN_MM / time) * 1000000); - } - print(LogLevel::LOG_ERROR, "GPIO 2 DATA %d\n",check_GPIO(WHEEL_GPIO_TWO)); - print(LogLevel::LOG_ERROR, "GPIO 2 POS/VEL DATA %d %d \n",old_data.wheel_distance[1],old_data.wheel_velocity[1]); - if (check_GPIO(ORANGE_TAPE_GPIO_ONE) == 0) { - old_data.orange_distance[0] = old_data.orange_distance[0] + HUNDRED_FEET_IN_MM; - int64_t time = Utils::microseconds() - orange_one_last_time; - orange_one_last_time = Utils::microseconds(); - old_data.orange_velocity[0] = (int32_t) ((HUNDRED_FEET_IN_MM / time) * 1000000); - } - if (check_GPIO(ORANGE_TAPE_GPIO_TWO) == 0) { - old_data.orange_distance[1] = old_data.orange_distance[1] + HUNDRED_FEET_IN_MM; - int64_t time = Utils::microseconds() - orange_two_last_time; - orange_two_last_time = Utils::microseconds(); - old_data.orange_velocity[1] = (int32_t) ((HUNDRED_FEET_IN_MM / time) * 1000000); - } - if (check_GPIO(WATCHDOG_GPIO) == 0) { - int64_t time = Utils::microseconds() - watchdog_last_time; - watchdog_last_time = Utils::microseconds(); - old_data.watchdog_hz = (int32_t) (1000000 / time); - } + const int watchdog_input_pin = 3; //p8_44 + new_data.watchdog_hz = convert_to_velocity(raw_data.decays[watchdog_input_pin], + raw_data.deltas[watchdog_input_pin], + 1); // 1 for distance, so we get hz + // Store in shared_ptr std::shared_ptr ret_data = std::make_shared(); - *ret_data = old_data; + *ret_data = new_data; return ret_data; } -/** + inline int32_t PRUManager::convert_to_velocity(uint32_t decay, uint32_t delta, uint32_t distance) { // Pick the one that gives us the slower velocity @@ -205,7 +157,7 @@ int32_t PRUManager::convert_to_velocity(uint32_t decay, uint32_t delta, uint32_t return distance / time_diff; // divide distance by time to get a velocity } } -**/ + std::shared_ptr PRUManager::refresh_sim() { #ifdef SIM return SimulatorManager::sim.sim_get_pru(); @@ -225,13 +177,12 @@ void PRUManager::initialize_sensor_error_configs() { } // Define some sort of difference / variance that indicates that shit broke } -/** + void PRUManager::reset_pru() { reset_mutex.lock(); do_reset = true; reset_mutex.unlock(); } -**/ void PRUManager::check_for_sensor_error(const std::shared_ptr & check_data, E_States state) { // hardcoded for two of each type of sensor right now, could use standard dev? @@ -261,11 +212,12 @@ void PRUManager::check_for_sensor_error(const std::shared_ptr & check_d } } - if (check_data->watchdog_hz < error_watchdog_heartbeat_min_hz) { - set_error_flag(Command::Network_Command_ID::SET_PRU_ERROR, PRUErrors::PRU_WATCHDOG_FAIL); + // Wait 2 second after reset, to avoid transient errors + if (microseconds() - reset_timeout_start > 2000000 ) { + if (check_data->watchdog_hz < error_watchdog_heartbeat_min_hz) { + set_error_flag(Command::Network_Command_ID::SET_PRU_ERROR, PRUErrors::PRU_WATCHDOG_FAIL); + } } } - - diff --git a/CentralComputing/PRUManager.h b/CentralComputing/PRUManager.h index 93ea3e05..a0b3dd8d 100644 --- a/CentralComputing/PRUManager.h +++ b/CentralComputing/PRUManager.h @@ -9,48 +9,34 @@ #include #include #include -#include -#include -//#define DEVICE_NAME "/dev/rpmsg_pru31" +#define DEVICE_NAME "/dev/rpmsg_pru31" #define MAX_BUFFER_SIZE 512 +#define HUNDRED_FEET_IN_MM (30480) +#define WHEEL_CIRCUMFRENCE_IN_MM (319) + +#define CLOCK_TO_SEC ((double)21.474836475 / (double)4294967295.0) -//#define CLOCK_TO_SEC ((double)21.474836475 / (double)4294967295.0) -/** struct RawPRUData { uint32_t counts[11]; uint32_t decays[11]; uint32_t deltas[11]; }; -**/ class PRUManager : public SourceManagerBase { private: - const int32_t WHEEL_CIRCUMFERENCE_IN_MM = 500; - const int32_t HUNDRED_FEET_IN_MM = 30480; - const int WHEEL_GPIO_ONE = 66; - const int WHEEL_GPIO_TWO = 26; - const int ORANGE_TAPE_GPIO_ONE = 46; - const int ORANGE_TAPE_GPIO_TWO = 65; - const int WATCHDOG_GPIO =111; - int64_t wheel_one_last_time = Utils::microseconds(); - int64_t wheel_two_last_time = Utils::microseconds(); - int64_t orange_one_last_time = Utils::microseconds(); - int64_t orange_two_last_time = Utils::microseconds(); - int64_t watchdog_last_time = Utils::microseconds(); - PRUData old_data; bool initialize_source(); void stop_source(); std::shared_ptr refresh(); std::shared_ptr refresh_sim(); - //int32_t convert_to_velocity(uint32_t decay, uint32_t delta, uint32_t distance); - int check_GPIO(int GPIONumber); + int32_t convert_to_velocity(uint32_t decay, uint32_t delta, uint32_t distance); + std::string name(); uint8_t readBuf[MAX_BUFFER_SIZE]; - //struct pollfd pollfds[1]; + struct pollfd pollfds[1]; // Variables used for pru processing const int orange_idx[NUM_ORANGE_INPUTS] = {3, 3}; @@ -68,13 +54,13 @@ class PRUManager : public SourceManagerBase { int32_t orange_diff_counter; int32_t wheel_diff_counter; - //bool do_reset = 0; - //int64_t reset_timeout_start; - //std::mutex reset_mutex; + bool do_reset = 0; + int64_t reset_timeout_start; + std::mutex reset_mutex; public: - //void reset_pru(); + void reset_pru(); // Public for testing purposes void initialize_sensor_error_configs(); void check_for_sensor_error(const std::shared_ptr &, E_States state); diff --git a/CentralComputing/TCPManager.cpp b/CentralComputing/TCPManager.cpp index 5b56ba03..2b1e2244 100644 --- a/CentralComputing/TCPManager.cpp +++ b/CentralComputing/TCPManager.cpp @@ -122,7 +122,7 @@ int TCPManager::write_data() { memcpy(&bms_data, &SourceManager::CAN.public_cell_data, sizeof(BMSCells)); SourceManager::CAN.cell_data_mutex.unlock(); last_sent_times[3] = cur_time; - if ((write_all_to_socket(socketfd, &TCPID.can_id, sizeof(uint8_t)) <= 0) || + if ((write_all_to_socket(socketfd, &TCPID.bms_id, sizeof(uint8_t)) <= 0) || (write_all_to_socket(socketfd, reinterpret_cast(&bms_data), sizeof(BMSCells)) <= 0)) { //NOLINT return -1; }