Added event logging

Makefile

@@ -160,6 +160,7 @@ CSRC = $(STARTUPSRC) \
        mempools.c \
        worker.c \
        bms.c \
+       events.c \
        $(HWSRC) \
        $(APPSRC) \
        $(NRFSRC) \

conf_general.h

@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		39
+#define FW_TEST_VERSION_NUMBER		40
 
 #include "datatypes.h"
 

events.c

@@ -0,0 +1,119 @@
+/*
+	Copyright 2021 Benjamin Vedder	benjamin@vedder.se
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+#include "events.h"
+#include "terminal.h"
+#include "commands.h"
+#include "utils.h"
+#include "ch.h"
+#include <string.h>
+#include <math.h>
+
+// Settings
+#define EVENTS_LEN	30
+
+// Private types
+typedef struct {
+	const char *name;
+	thread_t *thread;
+	float param;
+	systime_t time;
+	bool set;
+} event_t;
+
+// Private variables
+static volatile event_t m_events[EVENTS_LEN];
+static volatile int m_event_now = 0;
+static mutex_t m_mtx;
+
+// Private functions
+static void terminal_print(int argc, const char **argv);
+
+void events_init(void) {
+	chMtxObjectInit(&m_mtx);
+
+	for (int i = 0;i < EVENTS_LEN;i++) {
+		volatile event_t *e = &m_events[i];
+		e->set = false;
+	}
+
+	terminal_register_command_callback(
+			"events",
+			"Print recent motor events",
+			0,
+			terminal_print);
+}
+
+void events_add(const char *name, float param) {
+	chMtxLock(&m_mtx);
+
+	int event = m_event_now;
+
+	event--;
+	if (event < 0) {
+		event = EVENTS_LEN - 1;
+	}
+	volatile event_t *e = &m_events[event];
+
+	// Just update the last event if it looks like
+	// a repeated command. Otherwise the buffer will
+	// fill too fast.
+	if (e->name != name || // Comparing memory location is enough
+			e->thread != chThdGetSelfX() ||
+			UTILS_AGE_S(e->time) > 0.2 ||
+			(fabsf(param) > 1e-4) != (fabsf(e->param) > 1e-4)) {
+		event = (event + 1) % EVENTS_LEN;
+		e = &m_events[event];
+	}
+
+	e->name = name;
+	e->thread = chThdGetSelfX();
+	e->param = param;
+	e->time = chVTGetSystemTimeX();
+	e->set = true;
+
+	event = (event + 1) % EVENTS_LEN;
+	m_event_now = event;
+
+	chMtxUnlock(&m_mtx);
+}
+
+static void terminal_print(int argc, const char **argv) {
+	(void)argc; (void)argv;
+
+	int event = m_event_now;
+	int print_cnt = 0;
+
+	do {
+		volatile event_t *e = &m_events[event];
+
+		if (e->set) {
+			print_cnt++;
+			commands_printf("Age    : %.2f s", (double)UTILS_AGE_S(e->time));
+			commands_printf("Thread : %s", e->thread->p_name);
+			commands_printf("Motor  : %i", e->thread->motor_selected);
+			commands_printf("Command: %s", e->name);
+			commands_printf("Param  : %.3f\n", (double)e->param);
+		}
+
+		event = (event + 1) % EVENTS_LEN;
+	} while (event != m_event_now);
+
+	commands_printf("Events total: %d\n", print_cnt);
+}

events.h

@@ -0,0 +1,29 @@
+/*
+	Copyright 2021 Benjamin Vedder	benjamin@vedder.se
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+#ifndef EVENTS_H_
+#define EVENTS_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+void events_init(void);
+void events_add(const char *name, float param);
+
+#endif /* EVENTS_H_ */

main.c

@@ -53,6 +53,7 @@
 #endif
 #include "shutdown.h"
 #include "mempools.h"
+#include "events.h"
 
 /*
  * HW resources used:
@@ -205,6 +206,7 @@ int main(void) {
 
 	chThdSleepMilliseconds(100);
 
+	events_init();
 	hw_init_gpio();
 	LED_RED_OFF();
 	LED_GREEN_OFF();

mc_interface.c

@@ -38,6 +38,7 @@
 #include "mempools.h"
 #include "crc.h"
 #include "bms.h"
+#include "events.h"
 
 #include <math.h>
 #include <stdlib.h>
@@ -589,6 +590,8 @@ void mc_interface_set_duty(float dutyCycle) {
 	default:
 		break;
 	}
+
+	events_add("set_duty", dutyCycle);
 }
 
 void mc_interface_set_duty_noramp(float dutyCycle) {
@@ -613,6 +616,8 @@ void mc_interface_set_duty_noramp(float dutyCycle) {
 	default:
 		break;
 	}
+
+	events_add("set_duty_noramp", dutyCycle);
 }
 
 void mc_interface_set_pid_speed(float rpm) {
@@ -637,6 +642,8 @@ void mc_interface_set_pid_speed(float rpm) {
 	default:
 		break;
 	}
+
+	events_add("set_pid_speed", rpm);
 }
 
 void mc_interface_set_pid_pos(float pos) {
@@ -664,6 +671,8 @@ void mc_interface_set_pid_pos(float pos) {
 	default:
 		break;
 	}
+
+	events_add("set_pid_pos", pos);
 }
 
 void mc_interface_set_current(float current) {
@@ -688,6 +697,8 @@ void mc_interface_set_current(float current) {
 	default:
 		break;
 	}
+
+	events_add("set_current", current);
 }
 
 void mc_interface_set_brake_current(float current) {
@@ -717,6 +728,8 @@ void mc_interface_set_brake_current(float current) {
 	default:
 		break;
 	}
+
+	events_add("set_current_brake", current);
 }
 
 /**
@@ -776,6 +789,8 @@ void mc_interface_set_handbrake(float current) {
 	default:
 		break;
 	}
+
+	events_add("set_handbrake", current);
 }
 
 /**
@@ -819,6 +834,8 @@ void mc_interface_release_motor_override(void) {
 	default:
 		break;
 	}
+
+	events_add("release_motor_override", 0.0);
 }
 
 bool mc_interface_wait_for_motor_release(float timeout) {

timeout.c

@@ -209,13 +209,14 @@ static THD_FUNCTION(timeout_thread, arg) {
 		}
 
 		if (kill_sw || (timeout_msec != 0 && chVTTimeElapsedSinceX(last_update_time) > MS2ST(timeout_msec))) {
-			mc_interface_release_motor_override();
+			if (!has_timeout && !kill_sw_active) {
+				mc_interface_release_motor_override();
+			}
 			mc_interface_unlock();
 			mc_interface_select_motor_thread(1);
 			mc_interface_set_brake_current(timeout_brake_current);
 			mc_interface_select_motor_thread(2);
 			mc_interface_set_brake_current(timeout_brake_current);
-			mc_interface_ignore_input(20);
 
 			if (!kill_sw) {
 				has_timeout = true;