Skip to content

Commit

Permalink
Competition code
Browse files Browse the repository at this point in the history
  • Loading branch information
@peanat-byte
peanat-byte committed Jun 26, 2024
1 parent e625b0d commit 05897c9
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 103 deletions.
95 changes: 17 additions & 78 deletions src/SUAS_Link_2024/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,16 +85,7 @@ char buffer[100]; // For printing things
void loop() {

// Read drop point and bottle number from Pi (blocking)
// drop_data = recieveData();

// drop_data.lon = -76.5207332; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
// drop_data.lat = 38.3022803;
// drop_data.lon = -76.5494557; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! ,
// drop_data.lat = 38.3140069;
drop_data.lon = -76.5216808; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! ,
drop_data.lat = 38.3012800;
drop_data.heading = 0.0;
drop_data.bottleID = 2;
drop_data = recieveData();

bool notDropped = true;
snprintf(buffer, sizeof(buffer), "Received data from Pi: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer);
Expand All @@ -106,50 +97,13 @@ void loop() {
// Serial.println("Wind: " + String(windspeed) + ", " + String(wind_heading));
// calc_des_drop_state(windspeed, wind_heading, drop_data, &des_drop_data);

calc_des_drop_state(0, 0, drop_data, &des_drop_data); // TODO: REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// Serial.println(latitudeToMeters(des_drop_data.lat)-latitudeToMeters(drop_data.lat));
// Serial.println(latitudeToMeters(des_drop_data.lon)-latitudeToMeters(drop_data.lon));
calc_des_drop_state(0, 0, drop_data, &des_drop_data);

// Send desired drop point to Pi
snprintf(buffer, sizeof(buffer), "%.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID);
PiSerial.print(buffer);
Serial.print("Sent desired drop point to Pi (not): "); Serial.print(buffer);

// Send message to parachutes (try 3 times; this is time sensitive so nothing we can do if it fails)
for (int i = 0; i < 3; i++) {
broadcastMessage(des_drop_data);
delay(500);
}

// REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
des_drop_data.bottleID = 3;
for (int i = 0; i < 3; i++) {
broadcastMessage(des_drop_data);
delay(500);
}
des_drop_data.bottleID = 4;
for (int i = 0; i < 3; i++) {
broadcastMessage(des_drop_data);
delay(500);
}

// struct_message drop_data_2;
// drop_data_2.lon = test_lon; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! ,
// drop_data_2.lat = test_lat;
// drop_data_2.heading = 0.0;
// drop_data_2.bottleID = 3;
// for (int i = 0; i < 3; i++) {
// broadcastMessage(drop_data_2);
// }
// struct_message drop_data_4;
// drop_data_4.lon = test_lon; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!! ,
// drop_data_4.lat = test_lat;
// drop_data_4.heading = 0.0;
// drop_data_4.bottleID = 4;
// for (int i = 0; i < 3; i++) {
// broadcastMessage(drop_data_4);
// }

// Wait to get close enough to desired drop point
digitalWrite(LED_RED, HIGH);
while (notDropped) {
Expand All @@ -159,36 +113,21 @@ void loop() {
double lon = (double) msg.global_position_int.lon / 10000000.0;
// Serial.println("Coords: " + String(lat, 8) + ", " + String(lon, 8));

if (FAILSAFE_MODE) {
if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the right bottle
des_drop_data.bottleID = 1;
notDropped = false;
Serial.println("Failsafe mode: Right bottle dropped.");
}
if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the left bottle
// drop_data.lon = msg.global_position_int.lon / 10000000.0; // For testing REMOVE LATER!!!!!!!!!!!!!!!!!!!!!!!
// drop_data.lat = msg.global_position_int.lat / 10000000.0;
// drop_data.heading = 0.0;
// drop_data.bottleID = 2;
// snprintf(buffer, sizeof(buffer), "Received data from failsafe activation: %.8f,%.8f,%.2f,%d\n", drop_data.lat, drop_data.lon, drop_data.heading, drop_data.bottleID); Serial.print(buffer);
// calc_des_drop_state(1, 0, drop_data, &des_drop_data);
// snprintf(buffer, sizeof(buffer), "Destination: %.8f,%.8f,%.2f,%d\n", des_drop_data.lat, des_drop_data.lon, des_drop_data.heading, des_drop_data.bottleID); Serial.print(buffer);
// for (int i = 0; i < 3; i++) {
// broadcastMessage(des_drop_data);
// }

des_drop_data.bottleID = 2;
notDropped = false;
Serial.println("Failsafe mode: Left bottle dropped.");
}
}
else {
double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon);
Serial.println("Dist: " + String(dist_from_drop_point));
if (dist_from_drop_point < RELEASE_MARGIN) {
notDropped = false;
Serial.println("Reached drop point");
}
if (msg.servo_output_raw.servo13_raw > 1500) { // If servo 13 is high, then drop the left bottle
des_drop_data.bottleID = 1;
notDropped = false;
Serial.println("Failsafe mode: Right bottle dropped.");
}
if (msg.servo_output_raw.servo14_raw > 1500) { // If servo 14 is high, then drop the right bottle
des_drop_data.bottleID = 2;
notDropped = false;
Serial.println("Failsafe mode: Left bottle dropped.");
}
double dist_from_drop_point = distance(lat, lon, des_drop_data.lat, des_drop_data.lon);
Serial.println("Dist: " + String(dist_from_drop_point));
if (dist_from_drop_point < RELEASE_MARGIN) {
notDropped = false;
Serial.println("Reached drop point");
}
}
digitalWrite(LED_RED, LOW);
Expand Down
2 changes: 1 addition & 1 deletion src/SUAS_Link_2024/src/params.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ extern HardwareSerial CubeSerial;

#define WINDOW_SIZE 10

#define FAILSAFE_MODE true
#define FAILSAFE_MODE false

// MAC Address of responder - edit as required
static const uint8_t ADDRESS_1[] = {0xD8, 0xBC, 0x38, 0xE4, 0x9E, 0x5C}; // Parachute #1
Expand Down
48 changes: 24 additions & 24 deletions src/main/sensors.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,30 +183,30 @@ SENSORS_Status_t sensors::initIMU(){
SERIAL_PORT.println(F("EEPROM.begin failed! Bias Save and Restore not possible"));
}
EEPROM.get(0, sensorData.imuData.IMUDmpBias); //read the bias if stored
if(isBiasStoreValid(&sensorData.imuData.IMUDmpBias)){
SERIAL_PORT.println(F("Bias data in EEPROM is valid. Restoring it..."));
success &= (imu.setBiasGyroX(sensorData.imuData.IMUDmpBias.biasGyroX) == ICM_20948_Stat_Ok);
success &= (imu.setBiasGyroY(sensorData.imuData.IMUDmpBias.biasGyroY) == ICM_20948_Stat_Ok);
success &= (imu.setBiasGyroZ(sensorData.imuData.IMUDmpBias.biasGyroZ) == ICM_20948_Stat_Ok);
success &= (imu.setBiasAccelX(sensorData.imuData.IMUDmpBias.biasAccelX) == ICM_20948_Stat_Ok);
success &= (imu.setBiasAccelY(sensorData.imuData.IMUDmpBias.biasAccelY) == ICM_20948_Stat_Ok);
success &= (imu.setBiasAccelZ(sensorData.imuData.IMUDmpBias.biasAccelZ) == ICM_20948_Stat_Ok);
success &= (imu.setBiasCPassX(sensorData.imuData.IMUDmpBias.biasCPassX) == ICM_20948_Stat_Ok);
success &= (imu.setBiasCPassY(sensorData.imuData.IMUDmpBias.biasCPassY) == ICM_20948_Stat_Ok);
success &= (imu.setBiasCPassZ(sensorData.imuData.IMUDmpBias.biasCPassZ) == ICM_20948_Stat_Ok);
if (success)
{
SERIAL_PORT.println(F("Biases restored."));
printBiases(&sensorData.imuData.IMUDmpBias);
sensorData.imuData.imuBiasFoundinEEPROM = true;
delay(2000);
}
else{
SERIAL_PORT.println(F("Bias restore failed!"));
sensorData.imuData.imuBiasFoundinEEPROM = false;
delay(2000);
}
}
// if(isBiasStoreValid(&sensorData.imuData.IMUDmpBias)){
// SERIAL_PORT.println(F("Bias data in EEPROM is valid. Restoring it..."));
// success &= (imu.setBiasGyroX(sensorData.imuData.IMUDmpBias.biasGyroX) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasGyroY(sensorData.imuData.IMUDmpBias.biasGyroY) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasGyroZ(sensorData.imuData.IMUDmpBias.biasGyroZ) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasAccelX(sensorData.imuData.IMUDmpBias.biasAccelX) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasAccelY(sensorData.imuData.IMUDmpBias.biasAccelY) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasAccelZ(sensorData.imuData.IMUDmpBias.biasAccelZ) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasCPassX(sensorData.imuData.IMUDmpBias.biasCPassX) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasCPassY(sensorData.imuData.IMUDmpBias.biasCPassY) == ICM_20948_Stat_Ok);
// success &= (imu.setBiasCPassZ(sensorData.imuData.IMUDmpBias.biasCPassZ) == ICM_20948_Stat_Ok);
// if (success)
// {
// SERIAL_PORT.println(F("Biases restored."));
// printBiases(&sensorData.imuData.IMUDmpBias);
// sensorData.imuData.imuBiasFoundinEEPROM = true;
// delay(2000);
// }
// else{
// SERIAL_PORT.println(F("Bias restore failed!"));
// sensorData.imuData.imuBiasFoundinEEPROM = false;
// delay(2000);
// }
// }
SERIAL_PORT.println("IMU init successful");
return status;
}
Expand Down

0 comments on commit 05897c9

Please sign in to comment.