/****************************************************************************/
/* Copyright (c) 2015 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Implementation of the CANBus power buoy controller commands.  */
/* Filename : cmd_code.c                                                    */
/* Author   : Henthorn                                                      */
/* Project  : Power Buoy                                                    */
/* Version  : 1.0                                                           */
/* Created  : 08/10/2015                                                    */
/* Modified : 10/25/2016   Added Rudder Controller commands                 */
/* Modified : 09/11/2018   Added TF Controller commands                     */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*                                                                          */

#include <math.h>
#include "pbcmd.h"
#include "config.h"

// 
/* CAN controller ids */
//                                            ID   CMD    SEQ #
#define BC_ID 0x80001400;     // ID=0 binary 0000 01010 000000000  Battery Controller
#define SC_ID 0x80005400;     // ID=1 binary 0001 01010 000000000  Spring Controller
#define PC_ID 0x80009400;     // ID=2 binary 0010 01010 000000000  Power Converter
#define RC_ID 0x8000D400;     // ID=3 binary 0011 01010 000000000  Rudder Controller
#define TF_ID 0x80011400;     // ID=4 binary 0100 01010 000000000  Trefoil Controller

/* Command ids */
#define RESET           255

#define BENDER            1
#define PUMP             11
#define VALVE            12
#define TETHER           13

// Power Converter Command ids
//
#define SET_SCALE        21
#define SET_RETRACT      22
#define SET_VMAX         23
#define SET_IMAX         24
#define SET_BATTSWMODE   25
#define STOP_CHARGEMODE  26
#define SET_GAIN         27
#define SET_RPMDEVTARGET 28
#define SET_MAXBATTDRAW  29
#define SET_WINDCURRENT  31
#define SET_RPM          32  // gone away?
#define SET_BIASWINDCURR 34
#define SET_CANMSGRATE   35

// Rudder Controller Command ids
//
#define RCMODE           31
#define RCPOS            32
#define RCTARGET         33
#define RCCHDG           34
#define RCFILTER         35
#define RCPERIOD         36
#define RCPGAIN          37
#define RCIGAIN          38
#define RCDGAIN          39
#define RCMAXPOS         40
#define RCDEADBAND       41

// Trefoil Controller Command ids
//
#define TFSETPOS          1
#define TFSETACTUALPOS    7
#define TFSETMODE         2
#define TFSETCHARGEMODE   5
#define TFSETSTATEMACHINE 8
#define TFSETCURRLIM      3
#define TFWATCHDOG        4

// Return a socket fd setup for CANBus comms.
// Returns BAD_SOCK on failure.
//
int open_cansock(const char *port)
{
   struct ifreq ifr;
   struct sockaddr_can addr;
   int cansock;

   // Get a CAN_RAW socket
   //
   if ( (cansock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
   {
      perror("pbcmd: Unable to acquire socket");
      return BAD_SOCK;
   }

   // Get appropriate ioctl() for socket
   //
   addr.can_family = AF_CAN;
   strcpy(ifr.ifr_name, port);
   if (ioctl(cansock, SIOCGIFINDEX, &ifr) < 0)
   {
      perror("pbcmd: socket ioctl() failed");
      return BAD_SOCK;
   }
   addr.can_ifindex = ifr.ifr_ifindex;

   // Set 
   fcntl(cansock, F_SETFL, O_NONBLOCK);
   if (bind(cansock, (struct sockaddr *)&addr, sizeof(addr)) < 0)
   {
      return BAD_SOCK;
   }

   return cansock;
}

// General purpose functions for use by specific commands
//
struct can_frame* set_frame(struct can_frame *cf, char can_id,
                                char dlc, char cmd_id, unsigned short param)
{
   // Format the command in the CANBus frame
   //
   memset(cf, 0, sizeof(struct can_frame));
   cf->can_id  = can_id; 
   cf->can_dlc = dlc;
   cf->data[0] = cmd_id;
   cf->data[1] = param;     // parameter

   return cf;
}


int send_frame(struct can_frame *cf)
{
   int cansock = open_cansock(CAN0);

   int retval = write(cansock, cf, sizeof(struct can_frame));
   close(cansock);
   if (retval != sizeof(struct can_frame))
   {
      return BAD_SOCK;
   }
   else
   {
      return OK;
   }
}

int fire(struct can_frame *f)
{
   short param;

   // Send the command (fire-and-forget)
   //
   // Some debug code
   // Display Power Buoy codes in the can_id
   //
#if 1
   unsigned int cmdid = 0, ctlid = 0;
   unsigned char op = f->data[0];
   unsigned char p1 = f->data[1];
   unsigned char p2 = f->data[2];
   unsigned short *p3 = (unsigned short*)&f->data[1];

   cmdid = f->can_id & 0x3E00;
   cmdid >>= 9;
   ctlid = f->can_id & 0x3C000;
   ctlid >>= 14;

   printf("CTLR  CMD   OP    PARAM\n");
   printf("====  ====  ====  =======\n");
   printf("%-4d  %-4d  %-4d  ", ctlid, cmdid, op);
   //if (f->can_dlc > 1) printf("%-#2x ", p1);
   //if (f->can_dlc > 2) printf("%-#2x ", p2);
   if (f->can_dlc > 1) printf("%-7d", *p3);
   printf("\n");
#endif
   if (OK != (param = send_frame(f)) )
   {
      perror("CAN comms failure");
      return param;
   }

   return OK;
}

/******************************************************************/
/* bender command:   */
/******************************************************************/
int bender_usage()
{
   printf("usage:\n"
          " bender [off | on | auto]\n");

   return 0;
}

int bender(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return bender_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else if (!strcmp("auto", argv[1]))
      param = 2;
   else
      return bender_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame;
   memset(&frame, 0, sizeof(frame));
   //set_frame(&frame, BC_ID, 2, BENDER, param);
   frame.can_id  = BC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 1;         // cmd byte 1=>bender
   frame.data[1] = param;     // parameter

   printf("bender: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* reset_bc command:   */
/******************************************************************/
int reset_bc_usage()
{
   printf("usage:\n"
          " reset_bc\n");

   return 0;
}

int reset_bc(int argc, char** argv)
{
   if (argc > 1)
      return reset_bc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = BC_ID; 
   frame.can_dlc = 1;        // 1 byte
   frame.data[0] = RESET;    // cmd byte 255=>reset

   printf("reset_bc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* valve command:   */
/******************************************************************/
int pump_usage()
{
   printf("usage:\n"
          " pump [off | 1..127 minutes]]\n");

   return 0;
}

int pump(int argc, char** argv)
{
   unsigned short param = 0;

   if (argc < 2 || argc > 2)
      return pump_usage();

   if (!strcmp("off", argv[1]))
      param = 0;
   else if ( (param = atoi(argv[1])) == 0 || param > 127)
      return pump_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID;     // binary 00 01010 000000000
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 11;        // cmd byte 11=>pump
   frame.data[1] = param;     // parameter

   printf("pump: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* valve command:   */
/******************************************************************/
int valve_usage()
{
   printf("usage:\n"
          " valve [off | 1..127 seconds]\n");

   return 0;
}

int valve(int argc, char** argv)
{
   unsigned short param = 0;

   if (argc < 2 || argc > 2)
      return valve_usage();

   if (!strcmp("off", argv[1]))
      param = 0;
   else if ( (param = atoi(argv[1])) == 0 || param > 127)
      return valve_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 12;        // cmd byte 12=>valve
   frame.data[1] = param;     // parameter

   printf("valve: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* tether command:   */
/******************************************************************/
int tether_usage()
{
   printf("usage:\n"
          " tether [off | on]\n");

   return 0;
}

int tether(int argc, char** argv)
{
   unsigned short param = 0;

   if (argc < 2 || argc > 2)
      return tether_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return tether_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID;     // binary 00 01010 000000000
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 13;        // cmd byte 13=>tether
   frame.data[1] = param;     // parameter

   printf("tether: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* reset_sc command:   */
/******************************************************************/
int reset_sc_usage()
{
   printf("usage:\n"
          " reset_sc\n");

   return 0;
}

int reset_sc(int argc, char** argv)
{
   if (argc > 1)
      return reset_sc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = SC_ID;     // binary 00 01010 000000000
   frame.can_dlc = 1;         // 1 bytes
   frame.data[0] = RESET;     // cmd byte 255=>reset

   printf("reset_sc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}


/******************************************************************/
/* scale command:   */
/******************************************************************/
int scale_usage()
{
   printf("usage:\n"
          " scale (%.1f - %.1f\n", MIN_SCALE_FACTOR, MAX_SCALE_FACTOR);

   return 0;
}

int scale(int argc, char** argv)
{
   unsigned char param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return scale_usage();

   if ( (f = atof(argv[1])) < MIN_SCALE_FACTOR || f > MAX_SCALE_FACTOR)
      return scale_usage();

   f += 0.01;
   param = (unsigned char)(f*CRSF_PERCENTAGES);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc =  2;         // 2 bytes
   frame.data[0] = SET_SCALE;         // cmd byte 21=>set scale
   frame.data[1] = param;

   printf("scale: Sending val of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* retract command:   */
/******************************************************************/
int retract_usage()
{
   printf("usage:\n"
          " retract (%.1f - %.1f)\n", MIN_RETRACT_FACTOR, MAX_RETRACT_FACTOR);

   return 0;
}

int retract(int argc, char** argv)
{
   unsigned char param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return retract_usage();

   if ( (f = atof(argv[1])) < MIN_RETRACT_FACTOR || f > MAX_RETRACT_FACTOR)
      return retract_usage();

   f += 0.01;
   param = (unsigned char)(f*CRSF_PERCENTAGES);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 22;        // cmd byte 22=>set retract
   frame.data[1] = param;

   printf("retract: Sending param of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* vmax command:   */
/******************************************************************/
int vmax_usage()
{
   printf("usage:\n"
          " vmax [0.0 - %.2f]\n", (float)ABSOLUTEMAX_TARGETVOLTAGE);

   return 0;
}

int vmax(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return vmax_usage();

   if ( (f = atof(argv[1])) < 0.0001 || f > ABSOLUTEMAX_TARGETVOLTAGE)
      return vmax_usage();

   param = (int)(f*CRSF_VOLTAGE);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 23;        // cmd byte 23=>set vmax
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("vmax: Sending command of %d...\n", *p);
   return fire(&frame);
}


/******************************************************************/
/* imax command:   */
/******************************************************************/
int imax_usage()
{
   printf("usage:\n"
          " imax [0.0 - %.2f]\n", (float)MAX_BATTCHARGECURRENTLIMIT);

   return 0;
}

int imax(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return imax_usage();

   if ( (f = atof(argv[1])) < 0.00001 || f > MAX_BATTCHARGECURRENTLIMIT)
      return imax_usage();

   param = (int)(f*CRSF_CURRENT);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 24;        // cmd byte 24=>set imax
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("imax: Sending command of %d...\n", *p);
   return fire(&frame);
}


/******************************************************************/
/* bus command: superseded by batt_mode function                  */
/******************************************************************/
int bus_usage()
{
   printf("usage:\n"
          " bus [off | on | auto]\n");

   return 0;
}

int bus(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return bus_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("auto", argv[1]))
      param = 2;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return bus_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 25;        // cmd byte 25=>bus
   frame.data[1] = param;     // parameter

   printf("bus: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* scm command:                                                   */
/******************************************************************/
int scm_usage()
{
   printf("usage:\n"
          " stopchargemode [off | on]\n");

   return 0;
}

int scm(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return scm_usage();

   if (!strcmp("on", argv[1]))
      param = 1;
   else if (!strcmp("off", argv[1]))
      param = 0;
   else
      return scm_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 26;        // cmd byte 26=>stopchargemode
   frame.data[1] = param;     // parameter

   printf("stopchargemode: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* gain command:   */
/******************************************************************/
int gain_usage()
{
   printf("usage:\n"
          " gain (0 - %d)\n"
          " A 0 value turns the Gain Schedule Mode Off\n",
          MAX_SCALE_INTEGRATION_GAIN);

   return 0;
}

int gain(int argc, char** argv)
{
   unsigned short param;
   float f = -1.;

   if (argc < 2 || argc > 2)
      return gain_usage();

   if ( (param = atoi(argv[1])) < 0 || atoi(argv[1]) > MAX_SCALE_INTEGRATION_GAIN)
      return gain_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = SET_GAIN;        // cmd byte 27->gain
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("gain: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* target command:   */
/******************************************************************/
int target_usage()
{
   printf("usage:\n"
          " target (%d - %d)\n",
          MIN_TARGETVOLTAGE, MAX_TARGETVOLTAGE);

   return 0;
}

int vstddev(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return target_usage();

   if ( (param = atoi(argv[1])) < MIN_TARGETVOLTAGE || param > MAX_TARGETVOLTAGE)
      return target_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = SET_RPMDEVTARGET;        // cmd byte 28->target
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("target: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* reset_pc command:   */
/******************************************************************/
int reset_pc_usage()
{
   printf("usage:\n"
          " reset_pc\n");

   return 0;
}

int reset_pc(int argc, char** argv)
{
   if (argc > 1)
      return reset_pc_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 1;         // 1 byte
   frame.data[0] = RESET;     // cmd byte 255=>reset

   printf("reset_pc: Sending command of %d...\n", frame.data[1]);
   return fire(&frame);
}

/******************************************************************/
/* pc torque command:   */
/******************************************************************/
int torque_usage()
{
   printf("usage:\n"
          " set_torque (-40 to 40 amps)\n");

   return 0;
}

int set_torque(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return torque_usage();

   if ( (f = atof(argv[1])) < -40.0 || f > 40.0)
      return torque_usage();

   param = (int)roundf(f * 500);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = 31;        // cmd byte 31->set_torque
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("set_torque: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* pc rpm command:   */
/******************************************************************/
int rpm_usage()
{
   printf("usage:\n"
          " set_rpm (-30000 to 30000)\n");

   return 0;
}

int set_rpm(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rpm_usage();

   if ( (param = atoi(argv[1])) < -30000 || atoi(argv[1]) > 30000)
      return rpm_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;         // 2 bytes
   frame.data[0] = 32;        // cmd byte 32->set_rpm
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("set_rpm: Sending command of %d...\n", *p);
   return fire(&frame);
}

/******************************************************************/
/* pc max_batt_chrg current command:   */
/******************************************************************/
int max_batt_chrg_usage()
{
   printf("usage:\n"
          " max_batt_chrg (0.0 to %.2f)\n", MAX_BATTCHARGECURRENTLIMIT);

   return 0;
}

int max_batt_chrg(int argc, char** argv)
{
   unsigned short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return max_batt_chrg_usage();

   if ( (f = atof(argv[1])) < 0.0001 || param > (MAX_BATTCHARGECURRENTLIMIT + 0.0001))
      return max_batt_chrg_usage();

   param = (int)roundf(f * CRSF_CURRENT);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;                  // 3 bytes
   frame.data[0] = SET_MAXBATTDRAW;
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("max_batt_chrg: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* pc max_batt_draw current command:   */
/******************************************************************/
int max_batt_draw_usage()
{
   printf("usage:\n"
          " max_batt_draw (0.0 to %.2f)\n", MAX_BATTDRAWCURRENTLIMIT);

   return 0;
}

int max_batt_draw(int argc, char** argv)
{
   unsigned short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return max_batt_draw_usage();

   if ( (f = atof(argv[1])) < 0.0001 || param > (MAX_BATTDRAWCURRENTLIMIT + 0.0001))
      return max_batt_draw_usage();

   param = (int)roundf(f * CRSF_CURRENT);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;                  // 3 bytes
   frame.data[0] = SET_MAXBATTDRAW;
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("max_batt_draw: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* pc set_batt_mode current command:   */
/******************************************************************/
int batt_mode_usage()
{
   printf("usage:\n"
          " batt_mode (0=>Off | 1=>On | 2=>Auto)\n");

   return 0;
}

int batt_mode(int argc, char** argv)
{
   unsigned short param;

   if (argc < 2 || argc > 2)
      return batt_mode_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 2 )
      return batt_mode_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 2;                  // 2 bytes
   frame.data[0] = SET_BATTSWMODE;
   unsigned short *p = (unsigned short*)&frame.data[1];
   *p = param;

   printf("max_batt_mode: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* pc wind_curr command:   */
/******************************************************************/
int wind_curr_usage()
{
   printf("usage:\n"
          " wind_curr (%.2f to %.2f)\n",
          -(float)MAX_WINDCURRENTLIMIT, (float)MAX_WINDCURRENTLIMIT);

   return 0;
}

int wind_curr(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return wind_curr_usage();

   if ( (f = atof(argv[1])) < -MAX_WINDCURRENTLIMIT || param > MAX_WINDCURRENTLIMIT)
      return wind_curr_usage();

   param = (int)roundf(f * CRSF_CURRENT);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;                  // 2 bytes
   frame.data[0] = SET_WINDCURRENT;
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("wind_curr: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* pc bias_wind_curr command:   */
/******************************************************************/
int bias_wind_curr_usage()
{
   printf("usage:\n"
          " bias_wind_curr (%.2f to %.2f)\n",
          -MAX_BIASCURRENT, MAX_BIASCURRENT);

   return 0;
}

int bias_wind_curr(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return bias_wind_curr_usage();

   if ( (f = atof(argv[1])) < -MAX_BIASCURRENT || param > MAX_BIASCURRENT)
      return bias_wind_curr_usage();

   param = (int)roundf(f * CRSF_CURRENT);

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;                  // 2 bytes
   frame.data[0] = SET_BIASWINDCURR;
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("bias_wind_curr: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* pc can_pack_rate command:   */
/******************************************************************/
int can_pack_rate_usage()
{
   printf("usage:\n"
          " can_pack_rate (Hz) [10 or 40]\n");

   return 0;
}

int can_pack_rate(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return can_pack_rate_usage();

   if ( (param = atoi(argv[1])) != 10 && param != 40)
      return can_pack_rate_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = PC_ID; 
   frame.can_dlc = 3;                  // 2 bytes
   frame.data[0] = SET_CANMSGRATE;
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("can_pack_rate: Sending command of %d...\n", *p);
   return 0;
   return fire(&frame);
}

/******************************************************************/
/* rc mode command:   */
/******************************************************************/
int rcmode_usage()
{
   printf("usage:\n"
          " rcmode (heading | rudder | bangbang)\n");

   return 0;
}

int rcmode(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcmode_usage();

   // Heading mode: param == 0
   // Rudder mode : param == 1
   // 
   if (!strcasecmp(argv[1], "heading"))
      param = 0;
   else if (!strcasecmp(argv[1], "rudder"))
      param = 1;
   else if (!strncasecmp(argv[1], "bang", 4))
      param = 2;
   else
      return rcmode_usage();
   
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 31;        // cmd byte 31->rcmode
   frame.data[1] = (char)param;

   printf("rcmode: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc position command:   */
/******************************************************************/
int rcpos_usage()
{
   printf("usage:\n"
          " rcpos (-1.0 .. 1.0)\n");

   return 0;
}

int rcpos(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcpos_usage();

   if ( (f = atof(argv[1])) < -1.0 || f > 1.0)
      return rcpos_usage();

   param = (int)roundf(f * 32767);
   if (param > 32767) param = 32767;
   else if (param < -32768) param = -32768;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 32;     // cmd byte 32=>rcpos
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcpos: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc target heading command:   */
/******************************************************************/
int rctarget_usage()
{
   printf("usage:\n"
          " rctarget (0.0 .. 359.5)\n");

   return 0;
}

int rctarget(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rctarget_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 359.5)
      return rctarget_usage();

   param = (int)roundf(f);
   if (param > 359) param = 359;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 33;        // cmd byte 33=>rctarget
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rctarget: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc default heading command:   */
/******************************************************************/
int rchdg_usage()
{
   printf("usage:\n"
          " rchdg (0.0 .. 359.5)\n");

   return 0;
}

int rchdg(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rchdg_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 359.5)
      return rchdg_usage();

   param = (int)roundf(f);
   if (param > 359) param = 359;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 34;        // cmd byte 34=>rchdg
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rchdg: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc heading filter command:   */
/******************************************************************/
int rcfilter_usage()
{
   printf("usage:\n"
          " rcfilter (0..1000)\n");

   return 0;
}

int rcfilter(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcfilter_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 1000)
      return rcfilter_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 35;        // cmd byte 35=>rcfilter
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcfilter: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc period command:   */
/******************************************************************/
int rcperiod_usage()
{
   printf("usage:\n"
          " rcperiod (0..100)\n");

   return 0;
}

int rcperiod(int argc, char** argv)
{
   short param;

   if (argc < 2 || argc > 2)
      return rcperiod_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 100)
      return rcperiod_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 36;        // cmd byte 36=>rcperiod
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcperiod: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc prop gain command:   */
/******************************************************************/
int rcpgain_usage()
{
   printf("usage:\n"
          " rcpgain (0.0 .. 1.0)\n");

   return 0;
}

int rcpgain(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcpgain_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcpgain_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 37;        // cmd byte 37=>rcpgain
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcpgain: Sending command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* rc int gain command:   */
/******************************************************************/
int rcigain_usage()
{
   printf("usage:\n"
          " rcigain (0.0 .. 1.0)\n");

   return 0;
}

int  rcigain(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcigain_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcigain_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 38;        // cmd byte 38=>rcigain
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcigain: Sending command of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* rc derivative gain command:   */
/******************************************************************/
int rcdgain_usage()
{
   printf("usage:\n"
          " rcdgain (0.0 .. 1.0)\n");

   return 0;
}

int rcdgain(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcdgain_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcdgain_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 39;        // cmd byte 39=>rcdgain
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcdgain: Sending command of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* rc max rudder position command:                                */
/******************************************************************/
int rcmaxpos_usage()
{
   printf("usage:\n"
          " rcmaxpos (0.0 .. 1.0)\n");

   return 0;
}

int rcmaxpos(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcmaxpos_usage();

   if ( (f = atof(argv[1])) < 0.0 || f > 1.0)
      return rcmaxpos_usage();

   param = (int)roundf(f*32767.);
   if (param > 32767) param = 32767;
   else if (param < 0) param = 0;

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 40;        // cmd byte 40=>rcmaxpos
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rcmaxpos: Sending command of %d...\n", param);
   return fire(&frame);
}


/******************************************************************/
/* rc deadband command:                                           */
/******************************************************************/
int rcdeadband_usage()
{
   printf("usage:\n"
          " rcdeadband (0 .. 90)\n");

   return 0;
}

int rcdeadband(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return rcdeadband_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 90)
      return rcdeadband_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 41;        // cmd byte 41=>rcdeadband
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("rc: Sending rcdeadbandcommand of %d...\n", param);
   return fire(&frame);
}


int  rcreset(int argc, char** argv)
{
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = RC_ID; 
   frame.can_dlc = 1;         // 3 bytes
   frame.data[0] = RESET;     // cmd byte 255=>rcreset

   printf("reset_rudder: Sending command...\n");
   return fire(&frame);
}

/******************************************************************/
/* tf position command:                                           */
/******************************************************************/
int tf_setpos_usage()
{
   printf("usage:\n"
          " tf_SetPos (0 | 1) 0 => fully 0pened  1 => fully closed\n");
   return 0;
}

int  tf_setpos(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setpos_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 1)
      return tf_setpos_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 1;        // cmd byte 1=>position
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetPos command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf actual position command:                                    */
/******************************************************************/
int tf_setactualpos_usage()
{
   printf("usage:\n"
          " tf_SetActualPos (0 | 1) 0 => fully 0pened  1 => fully closed\n");
   return 0;
}

int  tf_setactualpos(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setactualpos_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 1)
      return tf_setactualpos_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 8;        // cmd byte 1=>position
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetActualPos command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf position command:                                           */
/******************************************************************/
int tf_setstatemachine_usage()
{
   printf("usage:\n"
          " tf_SetStateMachine (0 | 1) 0 => HomeMove  1 => VelMove\n");
   return 0;
}

int  tf_setstatemachine(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setstatemachine_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 1)
      return tf_setstatemachine_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 2;        // 2 bytes
   frame.data[0] = 7;        // cmd byte 8=>state machine
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetStateMachinePos command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf mode command:                                               */
/******************************************************************/
int tf_setmode_usage()
{
   printf("usage:\n"
          " tf_SetMode (0|1|2) 0=>l0w pwr  1=>monitor pos  2=>hold pos\n");
   return 0;
}

int  tf_setmode(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setmode_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 2)
      return tf_setmode_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 2;         // cmd byte 2=>mode
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetMode command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf charge mode command:                                        */
/******************************************************************/
int tf_setchargemode_usage()
{
   printf("usage:\n"
          " tf_SetChargeMode (0|1|2|3) 0=>0ff  1=>on  2=>auto-time  3=>auto-voltage\n");
   return 0;
}

int tf_setchargemode(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setchargemode_usage();

   if ( (param = atoi(argv[1])) < 0 || param > 3)
      return tf_setchargemode_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 2;         // 2 bytes
   frame.data[0] = 5;         // cmd byte 5=>charge mode
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetChargeMode command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf current limit command:                                      */
/******************************************************************/
int tf_setcurrlim_usage()
{
   printf("usage:\n"
          " tf_SetCurrLim (500 .. 5000)\n");
   return 0;
}

int  tf_setcurrlim(int argc, char** argv)
{
   short param;
   float f = 0.;

   if (argc < 2 || argc > 2)
      return tf_setcurrlim_usage();

   if ( (param = atoi(argv[1])) < 500 || param > 5000)
      return tf_setcurrlim_usage();

   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 3;         // 3 bytes
   frame.data[0] = 3;         // cmd byte 3=>currlim
   short *p = (short*)&frame.data[1];
   *p = param;

   printf("tf: Sending tf_SetCurrLim command of %d...\n", param);
   return fire(&frame);
}

/******************************************************************/
/* tf watchdog command:                                           */
/******************************************************************/
int  tf_watchdog(int argc, char** argv)
{
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 1;         // 1 bytes
   frame.data[0] = 4;         // cmd byte 4=>watchdog

   printf("tf: Sending tf_WatchDog command...\n");
   return fire(&frame);
}

int  tf_reset(int argc, char** argv)
{
   // Format the command in the CANBus frame
   //
   struct can_frame frame; memset(&frame, 0, sizeof(frame));
   frame.can_id  = TF_ID; 
   frame.can_dlc = 1;         // 1 bytes
   frame.data[0] = RESET;     // cmd byte 255=>rcreset

   printf("tf_Reset: Sending reset command...\n");
   return fire(&frame);
}