nxtOSEK/lejos_nxj/src/nxtvm/platform/nxt/nxt_avr.c

   1 #include "nxt_avr.h"
   2
   3
   4 #include "twi.h"
   5 #include "nxt_motors.h"
   6
   7 #include "systick.h"
   8 #include <string.h>
   9
  10
  11
  12
  13 #define NXT_AVR_ADDRESS 1
  14 #define NXT_AVR_N_OUTPUTS 4
  15 #define NXT_AVR_N_INPUTS  4
  16
  17
  18 const char avr_brainwash_string[] =
  19   "\xCC" "Let's samba nxt arm in arm, (c)LEGO System A/S";
  20
  21 static U32 nxt_avr_initialised;
  22
  23 static struct {
  24   U8 power;
  25   U8 pwm_frequency;
  26   S8 output_percent[NXT_AVR_N_OUTPUTS];
  27   U8 output_mode;
  28   U8 input_power;
  29 } io_to_avr;
  30
  31
  32 static struct {
  33   U16 adc_value[NXT_AVR_N_INPUTS];
  34   U16 buttons;
  35   U16 battery_is_AA;
  36   U16 battery_mV;
  37   U8 avr_fw_version_major;
  38   U8 avr_fw_version_minor;
  39 } io_from_avr;
  40
  41 static U8 data_from_avr[(2 * NXT_AVR_N_INPUTS) + 5];
  42
  43 static U8 data_to_avr[5 + NXT_AVR_N_OUTPUTS];
  44
  45
  46
  47 /* We're assuming that we get good packing */
  48
  49 static void
  50 nxt_avr_start_read(void)
  51 {
  52   memset(data_from_avr, 0, sizeof(data_from_avr));
  53   twi_start_read(NXT_AVR_ADDRESS, 0, 0, data_from_avr, sizeof(data_from_avr));
  54 }
  55
  56 static void
  57 nxt_avr_start_send(void)
  58 {
  59   int i;
  60   U8 checkByte = 0;
  61   U8 *a = data_to_avr;
  62   U8 *b = (U8 *) (&io_to_avr);
  63
  64   i = sizeof(io_to_avr);
  65   while (i) {
  66     *a = *b;
  67     checkByte += *b;
  68     a++;
  69     b++;
  70     i--;
  71   }
  72
  73   *a = ~checkByte;
  74
  75   twi_start_write(NXT_AVR_ADDRESS, 0, 0, data_to_avr, sizeof(data_to_avr));
  76
  77 }
  78
  79 void
  80 nxt_avr_power_down(void)
  81 {
  82   io_to_avr.power = 0x5a;
  83   io_to_avr.pwm_frequency = 0x00;
  84 }
  85
  86
  87 void
  88 nxt_avr_firmware_update_mode(void)
  89 {
  90   io_to_avr.power = 0xA5;
  91   io_to_avr.pwm_frequency = 0x5A;
  92 }
  93
  94 void
  95 nxt_avr_link_init(void)
  96 {
  97   twi_start_write(NXT_AVR_ADDRESS, 0, 0, (const U8 *) avr_brainwash_string,
  98                   strlen(avr_brainwash_string));
  99 }
 100
 101
 102 static U16
 103 Unpack16(const U8 *x)
 104 {
 105   U16 retval;
 106
 107   retval = (((U16) (x[0])) & 0xff) | ((((U16) (x[1])) << 8) & 0xff00);
 108   return retval;
 109 }
 110
 111
 112 static struct {
 113   U32 good_rx;
 114   U32 bad_rx;
 115   U32 resets;
 116   U32 still_busy;
 117   U32 not_ok;
 118 } nxt_avr_stats;
 119
 120 static void
 121 nxt_avr_unpack(void)
 122 {
 123   U8 check_sum;
 124   U8 *p;
 125   U16 buttonsVal;
 126   U32 voltageVal;
 127   int i;
 128
 129   p = data_from_avr;
 130
 131   for (check_sum = i = 0; i < sizeof(data_from_avr); i++) {
 132     check_sum += *p;
 133     p++;
 134   }
 135
 136   if (check_sum != 0xff) {
 137     nxt_avr_stats.bad_rx++;
 138     return;
 139   }
 140
 141   nxt_avr_stats.good_rx++;
 142
 143   p = data_from_avr;
 144
 145   // Marshall
 146   for (i = 0; i < NXT_AVR_N_INPUTS; i++) {
 147     io_from_avr.adc_value[i] = Unpack16(p);
 148     p += 2;
 149   }
 150
 151   buttonsVal = Unpack16(p);
 152   p += 2;
 153
 154
 155   io_from_avr.buttons = 0;
 156
 157   if (buttonsVal > 1023) {
 158     io_from_avr.buttons |= 1;
 159     buttonsVal -= 0x7ff;
 160   }
 161
 162   if (buttonsVal > 720)
 163     io_from_avr.buttons |= 0x08;
 164   else if (buttonsVal > 270)
 165     io_from_avr.buttons |= 0x04;
 166   else if (buttonsVal > 60)
 167     io_from_avr.buttons |= 0x02;
 168
 169   voltageVal = Unpack16(p);
 170
 171   io_from_avr.battery_is_AA = (voltageVal & 0x8000) ? 1 : 0;
 172   io_from_avr.avr_fw_version_major = (voltageVal >> 13) & 3;
 173   io_from_avr.avr_fw_version_minor = (voltageVal >> 10) & 7;
 174
 175
 176   // Figure out voltage
 177   // The units are 13.848 mV per bit.
 178   // To prevent fp, we substitute 13.848 with 14180/1024
 179
 180   voltageVal &= 0x3ff;          // Toss unwanted bits.
 181   voltageVal *= 14180;
 182   voltageVal >>= 10;
 183   io_from_avr.battery_mV = voltageVal;
 184
 185 }
 186
 187
 188 void
 189 nxt_avr_init(void)
 190 {
 191   twi_init();
 192
 193   memset(&io_to_avr, 0, sizeof(io_to_avr));
 194   io_to_avr.power = 0;
 195   io_to_avr.pwm_frequency = 8;
 196
 197   nxt_avr_initialised = 1;
 198 }
 199
 200 static U32 update_count;
 201 static U32 link_init_wait;
 202 static U32 link_running;
 203
 204 void
 205 nxt_avr_1kHz_update(void)
 206 {
 207
 208   if (!nxt_avr_initialised)
 209     return;
 210
 211   if (link_init_wait) {
 212     link_init_wait--;
 213     return;
 214   }
 215
 216   if (!twi_ok()) {
 217     nxt_avr_stats.not_ok++;
 218     link_running = 0;
 219   }
 220
 221   if (twi_busy()) {
 222     nxt_avr_stats.still_busy++;
 223     link_running = 0;
 224   }
 225
 226
 227   if (!twi_ok() || twi_busy() || !link_running) {
 228     memset(data_from_avr, 0, sizeof(data_from_avr));
 229     link_running = 1;
 230     nxt_avr_link_init();
 231     link_init_wait = 2;
 232     update_count = 0;
 233     nxt_avr_stats.resets++;
 234     return;
 235   }
 236
 237   if (update_count & 1) {
 238     nxt_avr_start_read();
 239   } else {
 240     nxt_avr_unpack();
 241     nxt_avr_start_send();
 242   }
 243   update_count++;
 244 }
 245
 246 U32
 247 buttons_get(void)
 248 {
 249   return io_from_avr.buttons;
 250 }
 251
 252 U32
 253 battery_voltage(void)
 254 {
 255   return io_from_avr.battery_mV;
 256 }
 257
 258 U32
 259 sensor_adc(U32 n)
 260 {
 261   if (n < 4)
 262     return io_from_avr.adc_value[n];
 263   else
 264     return 0;
 265 }
 266
 267
 268 void
 269 nxt_avr_set_motor(U32 n, int power_percent, int brake)
 270 {
 271   if (n < NXT_N_MOTORS) {
 272     io_to_avr.output_percent[n] = power_percent;
 273     if (brake)
 274       io_to_avr.output_mode |= (1 << n);
 275     else
 276       io_to_avr.output_mode &= ~(1 << n);
 277   }
 278 }
 279
 280 void
 281 nxt_avr_set_input_power(U32 n, U32 power_type)
 282 {
 283   // The power to the sensor is controlled by a bit in
 284   // each of the two nibbles of the byte. There is one
 285   // bit for each of the four sensors. if the low nibble
 286   // bit is set then the sensor is "ACTIVE" and 9v is
 287   // supplied to it but it will be pulsed off to allow
 288   // the sensor to be be read. A 1 in the high nibble
 289   // indicates that it is a 9v always on sensor and
 290   // 9v will be supplied constantly. If both bits are
 291   // clear then 9v is not supplied to the sensor.
 292   // Having both bits set is currently not supported.
 293   if (n < NXT_AVR_N_INPUTS && power_type <= 2) {
 294     U8 val = (power_type & 0x2 ? 0x10 << n : 0) | ((power_type & 1) << n);
 295     io_to_avr.input_power &= ~(0x11 << n);
 296     io_to_avr.input_power |= val;
 297   }
 298 }