Skip to content

Commit

Permalink
Merge branch 'New-Frontend' of https://github.com/IlliniHyperloopComp…
Browse files Browse the repository at this point in the history 
…uting/Pod into New-Frontend
  • Loading branch information
Evan committed Jul 20, 2019
2 parents 4f80af5 + f22760f commit b699c02
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 100 deletions.
8 changes: 4 additions & 4 deletions CentralComputing/CANManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -217,9 +217,9 @@ std::shared_ptr<CANData> 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] );
Expand All @@ -241,7 +241,7 @@ std::shared_ptr<CANData> 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)
Expand Down
90 changes: 21 additions & 69 deletions CentralComputing/PRUManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand All @@ -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<PRUData> PRUManager::refresh() {
/**reset_mutex.lock();

reset_mutex.lock();
if (do_reset) {
int result = write(pollfds[0].fd, "RESETING\n", 9);
if (result == 0) {
Expand Down Expand Up @@ -105,7 +84,8 @@ std::shared_ptr<PRUData> 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
Expand All @@ -125,7 +105,6 @@ std::shared_ptr<PRUData> 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],
Expand All @@ -150,48 +129,21 @@ std::shared_ptr<PRUData> 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<PRUData> ret_data = std::make_shared<PRUData>();
*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
Expand All @@ -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<PRUData> PRUManager::refresh_sim() {
#ifdef SIM
return SimulatorManager::sim.sim_get_pru();
Expand All @@ -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<PRUData> & check_data, E_States state) {
// hardcoded for two of each type of sensor right now, could use standard dev?
Expand Down Expand Up @@ -261,11 +212,12 @@ void PRUManager::check_for_sensor_error(const std::shared_ptr<PRUData> & 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);
}
}

}



38 changes: 12 additions & 26 deletions CentralComputing/PRUManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,48 +9,34 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <fstream>

//#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<PRUData> {
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<PRUData> refresh();
std::shared_ptr<PRUData> 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};
Expand All @@ -68,13 +54,13 @@ class PRUManager : public SourceManagerBase<PRUData> {
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<PRUData> &, E_States state);
Expand Down
2 changes: 1 addition & 1 deletion CentralComputing/TCPManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t*>(&bms_data), sizeof(BMSCells)) <= 0)) { //NOLINT
return -1;
}
Expand Down

0 comments on commit b699c02

Please sign in to comment.