Merge pull request #95 from waterloo-rocketry/vn-fix

Vn fix

STM32Cube/Tasks/vn_handler.c

@@ -5,6 +5,7 @@
 
 #include <math.h>
 #include <string.h>
+#include <stdlib.h>
 
 #include "stm32h7xx_hal.h"
 
@@ -15,6 +16,7 @@
 #include "state_estimation.h"
 #include "vn_handler.h"
 #include "can_handler.h"
+#include "millis.h"
 
 
 extern UART_HandleTypeDef huart1;
@@ -76,13 +78,17 @@ static uint16_t decimalFromDouble4(double num){
 }
 
 static double minFromDeg(double deg){
-	return (deg -  (uint32_t)deg)  * 60;
+	double x1=fabs(deg);
+	uint32_t x2 = (uint32_t) fabs(deg);
+	double result = (x1-x2)*60;
+
+	return result;
 }
 
-static void send3VectorStateCanMsg_float(uint64_t time, float vector[3], uint8_t firstStateIDOfVector){
+static void send3VectorStateCanMsg_float(uint32_t time, float vector[3], uint8_t firstStateIDOfVector){
 	can_msg_t msg;
 	for (int i = 0; i < 3; i++) {
-		build_state_est_data_msg((uint32_t) time, &vector[i], firstStateIDOfVector + i, &msg);
+		build_state_est_data_msg(time, &vector[i], firstStateIDOfVector + i, &msg);
 		xQueueSend(busQueue, &msg, MS_WAIT_CAN);
 	}
 }
@@ -141,7 +147,7 @@ void vnIMUHandler(void *argument)
 						if (packetLength == 53){
 							can_msg_t msg;
 
-							uint32_t time_startup = (uint32_t) VnUartPacket_extractUint64(&packet)/ NS_TO_MS; //time in ns -> s
+							uint32_t time_startup = (uint32_t) (VnUartPacket_extractUint64(&packet)/ NS_TO_MS); //time in ns -> s
 
 							uint8_t numSatellites = VnUartPacket_extractInt8(&packet);
 							vec3d pos = VnUartPacket_extractVec3d(&packet);
@@ -165,13 +171,26 @@ void vnIMUHandler(void *argument)
 
 							if (CANGroup1Counter >= CAN_RATE_DIVISOR_GROUP1){
 								//Logging + CAN
-								build_gps_lon_msg((uint32_t) time_startup, (uint8_t) pos.c[1], (uint8_t) minFromDeg(pos.c[1]), decimalFromDouble4(minFromDeg(pos.c[1])), (int) (pos.c[1] >= 0) ? 'N' : 'S', &msg);
+								uint8_t degrees_lon = (uint8_t) abs(pos.c[0]);
+								uint8_t minutes_lon = (uint8_t) minFromDeg(pos.c[0]);
+								uint8_t decimal_min_lon = decimalFromDouble4(minFromDeg(pos.c[0]));
+
+								uint8_t degrees_lat = (uint8_t) abs(pos.c[1]);
+								uint8_t minutes_lat = (uint8_t) minFromDeg(pos.c[1]);
+								uint8_t decimal_min_lat = decimalFromDouble4(minFromDeg(pos.c[1]));
+
+								uint16_t altitude = (uint16_t) abs(pos.c[2]);
+								uint16_t altitude_decimal = decimalFromDouble2(pos.c[2]);
+
+
+								uint32_t msg_ts = (uint32_t) millis_();
+								build_gps_lon_msg(msg_ts, degrees_lon, minutes_lon, decimal_min_lon, (int) (degrees_lon >= 0) ? 'W' : 'E', &msg);
 								xQueueSend(busQueue, &msg, MS_WAIT_CAN);
-								build_gps_lat_msg((uint32_t) time_startup, (uint8_t) pos.c[1], (uint8_t) minFromDeg(pos.c[1]), decimalFromDouble4(minFromDeg(pos.c[1])), (int) (pos.c[1] >= 0) ? 'E' : 'W', &msg);
+								build_gps_lat_msg(msg_ts, degrees_lat, minutes_lat, decimal_min_lat, (int) (degrees_lat >= 0) ? 'N' : 'S', &msg);
 								xQueueSend(busQueue, &msg, MS_WAIT_CAN);
-								build_gps_alt_msg((uint32_t) time_startup, (uint16_t)pos.c[2], decimalFromDouble2(pos.c[2]), (uint8_t) 'M', &msg);
+								build_gps_alt_msg(msg_ts, altitude, altitude_decimal , (uint8_t) 'M', &msg);
 								xQueueSend(busQueue, &msg, MS_WAIT_CAN);
-								build_gps_info_msg((uint32_t) time_startup, numSatellites, quality, &msg);
+								build_gps_info_msg(msg_ts, numSatellites, quality, &msg);
 								xQueueSend(busQueue, &msg, MS_WAIT_CAN);
 
 								CANGroup1Counter = 0;
@@ -218,8 +237,8 @@ void vnIMUHandler(void *argument)
 
 							if (CANGroup3Counter >= CAN_RATE_DIVISOR_GROUP3){
 								//Logging + CAN
-								send3VectorStateCanMsg_float(time_startup_ns, accelVec.c,STATE_ACC_X); //Linear Acceleration
-								send3VectorStateCanMsg_float(time_startup_ns, gyroVec.c,STATE_RATE_YAW); //Angular Velocity
+								send3VectorStateCanMsg_float((uint32_t) millis_(), accelVec.c,STATE_ACC_X); //Linear Acceleration
+								send3VectorStateCanMsg_float((uint32_t) millis_(), gyroVec.c,STATE_RATE_YAW); //Angular Velocity
 								CANGroup3Counter = 0;
 							}