#include #include #include "sphere_shell.h" #include #include #include #include #include "sensor_task.h" #include "ahrs_task.h" #include "rf_task.h" #include "parameters.h" static void cmd_mem(BaseSequentialStream *chp, int argc, char *argv[]) { size_t n, size; n = chHeapStatus(NULL, &size); chprintf(chp, "core free memory : %u bytes\r\n", chCoreStatus()); chprintf(chp, "heap fragments : %u\r\n", n); chprintf(chp, "heap free total : %u bytes\r\n", size); } extern unsigned long __main_thread_stack_base__, __main_thread_stack_end__; // From linker script static void thread_free_stack(Thread *thread, int *free_stack_now, int *free_stack_min) { uint32_t current_sp = (uint32_t)thread->p_ctx.r13; uint32_t stack_bottom; if (current_sp >= (uint32_t)&__main_thread_stack_base__ && current_sp <= (uint32_t)&__main_thread_stack_end__) stack_bottom = (uint32_t)&__main_thread_stack_base__; else stack_bottom = (uint32_t)(thread + 1); *free_stack_now = current_sp - stack_bottom; uint32_t *stackentry = (uint32_t*)stack_bottom; uint32_t empty_val = *stackentry; while (*stackentry == empty_val) stackentry++; *free_stack_min = (uint32_t)stackentry - stack_bottom; } static void cmd_threads(BaseSequentialStream *chp, int argc, char *argv[]) { static const char *states[] = {THD_STATE_NAMES}; Thread *tp; chprintf(chp, " addr stack prio refs state time free stack now/min\r\n"); tp = chRegFirstThread(); do { int stacknow, stackmin; thread_free_stack(tp, &stacknow, &stackmin); chprintf(chp, "%8s %.8lx %.8lx %4lu %4lu %9s %8lu %lu/%lu\r\n", tp->p_name, (uint32_t)tp, (uint32_t)tp->p_ctx.r13, (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1), states[tp->p_state], (uint32_t)tp->p_time, stacknow, stackmin); tp = chRegNextThread(tp); } while (tp != NULL); } static void cmd_sensors(BaseSequentialStream *chp, int argc, char *argv[]) { chSysLock(); int mx = mag_x; int my = mag_y; int mz = mag_z; int ax = acc_x; int ay = acc_y; int az = acc_z; chSysUnlock(); chprintf(chp, "Magnetometer: %5d %5d %5d\r\n", mx, my, mz); chprintf(chp, "Accelerometer: %5d %5d %5d\r\n", ax, ay, az); mx = 1000 * (mx - AHRS_MAG_BIAS_X) / AHRS_MAG_SCALE_X; my = 1000 * (my - AHRS_MAG_BIAS_Y) / AHRS_MAG_SCALE_Y; mz = 1000 * (mz - AHRS_MAG_BIAS_Z) / AHRS_MAG_SCALE_Z; ax = 1000 * (ax - AHRS_ACC_BIAS_X) / AHRS_ACC_SCALE_X; ay = 1000 * (ay - AHRS_ACC_BIAS_Y) / AHRS_ACC_SCALE_Y; az = 1000 * (az - AHRS_ACC_BIAS_Z) / AHRS_ACC_SCALE_Z; chprintf(chp, "Calib. mag.: %5d %5d %5d\r\n", mx, my, mz); chprintf(chp, "Calib. acc.: %5d %5d %5d\r\n", ax, ay, az); } static void cmd_sensor_dump(BaseSequentialStream *chp, int argc, char *argv[]) { BaseChannel *ch = (BaseChannel*)chp; chprintf(chp, "# Timestamp MagX, MagY, MagZ, AccX, AccY, AccZ\r\n"); char b = 0; do { chprintf(chp, "%12d %6d %6d %6d %6d %6d %6d\r\n", chTimeNow(), mag_x, mag_y, mag_z, acc_x, acc_y, acc_z); b = chnGetTimeout(ch, MS2ST(10)); } while (b != Q_RESET && b != '\r'); } static void cmd_stats(BaseSequentialStream *chp, int argc, char *argv[]) { unsigned time = (chTimeNow() - sensor_start_time) / 1000; chprintf(chp, "Sensor readings: %6d (%3d/s)\r\n", sensor_readings, sensor_readings / time); chprintf(chp, "Magnetometer errors: %6d (%3d/s)\r\n", mag_errors, mag_errors / time); chprintf(chp, "Accelerometer errors: %6d (%3d/s)\r\n", acc_errors, acc_errors / time); time = (chTimeNow() - ahrs_start_time) / 1000; chprintf(chp, "Kalman updates: %6d (%3d/s)\r\n", ahrs_updates, ahrs_updates / time); chprintf(chp, "RF packets: %6d\r\n", rf_packets); } static void cmd_ahrs(BaseSequentialStream *chp, int argc, char *argv[]) { chSysLock(); fix16_t n = ahrs_north; fix16_t e = ahrs_east; int i = fix16_to_int(ahrs_inclination); chSysUnlock(); chprintf(chp, "North: %5d\r\n", fix16_mul(n, 1000)); chprintf(chp, "East: %5d\r\n", fix16_mul(e, 1000)); chprintf(chp, "Magnetic inclination: %5d\r\n", i); } static void cmd_ahrs_dump(BaseSequentialStream *chp, int argc, char *argv[]) { BaseChannel *ch = (BaseChannel*)chp; chprintf(chp, "# Timestamp North, East, Incl\r\n"); char b = 0; do { chSysLock(); fix16_t n = ahrs_north; fix16_t e = ahrs_east; int i = fix16_to_int(ahrs_inclination); chSysUnlock(); chprintf(chp, "%12d %6d %6d %6d\r\n", chTimeNow(), fix16_mul(n, 1000), fix16_mul(e, 1000), i); b = chnGetTimeout(ch, MS2ST(10)); } while (b != Q_RESET && b != '\r'); } static void cmd_rf(BaseSequentialStream *chp, int argc, char *argv[]) { chprintf(chp, "RF heading: %4d\r\n", rf_heading); chprintf(chp, "RF speed: %4d\r\n", rf_speed); } static void cmd_parameters(BaseSequentialStream *chp, int argc, char *argv[]) { bool save = false, load = false, backup = false; if (argc == 1) { save = (strcmp(argv[0], "save") == 0); load = (strcmp(argv[0], "load") == 0); backup = (strcmp(argv[0], "backup") == 0); } if (save) { parameters_save(); } else if (load) { if (!parameters_load()) { chprintf(chp, "Load failed, invalid signature.\r\n"); } } else if (backup) { parameters_print(chp, true); } else { chprintf(chp, "Usage: parameters [load|save|backup]\r\n"); if (argc == 0) { parameters_print(chp, false); } } } static void cmd_get(BaseSequentialStream *chp, int argc, char *argv[]) { if (argc != 1) { chprintf(chp, "Usage: get . Type 'parameters' for a list.\r\n"); return; } int32_t value; if (parameter_get(argv[0], &value)) chprintf(chp, "%s: %6d\r\n", argv[0], value); else chprintf(chp, "No such parameter: %s\r\n", argv[0]); } static void cmd_set(BaseSequentialStream *chp, int argc, char *argv[]) { if (argc != 2) { chprintf(chp, "Usage: set . Type 'parameters' for a list.\r\n"); return; } int32_t value = atoi(argv[1]); if (parameter_set(argv[0], value)) chprintf(chp, "%s: %6d\r\n", argv[0], value); else chprintf(chp, "No such parameter: %s\r\n", argv[0]); } const ShellCommand shell_commands[] = { {"mem", cmd_mem}, {"threads", cmd_threads}, {"sensors", cmd_sensors}, {"sensor_dump", cmd_sensor_dump}, {"stats", cmd_stats}, {"ahrs", cmd_ahrs}, {"ahrs_dump", cmd_ahrs_dump}, {"rf", cmd_rf}, {"parameters", cmd_parameters}, {"get", cmd_get}, {"set", cmd_set}, {NULL, NULL} };