From e879588ccb174d76ab331dc7c907f2bc99c20419 Mon Sep 17 00:00:00 2001
From: "ucart@co3050-12" <dawehr@iastate.edu>
Date: Wed, 15 Mar 2017 14:48:52 -0500
Subject: [PATCH] Quadcopter can now be disabled and re-enabled without a
restart.
---
quad/sw/modular_quad_pid/src/log_data.c | 8 +++
quad/sw/modular_quad_pid/src/log_data.h | 5 ++
quad/sw/modular_quad_pid/src/main.c | 78 ++++++++++++++-----------
3 files changed, 56 insertions(+), 35 deletions(-)
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 1dd5dc9e8..1c7cfd6d7 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -124,6 +124,10 @@ int log_data(log_t* log_struct, parameter_t* ps)
*/
void printLogging(log_t* log_struct, parameter_t* ps){
+ if (arrayIndex == 0) {
+ // Don't send any logs if nothing was logged
+ return;
+ }
int i;//, j;
char buf[2048] = {};
buf[0] = 0; // Mark buffer as size 0
@@ -168,6 +172,10 @@ void printLogging(log_t* log_struct, parameter_t* ps){
}
}
+void resetLogging() {
+ arrayIndex = 0;
+}
+
int format_log(int idx, log_t* log_struct, char* buf) {
int i;
int end = 0;
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index bb3622c93..0db1751ba 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -49,6 +49,11 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id);
*/
void printLogging(log_t* log_struct, parameter_t* ps);
+ /**
+ * Resets and clears logged data
+ */
+ void resetLogging();
+
/**
* Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
*/
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index 64d82b3a9..52f8c0a83 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -41,9 +41,13 @@ int main()
protection_loops(&structs);
#endif
+ int last_kill_condition = kill_condition(&(structs.user_input_struct));
+
// Main control loop
- do
+ while (1)
{
+ int this_kill_condition = kill_condition(&(structs.user_input_struct));
+
// Processing of loop timer at the beginning of the control loop
timer_start_loop();
@@ -76,59 +80,63 @@ int main()
// Process the sensor data and put it into sensor_struct
sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
- // Run the control algorithm
- control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
- &(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
-
- // Process the commands going to the actuators
- //actuator_command_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
-
- // send the actuator commands
- send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+ if (!this_kill_condition) {
+ // Run the control algorithm
+ control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
+ &(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
+ // send the actuator commands
+ send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+ }
+ else {
+ pwm_kill();
+ }
// update the GUI
update_GUI(&(structs.log_struct));
#endif
// Processing of loop timer at the end of the control loop
timer_end_loop(&(structs.log_struct));
- if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
- {
- // Log the data collected in this loop
- log_data(&(structs.log_struct), &(structs.parameter_struct));
+ if (!this_kill_condition) {
+ if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
+ {
+ // Log the data collected in this loop
+ log_data(&(structs.log_struct), &(structs.parameter_struct));
- static int loop_counter = 0;
- loop_counter++;
+ static int loop_counter = 0;
+ loop_counter++;
- // toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
- if(loop_counter == 10)
- {
- MIO7_led_off();
- }
- else if(loop_counter >= 20)
- {
+ // toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
+ if(loop_counter == 10)
+ {
+ MIO7_led_off();
+ }
+ else if(loop_counter >= 20)
+ {
+ MIO7_led_on();
+ loop_counter = 0;
+ }
+
+ }
+ if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE) {
MIO7_led_on();
- loop_counter = 0;
}
+ }
+ if (this_kill_condition == 1 && last_kill_condition == 0) {
+ // Just disabled
+ printLogging(&(structs.log_struct), &(structs.parameter_struct));
+ resetLogging();
+ MIO7_led_off();
}
- if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE)
- MIO7_led_on();
- } while(
-#ifndef BENCH_TEST
- !kill_condition(&(structs.user_input_struct))
-#else
- 1
-#endif
- );
+ last_kill_condition = this_kill_condition;
+ }
pwm_kill();
- MIO7_led_off();
- printLogging(&(structs.log_struct), &(structs.parameter_struct));
flash_MIO_7_led(10, 100);
--
GitLab