1 /*
2 * Copyright (c) 2021 Kevin Townsend (KTOWN)
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdlib.h>
8 #include <ctype.h>
9 #include <zephyr/shell/shell.h>
10 #include <zsl/orientation/orientation.h>
11
12 #if CONFIG_ZSL_SHELL_ORIENT
13
14 static int
zsl_orient_shell_invalid_arg(const struct shell * shell,char * arg_name)15 zsl_orient_shell_invalid_arg(const struct shell *shell, char *arg_name)
16 {
17 shell_print(shell, "Error: invalid argument \"%s\"\n", arg_name);
18
19 return -EINVAL;
20 }
21
22 static int
zsl_orient_shell_missing_arg(const struct shell * shell,char * arg_name)23 zsl_orient_shell_missing_arg(const struct shell *shell, char *arg_name)
24 {
25 shell_print(shell, "Error: missing argument: \"%s\"\n", arg_name);
26
27 return -EINVAL;
28 }
29
30 static int
zsl_orient_shell_rc_code(const struct shell * shell,int rc)31 zsl_orient_shell_rc_code(const struct shell *shell, int rc)
32 {
33 shell_print(shell, "Error: unexpected return code: \"%d\"\n", rc);
34
35 return -EINVAL;
36 }
37
38 static int
zsl_orient_shell_cmd_lat2g(const struct shell * shell,size_t argc,char ** argv)39 zsl_orient_shell_cmd_lat2g(const struct shell *shell, size_t argc, char **argv)
40 {
41 int rc;
42 zsl_real_t lat = 0.0;
43 zsl_real_t alt = 0.0;
44 zsl_real_t g = 0.0;
45
46 if ((argc == 1) || (strcmp(argv[1], "help") == 0)) {
47 shell_print(shell, "Latitude and altitude to gravity (m/s^2)");
48 shell_print(shell, " $ %s <lat> <altitude>\n", argv[0]);
49 shell_print(shell, "Ex. (Barcelona, ES):");
50 shell_print(shell, " $ %s 41.3851 12.0", argv[0]);
51 shell_print(shell, " 9.794146 m/s^2");
52 return 0;
53 }
54
55 /* Missing argument. */
56 if (argc == 2) {
57 return zsl_orient_shell_missing_arg(shell, "altitude");
58 }
59
60 /* Too many arguments. */
61 if (argc > 3) {
62 return zsl_orient_shell_invalid_arg(shell, argv[3]);
63 }
64
65 /* Invalid latitude. */
66 lat = strtof(argv[1], NULL);
67 if (lat <= 0.0) {
68 return zsl_orient_shell_invalid_arg(shell, argv[1]);
69 }
70
71 /* Invalid altitude. Use sensible Earth limits for validation. */
72 alt = strtof(argv[2], NULL);
73 if ((alt > 10000.0) || (alt < -500.0)) {
74 return zsl_orient_shell_invalid_arg(shell, argv[2]);
75 }
76
77 rc = zsl_grav_lat_alt(lat, alt, &g);
78 if (rc) {
79 return zsl_orient_shell_rc_code(shell, rc);
80 }
81
82 shell_print(shell, "%f m/s^2", g);
83
84 return 0;
85 }
86
87 /* Subcommand array for "orient" (level 1). */
88 SHELL_STATIC_SUBCMD_SET_CREATE(
89 sub_orient,
90 /* 'lat2g' command handler. */
91 SHELL_CMD_ARG(lat2g, NULL, "Latitude and altitude to gravity",
92 zsl_orient_shell_cmd_lat2g, 1, 4),
93
94 /* Array terminator. */
95 SHELL_SUBCMD_SET_END
96 );
97
98 /* Root command "orient" (level 0). */
99 SHELL_CMD_REGISTER(orient, &sub_orient, "Orientation commands", NULL);
100
101 #endif
102
