summaryrefslogtreecommitdiff
path: root/src/unused
diff options
context:
space:
mode:
Diffstat (limited to 'src/unused')
-rw-r--r--src/unused/actuationPlan.cpp60
-rw-r--r--src/unused/ads.cpp286
-rw-r--r--src/unused/logger.cpp132
-rw-r--r--src/unused/motor.cpp46
-rw-r--r--src/unused/rocketUtils.cpp35
-rw-r--r--src/unused/sensorAltimeter.cpp115
-rw-r--r--src/unused/sensorIMU.cpp385
-rw-r--r--src/unused/surfaceFitModel.cpp40
8 files changed, 0 insertions, 1099 deletions
diff --git a/src/unused/actuationPlan.cpp b/src/unused/actuationPlan.cpp
deleted file mode 100644
index a987478..0000000
--- a/src/unused/actuationPlan.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "../include/actuationPlan.hpp"
-
-ActuationPlan::ActuationPlan() {}
-
-ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) {
-
-}
-
-
-void ActuationPlan::runPlan(Vehicle& rocket) {
-
-
- if (rocket.imuReadFail || rocket.altiReadFail) {
- rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment
- }
-
- rocket.fail_time = time(nullptr);
-
- // 2024 Mission---------------------------------------------------------------------
- if (rocket.status == GLIDE) {
-
- // Fin deployment based on current drag coefficient value
- try {
- double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude);
- cd = std::min(std::max(0.0, cd), 100.0);
- rocket.deployment_angle = deploy_percentage_to_angle(cd);
- }
-
- // Full deployment during coasting
- catch (...) {
- rocket.deployment_angle = deploy_percentage_to_angle(0);
-
- if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) {
- rocket.deployment_angle = deploy_percentage_to_angle(100);
- }
- }
- }
-
- else if (rocket.status == APOGEE) {
-
- rocket.deployment_angle = deploy_percentage_to_angle(50);
- }
-
- else {
-
- rocket.deploy_time = time(nullptr);
- }
- // End 2024 Mission------------------------------------------------------------------
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/ads.cpp b/src/unused/ads.cpp
deleted file mode 100644
index 5484970..0000000
--- a/src/unused/ads.cpp
+++ /dev/null
@@ -1,286 +0,0 @@
-#include "../include/ads.hpp"
-
-
-// Private----------------------------------------------------------------------
-void ADS::logSummary() {
-
- std::string output_string = "" + state_for_log[rocket.status];
-
- if (!rocket.altiInitFail && !rocket.altiReadFail) {
-
- output_string += format_data(" ", rocket.filtered_altitude, 3);
- }
-
- output_string += format_data(" ", rocket.deployment_angle, 2);
-
- if (!rocket.imuInitFail && !rocket.imuReadFail) {
-
- output_string += format_data(" ", rocket.acceleration[2], 2);
- output_string += format_data(" ", rocket.filtered_velocity, 2);
- }
-
- Logger::Get().log(output_string);
-}
-
-
-void ADS::updateOnPadAltitude() {
-
- std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
- double avg_alt = 0;
- double alt_read_count = 0;
-
- while (alt_read_count < COUNT_LIMIT) {
-
- altimeter.getData(&rocket.current_altitude);
- alt_read_count++;
- avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count;
- }
-
- Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3));
- rocket.ON_PAD_altitude = avg_alt;
-}
-
-
-void ADS::updateSensorData() {
-
- if (!rocket.imuInitFail) {
-
- try {
- imu.getData((void*)&rocket);
- }
-
- catch (...) {
- std::exception_ptr e = std::current_exception();
- Logger::Get().logErr(e.__cxa_exception_type()->name());
- rocket.imuReadFail = true;
- }
- }
-
- rocket.previous_altitude = rocket.current_altitude; // Why was this placed here????
-
- if (!rocket.altiInitFail) {
-
- try {
- altimeter.getData((void*)&rocket.current_altitude);
- if (rocket.ON_PAD_fail) {
- rocket.ON_PAD_altitude = rocket.current_altitude;
- rocket.ON_PAD_fail = false;
- }
-
- rocket.altiReadFail = false;
- }
-
- catch (...) {
- std::exception_ptr e = std::current_exception();
- Logger::Get().logErr(e.__cxa_exception_type()->name());
- rocket.altiReadFail = true;
- }
- }
-}
-
-
-void ADS::updateRocketState() {
-
- // Filter sensor data
- VectorXf control_input(1);
- VectorXf measurement(1);
- control_input << rocket.acceleration[2];
- measurement << rocket.current_altitude;
- VectorXf filtered = kf.run(control_input, measurement, rocket.dt);
- rocket.filtered_altitude = filtered(0);
- rocket.filtered_velocity = filtered(1);
-
- if (rocket.apogee_altitude < rocket.filtered_altitude) {
- rocket.apogee_altitude = rocket.filtered_altitude;
- }
-
- // (VEHICLE ON PAD)
- if (rocket.status == ON_PAD) {
-
- // If launch detected
- if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0
- && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) {
- Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
- }
-
- if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) {
- Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
- }
-
- if (time(nullptr) - rocket.relog_time > 2*60*60
- && rocket.status == ON_PAD) {
- std::cout << "OverWR Success" << std::endl;
- }
- }
-
- // (VEHICLE BOOSTING)
- else if (rocket.status == BOOST) {
-
- if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0
- || time(nullptr) - rocket.liftoff_time >= TIME_BO) {
- rocket.status = GLIDE;
- }
-
- }
-
- // (VEHICLE IN GLIDE)
- else if (rocket.status == GLIDE) {
-
- if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE
- || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) {
- rocket.status = APOGEE;
- Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2));
- }
- }
-
- // (VEHICLE AT APOGEE)
- else if (rocket.status == APOGEE) {
-
- if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) {
- rocket.status = DONE;
- return;
- }
- }
-}
-
-
-// Public----------------------------------------------------------------------
-ADS::ADS(ActuationPlan plan) : plan(plan) {
-
- rocket.status = ON_PAD;
-
- rocket.apogee_altitude = 0;
- rocket.previous_altitude = 0;
- rocket.current_altitude = 0;
- rocket.filtered_altitude = 0;
-
- rocket.filtered_velocity = 0;
-
- rocket.duty_span = DUTY_MAX - DUTY_MIN;
- rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT);
-
- rocket.dt = 0.1;
-
- rocket.imuInitFail = false;
- rocket.imuReadFail = false;
- rocket.altiInitFail = false;
- rocket.altiReadFail = false;
-
- rocket.ON_PAD_altitude = 0;
- rocket.ON_PAD_fail = false;
-
- rocket.start_time = time(nullptr);
- rocket.fail_time = rocket.start_time;
- rocket.relog_time = rocket.start_time;
- rocket.led_time = rocket.start_time;
-
- imu = IMUSensor();
- altimeter = AltimeterSensor();
- motor = Motor();
- kf = KalmanFilter(2, 1, 1, rocket.dt);
-
- Logger::Get().openLog(LOG_FILENAME);
-
- motor.init(&rocket);
-
- imu.init(nullptr);
- altimeter.init(nullptr);
-
- if (TEST_MODE) {
-
- Logger::Get().log("TEST Record Start --");
- }
-}
-
-
-
-void ADS::run() {
-
- if (!rocket.altiInitFail) {
- try {
- updateOnPadAltitude();
- }
-
- catch (...) {
- std::exception_ptr e = std::current_exception();
- Logger::Get().logErr(e.__cxa_exception_type()->name());
- rocket.ON_PAD_fail = true;
- }
- }
-
- rocket.loop_time = time(nullptr);
- while (rocket.status != DONE) {
-
- updateSensorData();
-
- if (!rocket.imuInitFail && !rocket.altiInitFail) {
-
- updateRocketState();
-
- // Run the Actuation Plan----------------------------------
- plan.runPlan(rocket);
-
- if (rocket.imuReadFail || rocket.altiReadFail) {
-
- if (rocket.imuReadFail) {
- imu.init(nullptr); // Restart
- Logger::Get().log("Altimeter reset attempt");
- }
-
- if (rocket.altiReadFail) {
- altimeter.init(nullptr); // Restart
- Logger::Get().log("IMU reset attempt");
- }
- }
- }
-
- // Altimeter or IMU setup failed. Attempt to reinitialize
- else {
-
- if (time(nullptr) - rocket.fail_time >= TIME_END) {
- rocket.status = DONE;
- }
-
- if (rocket.altiInitFail || rocket.altiReadFail) {
- imu.init(nullptr); // Restart
- Logger::Get().log("Altimeter reset attempt");
- }
-
- if (rocket.imuInitFail || rocket.imuReadFail) {
- altimeter.init(nullptr); // Restart
- Logger::Get().log("IMU reset attempt");
- }
-
- rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT);
- }
-
- // Actuate Servos
- motor.writeData(&rocket);
-
- logSummary();
-
- // Blink Beaglebone LED 1
- if (time(nullptr) - rocket.led_time > LED_GAP_TIME) {
- led_out(&rocket);
- }
-
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
- rocket.dt = time(nullptr) - rocket.loop_time;
- rocket.loop_time = time(nullptr);
- }
-
- Logger::Get().closeLog();
- std::cout << "Done" << std::endl;
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/logger.cpp b/src/unused/logger.cpp
deleted file mode 100644
index a857be8..0000000
--- a/src/unused/logger.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-#include "../include/logger.hpp"
-
-// Private----------------------------------------------------------------------
-std::string Logger::getDate() {
-
- t = time(nullptr);
- now = localtime(&t);
- return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " "
- + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")";
-}
-
-std::string Logger::getTime() {
-
- t = time(nullptr);
- now = localtime(&t);
- std::string hour = std::to_string(now->tm_hour);
- std::string min = std::to_string(now->tm_min);
- std::string sec = std::to_string(now->tm_sec);
- //string hour = "0" + to_string(now->tm_hour);
-
- if (now->tm_hour < 10) {
- hour = "0" + std::to_string(now->tm_hour);
- }
-
- if (now->tm_min < 10) {
- min = "0" + std::to_string(now->tm_min);
- }
-
- if (now->tm_sec < 10) {
- sec = "0" + std::to_string(now->tm_sec);
- }
-
- return hour + ":" + min +
- ":" + sec;
-}
-
-
-// Public----------------------------------------------------------------------
-Logger& Logger::Get() {
-
- static Logger loggerSingleton;
- return loggerSingleton;
-}
-
-//Logger Logger::loggerSingleton;
-
-
-bool Logger::openLog(std::string _filename) {
-
- filename = _filename;
-
- if (file_open) {
- return false;
- }
-
- file.open(filename, std::ios::in | std::ios::out | std::ios::app);
-
- if (!file) {
- return false;
- }
-
- file_open = true;
- std::string date = getDate();
- std::string timestamp = getTime();
- file << timestamp << infoTag << "Log Start---- " << date << std::endl;
-
- return true;
-}
-
-
-void Logger::closeLog() {
-
- std::string timestamp = getTime();
- file << timestamp << infoTag << "Log End----\n\n";
-
- file.close();
- file_open = false;
-}
-
-
-bool Logger::log(std::string data) {
-
- if (!file) {
- return false;
- }
-
- if (!file_open) {
- return false;
- }
- std::string timestamp = getTime();
- file << timestamp << infoTag << data << std::endl;
- return true;
-}
-
-bool Logger::logErr(std::string data) {
-
- if (!file) {
- return false;
- }
-
- if (!file_open) {
- return false;
- }
-
- std::string timestamp = getTime();
- file << timestamp << errorTag << data << std::endl;
- return true;
-}
-
-
-bool Logger::printLog() {
-
- if (file.is_open()) {
- std::cout << "Log still open. Please close Log." << std::endl;
- return false;
- }
-
- file.open(filename, std::ios::in);
-
- if (!file.is_open()) {
- return false;
- }
-
- std::string line;
- while(getline(file, line)) {
- std::cout << line << std::endl;
- }
-
- file.close();
-
- return true;
-}
diff --git a/src/unused/motor.cpp b/src/unused/motor.cpp
deleted file mode 100644
index 84785a9..0000000
--- a/src/unused/motor.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "../include/motor.hpp"
-
-
-
-Motor::Motor() {
-
-
-}
-
-bool Motor::init(void* data) {
-
- Vehicle *vehicle = (Vehicle *) data;
- double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN);
-
- // Initialize stuff
- // .....
- // .....
-
-
- data = (void*) vehicle; // Is this necessary?
- return true;
-}
-
-
-bool Motor::writeData(void* data) {
-
- Vehicle *vehicle = (Vehicle *) data;
- double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN);
-
- // Send the Data somewhere
- // ..... Pin
- // ..... Duty
- // ..... PWM frequency Hz
- // ..... Polarity
-
-
- if (1 == 2) {
- Logger::Get().logErr("Some type of Error");
- return false;
- }
-
- data = (void*) vehicle; // Is this necessary?
- return true;
-}
-
-
diff --git a/src/unused/rocketUtils.cpp b/src/unused/rocketUtils.cpp
deleted file mode 100644
index 45fcfc3..0000000
--- a/src/unused/rocketUtils.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "../include/rocketUtils.hpp"
-
-double deploy_percentage_to_angle(double percentage) {
-
- return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE;
-}
-
-
-std::string format_data(std::string prefix, double data, int precision) {
-
- std::stringstream stream;
- stream << std::fixed << std::setprecision(precision) << data;
- std::string s = stream.str();
- return prefix + s;
-}
-
-bool led_out(Vehicle *vehicle) {
-
- std::ofstream file;
- file.open(LED_FILENAME);
- if (!file.is_open()) {
- return false;
- }
-
- file << std::to_string(vehicle->led_brightness);
- file.close();
-
- vehicle->led_time = time(nullptr);
- vehicle->led_brightness = (vehicle->led_brightness + 1) % 2;
-
- return true;
-}
-
-std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"};
- \ No newline at end of file
diff --git a/src/unused/sensorAltimeter.cpp b/src/unused/sensorAltimeter.cpp
deleted file mode 100644
index 8ec065d..0000000
--- a/src/unused/sensorAltimeter.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-#include "sensorAltimeter.hpp"
-
-AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) {
- I2C_FILE = I2C_FILE_in;
- deviceAddress = 0x60;
-}
-
-//Startup routine copied from Adafruit library, as is most of the data getting methods
-//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation)
-bool AltimeterSensor::init() {
-
- // Vehicle *vehicle = (Vehicle *) data;
- // // Do Stuff
- // data = (void*) vehicle;
-
- //Pass file string from parent to setup function, actual I2C bus gets stored internally.
- setupI2C(I2C_FILE);
-
- // Check a register with a hard-coded value to see if comms are working
- uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI);
- if (whoami != 0xC4) {
- fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami);
- return false;
- }
-
- //Send device dedicated reset byte to CTRL1 Register
- writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST);
- //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register
- while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST);
-
- //Set oversampling (?) and altitude mode by default
- currentMode = MPL3115A2_ALTIMETER;
- ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT;
- writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-
- //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess
- writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE |
- MPL3115A2_PT_DATA_CFG_PDEFE |
- MPL3115A2_PT_DATA_CFG_DREM);
-
- return true;
-}
-
-//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION
-void AltimeterSensor::setMode(mpl3115a2_mode_t mode) {
- ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
- ctrl_reg1.bit.ALT = mode;
- writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
- currentMode = mode;
-}
-
-double AltimeterSensor::getAltitude() {
- //Request new data reading
- requestOneShotReading();
- //If new data is available, read it and store it to internal fields
- if (isNewDataAvailable()) {
- //Logger flag here for new data?
- updateCurrentDataBuffer();
- }
- //Return internal field, whether updated or not
- return internalAltitude;
-}
-
-double AltimeterSensor::getTemperature() {
- //Request new data reading
- requestOneShotReading();
- //If new data is available, read it and store it to internal fields
- if (isNewDataAvailable()) {
- //Logger flag here for new data?
- updateCurrentDataBuffer();
- }
- //Return internal field, whether updated or not
- return internalTemperature;
-}
-
-void AltimeterSensor::requestOneShotReading() {
- //Request current status of oneshot reading
- ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
- //If oneshot is complete, proc a new one; if it isn't, do nothing.
- //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR.
- if (!ctrl_reg1.bit.OST) {
- // initiate one-shot measurement
- ctrl_reg1.bit.OST = 1;
- writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
- }
-}
-
-bool AltimeterSensor::isNewDataAvailable() {
- //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available
- //There *are* registers available for exclusively temperature *or* pressure/altitude, but
- //for simplicity's sake we'll use the combined one for now.
- return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0);
-}
-
-//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead
-void AltimeterSensor::updateCurrentDataBuffer() {
- uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0};
- readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5);
-
- //Pressure is no longer used, assumed rocket is only logging altitude
- // uint32_t pressure;
- // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 |
- // uint32_t(buffer[2]);
- // return double(pressure) / 6400.0;
-
- //Altitude Conversion
- int32_t alt;
- alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 |
- uint32_t(buffer[2]) << 8;
- internalAltitude = double(alt) / 65536.0;
-
- int16_t t;
- t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]);
- internalTemperature = double(t) / 256.0;
-}
diff --git a/src/unused/sensorIMU.cpp b/src/unused/sensorIMU.cpp
deleted file mode 100644
index 941ea35..0000000
--- a/src/unused/sensorIMU.cpp
+++ /dev/null
@@ -1,385 +0,0 @@
-#include "../include/sensorIMU.hpp"
-
-IMUSensor::IMUSensor(std::string I2C_FILE) {
- this -> I2C_FILE = I2C_FILE;
-}
-
-bool IMUSensor::init(void* data) {
-
- //I2C_File passed on object creation, stored in sensorI2C parent
- setupI2C(I2C_FILE);
-
- //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second
- //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that
-
- //Sanity check for factory device ID
- uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR);
- if (id != BNO055_ID) {
- fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!");
- return false;
- }
-
- //Set default operating mode of IMU into config from startup (will be set properly after config phase)
- setModeHard(OPERATION_MODE_CONFIG);
-
- //Writes 1 to the system reset bit in the trigger register
- writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20);
- //Wait for reset to complete by doing sanity check again
- while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID);
-
- //Set power mode for sensor
- writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
-
- //Sensor chip uses two "pages" to multiplex register values
- //Page 0 contains the sensor data (not configuration), which is what we want
- writeRegister(BNO055_PAGE_ID_ADDR, 0);
-
- //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess
- writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0);
-
- setModeTemp(default_mode);
-
- return true;
-}
-
-//Sets mode so it can be undone for temporary changes, like operation setting
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
- currentMode = mode;
- writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-//Sets mode *AND* internal state variable
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
- writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-adafruit_bno055_opmode_t IMUSensor::getMode() {
- return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR);
-}
-
-imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) {
- imu::Vector<3> xyz;
- uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6);
-
- int16_t x, y, z;
- x = y = z = 0;
-
- /* Read vector data (6 bytes) */
- x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
- y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
- z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
-
- /*!
- * Convert the value to an appropriate range (section 3.6.4)
- * and assign the value to the Vector type
- */
- switch (vector_type) {
- case VECTOR_MAGNETOMETER:
- /* 1uT = 16 LSB */
- xyz[0] = ((double)x) / 16.0;
- xyz[1] = ((double)y) / 16.0;
- xyz[2] = ((double)z) / 16.0;
- break;
- case VECTOR_GYROSCOPE:
- /* 1dps = 16 LSB */
- xyz[0] = ((double)x) / 16.0;
- xyz[1] = ((double)y) / 16.0;
- xyz[2] = ((double)z) / 16.0;
- break;
- case VECTOR_EULER:
- /* 1 degree = 16 LSB */
- xyz[0] = ((double)x) / 16.0;
- xyz[1] = ((double)y) / 16.0;
- xyz[2] = ((double)z) / 16.0;
- break;
- case VECTOR_ACCELEROMETER:
- /* 1m/s^2 = 100 LSB */
- xyz[0] = ((double)x) / 100.0;
- xyz[1] = ((double)y) / 100.0;
- xyz[2] = ((double)z) / 100.0;
- break;
- case VECTOR_LINEARACCEL:
- /* 1m/s^2 = 100 LSB */
- xyz[0] = ((double)x) / 100.0;
- xyz[1] = ((double)y) / 100.0;
- xyz[2] = ((double)z) / 100.0;
- break;
- case VECTOR_GRAVITY:
- /* 1m/s^2 = 100 LSB */
- xyz[0] = ((double)x) / 100.0;
- xyz[1] = ((double)y) / 100.0;
- xyz[2] = ((double)z) / 100.0;
- break;
- }
-
- return xyz;
-}
-imu::Quaternion IMUSensor::getQuat() {
- uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8);
-
- int16_t x, y, z, w;
- x = y = z = w = 0;
-
- //Bit shift data into the right places and store it
- w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
- x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
- y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
- z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
-
- /*!
- * Assign to Quaternion
- * See
- * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
- * 3.6.5.5 Orientation (Quaternion)
- */
- const double scale = (1.0 / (1 << 14));
- imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
- return quat;
-}
-
-int8_t IMUSensor::getTemp() {
- int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR));
- return temp;
-}
-
-void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) {
- //Put into proper config for mapping stuff
- setModeTemp(OPERATION_MODE_CONFIG);
- writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
-
- //Return mode to operating mode
- setModeTemp(currentMode);
-}
-
-void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
- //See above method, pretty much the exact same
- setModeTemp(OPERATION_MODE_CONFIG);
- writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
- setModeTemp(currentMode);
-}
-
-//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it.
-void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) {
- //Make sure IMU is on proper register page to get system status
- writeRegister(BNO055_PAGE_ID_ADDR, 0);
-
- //If system status requested, read the status.
- if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR);
- //If self test result requested, pull the self test results.
- if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR);
- //Finally, if there's an error pull and stash it.
- if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR);
-}
-
-//Same as above method, byte pointers are fed into it as parameters that get populated by method.
-void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
- uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR);
- if (sys != NULL) {
- *sys = (calData >> 6) & 0x03;
- }
- if (gyro != NULL) {
- *gyro = (calData >> 4) & 0x03;
- }
- if (accel != NULL) {
- *accel = (calData >> 2) & 0x03;
- }
- if (mag != NULL) {
- *mag = calData & 0x03;
- }
-}
-
-/* Functions to deal with raw calibration data */
-bool IMUSensor::getSensorOffsets(uint8_t *calibData) {
- if (isFullyCalibrated()) {
- setModeTemp(OPERATION_MODE_CONFIG);
-
- calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS);
-
- setModeTemp(currentMode);
- return true;
- }
- return false;
-}
-
-//Fully populated offset getter using type of offset, not just calibration data
-bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) {
- if (isFullyCalibrated()) {
- setModeTemp(OPERATION_MODE_CONFIG);
-
- /* Accel offset range depends on the G-range:
- +/-2g = +/- 2000 mg
- +/-4g = +/- 4000 mg
- +/-8g = +/- 8000 mg
- +/-1§g = +/- 16000 mg */
- offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
- (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR));
- offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
- (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR));
- offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
- (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR));
-
- /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
- offsets_type.mag_offset_x =
- (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR));
- offsets_type.mag_offset_y =
- (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR));
- offsets_type.mag_offset_z =
- (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR));
-
- /* Gyro offset range depends on the DPS range:
- 2000 dps = +/- 32000 LSB
- 1000 dps = +/- 16000 LSB
- 500 dps = +/- 8000 LSB
- 250 dps = +/- 4000 LSB
- 125 dps = +/- 2000 LSB
- ... where 1 DPS = 16 LSB */
- offsets_type.gyro_offset_x =
- (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR));
- offsets_type.gyro_offset_y =
- (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR));
- offsets_type.gyro_offset_z =
- (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR));
-
- /* Accelerometer radius = +/- 1000 LSB */
- offsets_type.accel_radius =
- (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR));
-
- /* Magnetometer radius = +/- 960 LSB */
- offsets_type.mag_radius =
- (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR));
-
- setModeTemp(currentMode);
- return true;
- }
- return false;
-}
-
-void IMUSensor::setSensorOffsets(const uint8_t *calibData) {
- setModeTemp(OPERATION_MODE_CONFIG);
-
- /* Note: Configuration will take place only when user writes to the last
- byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
- Therefore the last byte must be written whenever the user wants to
- changes the configuration. */
-
- /* A writeLen() would make this much cleaner */
- writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
- writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
- writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
- writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
- writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
- writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
-
- writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
- writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
- writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
- writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
- writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
- writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
-
- writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
- writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
- writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
- writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
- writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
- writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
-
- writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
- writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
-
- writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]);
- writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]);
-
- setModeTemp(currentMode);
-}
-
-void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) {
- setModeTemp(OPERATION_MODE_CONFIG);
-
- /* Note: Configuration will take place only when user writes to the last
- byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
- Therefore the last byte must be written whenever the user wants to
- changes the configuration. */
-
- writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
- writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
- writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
- writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
- writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
- writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
-
- writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
- writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
- writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
- writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
- writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
- writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
-
- writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
- writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
- writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
- writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
- writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
- writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
-
- writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
- writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
-
- writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
- writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
-
- setModeTemp(currentMode);
-
-}
-
-bool IMUSensor::isFullyCalibrated() {
- uint8_t system, gyro, accel, mag;
- getCalibration(&system, &gyro, &accel, &mag);
-
- switch (currentMode) {
- case OPERATION_MODE_ACCONLY:
- return (accel == 3);
- case OPERATION_MODE_MAGONLY:
- return (mag == 3);
- case OPERATION_MODE_GYRONLY:
- case OPERATION_MODE_M4G: /* No magnetometer calibration required. */
- return (gyro == 3);
- case OPERATION_MODE_ACCMAG:
- case OPERATION_MODE_COMPASS:
- return (accel == 3 && mag == 3);
- case OPERATION_MODE_ACCGYRO:
- case OPERATION_MODE_IMUPLUS:
- return (accel == 3 && gyro == 3);
- case OPERATION_MODE_MAGGYRO:
- return (mag == 3 && gyro == 3);
- default:
- return (system == 3 && gyro == 3 && accel == 3 && mag == 3);
- }
-}
-
-/* Power managments functions */
-void IMUSensor::enterSuspendMode() {
- /* Switch to config mode (just in case since this is the default) */
- setModeTemp(OPERATION_MODE_CONFIG);
- writeRegister(BNO055_PWR_MODE_ADDR, 0x02);
- /* Set the requested operating mode (see section 3.3) */
- setModeTemp(currentMode);
-}
-
-void IMUSensor::enterNormalMode() {
- /* Switch to config mode (just in case since this is the default) */
- setModeTemp(OPERATION_MODE_CONFIG);
- writeRegister(BNO055_PWR_MODE_ADDR, 0x00);
- /* Set the requested operating mode (see section 3.3) */
- setModeTemp(modeback);
-}
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/surfaceFitModel.cpp b/src/unused/surfaceFitModel.cpp
deleted file mode 100644
index d48da49..0000000
--- a/src/unused/surfaceFitModel.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "../include/surfaceFitModel.hpp"
-
-SurfaceFitModel::SurfaceFitModel() {
-
- p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1);
-
- p(0, 0) = -781536.384794701;
- p(1, 0) = 8623.59011973048;
- p(0, 1) = 643.65918253;
- p(2, 0) = -34.3646691281487;
- p(1, 1) = -5.46066535343611;
- p(0, 2) = -0.177121900557321;
- p(3, 0) = 0.0573287698655951;
- p(2, 1) = 0.0150031142038895;
- p(1, 2) = 0.00101871763126609;
- p(0, 3) = 1.63862900553892e-05;
- p(4, 0) = -3.21785828407871e-05;
- p(3, 1) = -1.3161091180883e-05;
- p(2, 2) = -1.42505256569339e-06;
- p(1, 3) = -4.76209793830867e-08;
-}
-
-
-double SurfaceFitModel::getFit(double x, double y) {
-
- return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) +
- p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) +
- p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) +
- p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) +
- p(1, 3) * x * pow(y, 3);
-}
-
-
-
-
-
-
-
-
-
generated by cgit v1.2.3 (git 2.39.1) at 2025-12-12 19:34:46 +0000