/****************************************************************************/ /* Copyright (c) 2018 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : Power Buoy controller app entry point. */ /* Filename : pblog.cpp */ /* Author : Henthorn */ /* Project : Power Buoy */ /* Version : 1.0 */ /* Created : 05/13/2018 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ /* */ /* */ /****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define USE_NN 1 #if USE_NN #include #include // The url used for nanomsg IPC comms // #define NN_URL "ipc:///tmp/pbcon.ipc" #endif #include "Crossbow.hpp" #include "CanBus.hpp" #include "RangeEstimator.hpp" #include "PID.hpp" // Making use of existing pbcmd commands // extern "C" { #include "pbcmd.h" } #define OK 0 #define BAD_SOCK -1 // Socket comms failed #define BAD_OPTS -2 // Inconsistent option values #define DEFAULT_BASE_DIR "/home/buoy/Logs" #define DEFAULT_INTERVAL 60*60 #define DEFAULT_HIGH_LIMIT 70.0 #define DEFAULT_LOW_LIMIT 50.0 #define MODE_CMD "mode " #define ACK "ACK" #define NACK "NACK" #define RAM_LIMIT 80.0 #define HI_TARGET_REPS 3 #define HYDROGEN_LIMIT 10.0 enum Modes {MODE_INVALID, RANGE_CONTROL, DEPTH_CONTROL, POWER_CONTROL}; // Taken from Andy // struct TF_StatusBits { unsigned int ChargeMode : 2; // offset = 0 unsigned int ChargeEnabled : 1; // offset = 2 unsigned int MaxonPwrMode : 2; // offset = 3 unsigned int MaxonPower : 1; // offset = 5 unsigned int MaxonEnabled : 1; // offset = 6 unsigned int DoorPositionTarget : 1; // offset = 7 unsigned int DoorPositionActual : 1; // offset = 8 unsigned int State : 4; // offset = 9 unsigned int Maxon_ComError : 2; // offset = 13 }; typedef union { unsigned int status; struct TF_StatusBits bits; } TF_Status; // Structure contains the options: // verbose : Be chatty? // struct ConOptions { char verbose; char execute; // When true, execute the set_rpm command char watchdog; // When true, feed watchdog every 60-70 seconds char open_only; // Open-only: never send a command to close the doors Modes mode; // Controller mode double hi_limit; // Ram range hi limit double lo_limit; // Ram range lo limit unsigned hyst_time; // Required time SC range must be below low point }; int parse_options(int argc, char **argv); // Returns Mode according to the string (range, depth, power) // Returns the Mode value if successful. < 0 if invalid Modes get_mode(const char* mode_str); void usage() { printf("usage:\n"); printf(" pbcon [-v -x -m mode]\n"); printf(" -v Verbose - Be chatty\n"); printf(" -x Execute - Execute controller commands (default is NOT to execute, just report)\n"); printf(" -w Watchdog - Feed the watchdog every minute or so (default: no watchdog\n"); printf(" -o Open-only - Never send the command to close the doors (default is execute close)\n"); printf(" -h High limit - Ram range (inches) at which to automatically open doors (%.1f)\n", DEFAULT_HIGH_LIMIT); printf(" -l Low limit - Ram range (inches) at which to automatically close doors (%.1f)\n", DEFAULT_LOW_LIMIT); printf(" -t Low limit time - Time (seconds) range less than low point before closing doors (%d)\n", DEFAULT_INTERVAL); } void cleanup(int sig) { exit(0); } static struct ConOptions _options = {false, false, false, false, RANGE_CONTROL, DEFAULT_HIGH_LIMIT, DEFAULT_LOW_LIMIT, DEFAULT_INTERVAL}; static int _cmd_sock, _rv; // Send command to open or close the heave cone doors. // 0 == close doors, anything else means open. // If the cmd is zero, the close command will be sent. // Otherwise, open. We do not attempt to hold the state of // the doors here, since a command to open/close can // be sent outside of this app. // char _opened = 0; char _closed = 1; void open_doors(int cmd) { char *argv[2]; char zc[10]; argv[0] = (char*)"tf_SetPos"; argv[1] = zc; int argc = 2; // Do not close doors if the open-only option is enabled. // Return now if the command is to close the doors. // if (_options.open_only && cmd == 0) return; fprintf(stdout, "%s(%s) (%s really executing)\n", argv[0], argv[1], _options.execute ? "" : "Not"); if (cmd != 0) sprintf(zc, "%d", 0); else sprintf(zc, "%d", 1); printf("%s %s\n", argv[0], argv[1]); if (_options.execute) tf_setpos(argc, argv); else {_opened = cmd; _closed = !_opened;} sleep(2); } /////////////////////////////////////////////////////////////////////////////// // int main(int argc, char **argv) { unsigned int deci = 0; signal(SIGINT, cleanup); signal(SIGQUIT, cleanup); signal(SIGKILL, cleanup); //////////////////////////////////////////////////////////////////////////// /////////////////////// HIGH-LEVEL SETUP ///////////////////////////////// // Check cmdline options and optional base directory argument // if (parse_options(argc, argv) != 0) { usage(); return -1; } char verbose = _options.verbose; int max_fd = 0, fd; //////////////////////////////////////////////////////////////////////////// //////////////////////////// SETUP Readers //////////////////////////////// // Create the data readers, one for each data source. // Crossbow xbow(NULL); if ( (fd = xbow.get_fd()) < 0) perror("pblog: Failed to open Crossbow port: "); else if (fd > max_fd) max_fd = fd; // Track maximum fd for FD_SET CanBus can("can0", NULL); if ( (fd = can.get_fd()) < 0) perror("pblog: Failed to open CanBus port: "); else if (fd > max_fd) max_fd = fd; // Track maximum fd for FD_SET #if USE_NN _cmd_sock = nn_socket(AF_SP, NN_REP); _rv = nn_bind(_cmd_sock, NN_URL); printf("NN_REP cmd_sock = %d %d\n", _cmd_sock, _rv); #endif //////////////////////////////////////////////////////////////////////////// //////////////////////////// MAIN LOOP //////////////////////////////////// // loop (forever) // if (there is data for the readers to read) // let readers read new data // endif // end loop // RangeEstimator range_estimator; float z; PID range_pid(100.0, 0., 0.); PID depth_pid(0.5, 0., 0.); double pid_cmd = 0.0, depth_target = 30.; double range_htarget = _options.hi_limit, range_ltarget = _options.lo_limit; time_t now, doors_closed = 0, doors_opened = 0, hyst_time = time_t(_options.hyst_time); unsigned int hi_count = 0, wd_count = 0;; // first, get the latest TF data with the status of the doors // bool haveTF = false; time_t last_hydro_warning = 0; // while (1) //{ // if (can.read_data(_options.verbose) == TF_2) break; //} //can.get_record(&tf_rec, TF_2); while(1) { Modes mode = RANGE_CONTROL; CanBus::SC_Record sc_rec; CanBus::BC_Record bc_rec; CanBus::PC_Record pc_rec; CanBus::TF_Record tf_rec; Crossbow::XB_Record xb_rec; struct timeval timeout = {0, 25000L}; // 25 ms period (40Hz) fd_set readset; FD_ZERO(&readset); char slackcmd[500]; strcpy(slackcmd, ""); // Specifiy the ports to select on // if ( (fd = can.get_fd()) > 0 ) FD_SET(fd, &readset); // See if any ports have data waiting // if (select((max_fd + 1), &readset, NULL, NULL, &timeout) > 0) { // Get CanBus data // A return value < 0 signifies that a new data record has completed // if (FD_ISSET(can.get_fd(), &readset)) { // Feed the watchdog every so often. // Normally, the canbus delivers 1000 packets // in about 7 seconds. Let's wait for around a minute. // if (++wd_count > 10000) { wd_count = 0; if (_options.watchdog) { tf_watchdog(0, NULL); time_t tt = time(NULL); printf("Watchdog @ %s", ctime(&tt)); fflush(stdout); } } #if 0 // New Spring controller range data (SC_RANGE)? Trigger control process // if (can.read_data(_options.verbose) == SC_RANGE) { can.get_record(&sc_rec, SC_RANGE); z = (float) range_estimator.RLS_Range((double) sc_rec.range, (double) sc_rec.upsi, (double) sc_rec.lpsi); pid_cmd = range_pid.CommandSignal(range_target, z); char *argv[2]; char zc[10]; sprintf(zc, "%.4f", (pid_cmd)); argv[0] = (char*)"set_rpm"; argv[1] = zc; int argc = 2; fprintf(stderr, "%s(%s) (%s really executing)\n", argv[0], argv[1], _options.execute ? "" : "Not"); if (_options.execute) set_rpm(argc, argv); double xh0, xh1; range_estimator.GetxHat(&xh0, &xh1); } #endif int rec_type = can.read_data(_options.verbose); // Get latest trefoil record when available // if (rec_type == TF_2) { can.get_record(&tf_rec, TF_2); haveTF = true; } // If available, read new Battery controller hydrogen data to Trigger control process // else if (rec_type == BC_HYDROGEN) { can.get_record(&bc_rec, BC_HYDROGEN); if (bc_rec.hydrogen > HYDROGEN_LIMIT && (last_hydro_warning + 300) < time(0)) { last_hydro_warning = time(0); sprintf(slackcmd, "echo \"%s %.1f > %.1f @ %s\" | slacktee.sh", "WARNING: Battery hydrogen limit exceeded!", bc_rec.hydrogen, HYDROGEN_LIMIT, ctime(&last_hydro_warning)); printf("%s\n", slackcmd); } } // If available, read Spring controller range data to Trigger control process // else if (rec_type == SC_RANGE && haveTF) { can.get_record(&sc_rec, SC_RANGE); // What is the status of the doors? // TF_Status tf_stat; tf_stat.status = tf_rec.status; char opened = tf_stat.bits.DoorPositionActual == 0 && tf_stat.bits.DoorPositionTarget == 0; char opening = tf_stat.bits.DoorPositionActual == 1 && tf_stat.bits.DoorPositionTarget == 0; char closed = tf_stat.bits.DoorPositionActual == 1 && tf_stat.bits.DoorPositionTarget == 1; char closing = tf_stat.bits.DoorPositionActual == 0 && tf_stat.bits.DoorPositionTarget == 1; // Used for testing, ignored when executing // if (!_options.execute) { closed = _closed; opened = _opened; } // if (closed && !opening && ( (sc_rec.range > range_htarget) && (sc_rec.range < RAM_LIMIT) ) ) { hi_count++; // Open the doors only after a number of hits in a row. // Remember when the doors were opened. // if (hi_count > HI_TARGET_REPS) { open_doors(1); doors_closed = 0; doors_opened = time(NULL); fprintf(stdout, "Doors open on range value of %.2f @ %s", sc_rec.range, ctime(&doors_opened)); fflush(stdout); // range_htarget = 70.; // dummy value used for testing hi_count = 0; sprintf(slackcmd, "echo \"%s %.2f @ %s\" | slacktee.sh", "Doors open on range value of ", sc_rec.range, ctime(&doors_opened)); } } else if (!_options.open_only && (sc_rec.range < range_ltarget) && opened) { // Close the doors if we're in normal range for // a fair amount of time (hyst_time) // time_t now = time(NULL); if (hyst_time < (now - doors_opened)) { doors_opened = 0; doors_closed = now; fprintf(stdout, "Doors close on range value of %.2f @ %s", sc_rec.range, ctime(&doors_closed)); fflush(stdout); if (!_options.open_only) open_doors(0); // range_htarget = 3.4; // dummy value used for testing } hi_count = 0; } else if ((sc_rec.range >= range_ltarget) && opened) { // Reset the lo limit timer countdown // doors_opened = time(NULL); hi_count = 0; } } } #if 0 // Don't need Crossbow data in the controller yet. if ( (fd = xbow.get_fd()) > 0) FD_SET(fd, &readset); // Get Crossbow data // A return value < 0 signifies that a new data record has completed // if (FD_ISSET(xbow.get_fd(), &readset)) { //fprintf(stderr, "reading xbow data\n"); if (0 > xbow.read_data(options.verbose)) { // New Crossbow data - trigger process // xbow.get_record(&xb_rec); } } } #endif #if USE_NN // Non-blocking check for commands. Execute and respond. // char *buf = NULL; int bytes = nn_recv(_cmd_sock, &buf, NN_MSG, NN_DONTWAIT); if (bytes > 0) { if (!strncasecmp(buf, MODE_CMD, strlen(MODE_CMD))) { fprintf(stderr, "pbcon: recvd mode request:%s\n", buf+strlen(MODE_CMD)); Modes m = get_mode(buf+strlen(MODE_CMD)); if (m != MODE_INVALID) { _options.mode = m; nn_send(_cmd_sock, ACK, strlen(ACK)+1, 0); } } else { fprintf(stderr, "pbcon: recvd unknown request %s\n", buf); nn_send(_cmd_sock, NACK, strlen(NACK)+1, 0); } nn_freemsg(buf); } #endif // If there is a slack message, let it fly // if (strlen(slackcmd) > 0) system(slackcmd); } } return OK; } /////////////////////////////////////////////////////////////////////////////// /////////////////////// HELPER FUNCTIONS //////////////////////////////////// /////////////////////////////////////////////////////////////////////////////// // // Parse command line, populate the options struct // int parse_options(int argc, char** argv) { int o; Modes m; while ( (o = getopt(argc, argv, "vxowh:l:t:m:")) != -1) { switch(o) { case 'v': _options.verbose = true; break; case 'x': _options.execute = true; break; case 'w': _options.watchdog = true; break; case 'o': _options.open_only = true; break; case 'h': _options.hi_limit = atof(optarg); break; case 'l': _options.lo_limit = atof(optarg); break; case 't': _options.hyst_time = atoi(optarg); break; case 'm': m = get_mode(optarg); if (MODE_INVALID == m) { fprintf(stderr, "Unknown mode: %s\n", optarg); return -1; } else _options.mode = m; break; default: return -1; } } time_t now = time(NULL); printf("Starting @ %s", ctime(&now)); if (_options.open_only) { _options.lo_limit = 0; } else if (_options.lo_limit >= _options.hi_limit) { printf("Inconsistent limits! (%.2f >= %.2f) - using defaults\n\n", _options.lo_limit, _options.hi_limit); usage(); return -1; } printf("Execute : %d... \n", _options.execute); printf("Open-only : %d... \n", _options.open_only); printf("Watchdog : %d... \n", _options.watchdog); printf("High limit : %.2f...\n", _options.hi_limit); printf("Low limit : %.2f...\n", _options.lo_limit); printf("Low limit time: %d... \n", _options.hyst_time); fflush(stdout); return 0; } // Returns Mode according to the string (range, depth, power) // Returns the Mode value if successful. < 0 if invalid Modes get_mode(const char* mode_str) { if (NULL == mode_str) return MODE_INVALID; if (!strcasecmp(mode_str, "range")) return RANGE_CONTROL; else if (!strcasecmp(mode_str, "depth")) return DEPTH_CONTROL; else if (!strcasecmp(mode_str, "power")) return POWER_CONTROL; else return MODE_INVALID; }