From 2d1a97370d13301d0a882d93b1f3997ea60b710b Mon Sep 17 00:00:00 2001 From: Jonathan Ma Date: Fri, 15 Mar 2024 16:22:45 -0400 Subject: [PATCH] Make lat and lon fields in degrees --- src/ControlTasks/RadioControlTask.cpp | 8 ++--- src/MainControlLoop.cpp | 13 -------- src/Monitors/GPSMonitor.cpp | 45 ++++++++++++++++----------- src/Monitors/IMUMonitor.cpp | 24 +++++++------- src/constants.hpp | 6 ++-- src/main.cpp | 5 +-- src/sfr.cpp | 18 ++--------- 7 files changed, 49 insertions(+), 70 deletions(-) diff --git a/src/ControlTasks/RadioControlTask.cpp b/src/ControlTasks/RadioControlTask.cpp index bae61f3..0062b62 100644 --- a/src/ControlTasks/RadioControlTask.cpp +++ b/src/ControlTasks/RadioControlTask.cpp @@ -263,8 +263,8 @@ void RadioControlTask::execute() bool RadioControlTask::executeDownlink() { - uint16_t lat = map_range_16(sfr::gps::latitude, constants::gps::lat_min, constants::gps::lat_max); - uint16_t lon = map_range_16(sfr::gps::longitude, constants::gps::lon_min, constants::gps::lon_max); + uint16_t lat = map_range_16(sfr::gps::latitude * 10, constants::gps::lat_min, constants::gps::lat_max); + uint16_t lon = map_range_16(sfr::gps::longitude * 10, constants::gps::lon_min, constants::gps::lon_max); uint16_t alt = map_range_16(sfr::gps::altitude / 10, constants::gps::alt_min, constants::gps::alt_max); uint8_t flags = ((sfr::radio::mode == radio_mode_type::listen) ? 0xF0 : 0x00) | (sfr::gps::valid_msg ? 0x0F : 0x00); @@ -302,9 +302,9 @@ uint8_t RadioControlTask::map_range(float value, int min_val, int max_val) uint16_t RadioControlTask::map_range_16(float value, int min_val, int max_val) { - long raw = map(value, min_val, max_val, 0, 65536); + long raw = map(value, min_val, max_val, 0, 65535); Serial.println(raw); - raw = min(raw, 65536); + raw = min(raw, 65535); raw = max(raw, 0); return (uint16_t)raw; } diff --git a/src/MainControlLoop.cpp b/src/MainControlLoop.cpp index d527730..87f4c20 100644 --- a/src/MainControlLoop.cpp +++ b/src/MainControlLoop.cpp @@ -9,13 +9,6 @@ MainControlLoop::MainControlLoop() { } -int freeRam() -{ - extern int __heap_start, *__brkval; - int v; - return (int)&v - (__brkval == 0 ? (int)&__heap_start : (int)__brkval); -} - void MainControlLoop::execute() { @@ -56,9 +49,6 @@ void MainControlLoop::execute() Serial.print("UTC Time: "); Serial.println(sfr::gps::utc_time); - Serial.print("Free RAM: "); - Serial.println(freeRam()); - #endif temp_monitor.execute(); @@ -68,9 +58,6 @@ void MainControlLoop::execute() wdt_reset(); - // delay(5000); - // Serial.println("you should not be seeing this"); - #ifdef VERBOSE Serial.println(F("-------------------- END LOOP --------------------")); #endif diff --git a/src/Monitors/GPSMonitor.cpp b/src/Monitors/GPSMonitor.cpp index 2e5348f..898ca8b 100644 --- a/src/Monitors/GPSMonitor.cpp +++ b/src/Monitors/GPSMonitor.cpp @@ -71,46 +71,52 @@ bool GPSMonitor::encode(char c) strncpy(temp_buf, term_buffer, 10); temp_buf[10] = '\0'; sfr::gps::utc_time = atof(temp_buf); - Serial.println(atof(temp_buf)); + Serial.println(sfr::gps::utc_time); break; case 2: // Latitude value { - Serial.println("Latitude val"); + Serial.println("Latitude"); - char temp_buf[11]; - strncpy(temp_buf, term_buffer, 10); - temp_buf[10] = '\0'; - // sfr::gps::latitude = atof(temp_buf); - Serial.println(atof(temp_buf)); + char dd_buf[3] = {term_buffer[0], term_buffer[1], '\0'}; + char mm_buf[3] = {term_buffer[2], term_buffer[3], '\0'}; + // char dd_buf[3]; + // char mm_buf[3]; + // strncpy(dd_buf, term_buffer, 2); + // strncpy(mm_buf, term_buffer + 2, 2); + // dd_buf[2] = '\0'; + // mm_buf[2] = '\0'; + + sfr::gps::latitude = atoi(dd_buf) + atoi(mm_buf) / 60.0; + Serial.println(sfr::gps::latitude); break; } case 3: // Latitude hemisphere { if (term_buffer[0] == 'S') { - // sfr::gps::latitude = -sfr::gps::latitude; - // Serial.println(latitude); + sfr::gps::latitude = -sfr::gps::latitude; + Serial.println(sfr::gps::latitude); } break; } case 4: // Longitude value { - Serial.println("Longitude val"); + Serial.println("Longitude"); + + char ddd_buf[4] = {term_buffer[0], term_buffer[1], term_buffer[2], '\0'}; + char mm_buf[3] = {term_buffer[3], term_buffer[4], '\0'}; - char temp_buf[12]; - strncpy(temp_buf, term_buffer, 11); - temp_buf[11] = '\0'; - // sfr::gps::longitude = atof(temp_buf); - Serial.println(atof(temp_buf)); + sfr::gps::longitude = atoi(ddd_buf) + atoi(mm_buf) / 60.0; + Serial.println(sfr::gps::longitude); break; } case 5: // Longitude hemisphere { if (term_buffer[0] == 'W') { - // sfr::gps::longitude = -sfr::gps::longitude; - // Serial.println(longitude); + sfr::gps::longitude = -sfr::gps::longitude; + Serial.println(sfr::gps::longitude); } break; } @@ -121,8 +127,9 @@ bool GPSMonitor::encode(char c) char temp_buf[char_count]; strncpy(temp_buf, term_buffer, char_count - 1); temp_buf[char_count - 1] = '\0'; - // sfr::gps::altitude = atof(temp_buf); - Serial.println(atof(temp_buf)); + + sfr::gps::altitude = atof(temp_buf); + Serial.println(sfr::gps::altitude); break; } diff --git a/src/Monitors/IMUMonitor.cpp b/src/Monitors/IMUMonitor.cpp index 55ba04e..a9a574e 100644 --- a/src/Monitors/IMUMonitor.cpp +++ b/src/Monitors/IMUMonitor.cpp @@ -53,19 +53,19 @@ void IMUMonitor::execute() void IMUMonitor::capture_imu_values() { - // if (IMU.gyroscopeAvailable()) { // check if the gyroscope has new data available - // IMU.readGyroscope( - // sfr::imu::gyro_x, - // sfr::imu::gyro_y, - // sfr::imu::gyro_z); // data is in degrees/s - // } + if (IMU.gyroscopeAvailable()) { // check if the gyroscope has new data available + IMU.readGyroscope( + sfr::imu::gyro_x, + sfr::imu::gyro_y, + sfr::imu::gyro_z); // data is in degrees/s + } - // if (IMU.accelerationAvailable()) { // check if accelerometer is available - // IMU.readAcceleration( - // sfr::imu::acc_x, - // sfr::imu::acc_y, - // sfr::imu::acc_z); // data is in m/s^2 - // } + if (IMU.accelerationAvailable()) { // check if accelerometer is available + IMU.readAcceleration( + sfr::imu::acc_x, + sfr::imu::acc_y, + sfr::imu::acc_z); // data is in m/s^2 + } } void IMUMonitor::transition_to_normal() diff --git a/src/constants.hpp b/src/constants.hpp index a7f3a5a..ace942d 100644 --- a/src/constants.hpp +++ b/src/constants.hpp @@ -40,13 +40,13 @@ namespace constants { constexpr int tx_pin = 3; constexpr int buffer_size = 11; - constexpr int lat_min = -9000; + constexpr int lat_min = -9000; // In (10 * degrees) constexpr int lat_max = 9000; - constexpr int lon_min = -18000; + constexpr int lon_min = -18000; // In (10 * degrees) constexpr int lon_max = 18000; constexpr int alt_min = 0; constexpr int alt_max = 50000; // In 10's of meters - } // namespace gps + } // namespace gps namespace opcodes { constexpr int no_op = 0x00; constexpr int change_downlink_period = 0x11; diff --git a/src/main.cpp b/src/main.cpp index dd90b6e..c60cd4c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,8 +3,6 @@ MainControlLoop mcl; -#ifndef UNIT_TEST - void setup() { wdt_disable(); @@ -16,5 +14,4 @@ void setup() void loop() { mcl.execute(); -} -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/sfr.cpp b/src/sfr.cpp index c2b691f..214bda6 100644 --- a/src/sfr.cpp +++ b/src/sfr.cpp @@ -4,14 +4,8 @@ namespace sfr { namespace imu { - // TEMP Values - float gyro_x = -250; - float gyro_y = 250; - float gyro_z = 500; - - float acc_x = 30; - float acc_y = 0; - float acc_z = -50; + float gyro_x, gyro_y, gyro_z; + float acc_x, acc_y, acc_z; bool initialized = false; uint16_t failed_times = 0; @@ -25,13 +19,7 @@ namespace sfr { float temp_c; } // namespace temperature namespace gps { - float utc_time; - - // TEMP values - float latitude = 9000; - float longitude = 18000; - float altitude = 500000; - + float utc_time, latitude, longitude, altitude; bool valid_msg = false; } // namespace gps