X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=arducontrol%2Fmotorola.c;h=883457ecf52b4bd44b5badccf0ae3f7ec14be826;hb=49c7ae4a26f8dd7d09872b6b5e4c107ef33b4560;hp=e78ebfc34bbe6a0b1de6a939452aca3d96577f66;hpb=32dad098c247f7f98b14fcb9c6892dc8f302a8d1;p=model-railway-devices.git diff --git a/arducontrol/motorola.c b/arducontrol/motorola.c index e78ebfc..883457e 100644 --- a/arducontrol/motorola.c +++ b/arducontrol/motorola.c @@ -1,4 +1,6 @@ -#include "packet.h" +#include "commands.h" +#include "motorola.h" +#include "output.h" static uint8_t motorola_speed_to_value(uint8_t speed) { @@ -10,82 +12,77 @@ static uint8_t motorola_speed_to_value(uint8_t speed) return 0; } -static void motorola_common_packet(uint8_t addr, uint8_t aux, uint8_t value) +static OutputPacket *motorola_common_packet(uint8_t addr, uint8_t aux, uint8_t value) { - uint8_t i; + OutputPacket *packet = output_create_packet(); - clear_packet(); + packet->bit_duration = 2; + packet->length = 18*8; - packet.bit_duration = 2; - packet.length = 18*8; - - for(i=0; i<4; ++i) + for(uint8_t i=0; i<4; ++i) { uint8_t d = addr%3; addr /= 3; - packet.data[i*2] = (d==0 ? 0x01 : 0x7F); - packet.data[i*2+1] = (d==1 ? 0x7F : 0x01); + packet->data[i*2] = (d==0 ? 0x01 : 0x7F); + packet->data[i*2+1] = (d==1 ? 0x7F : 0x01); } - packet.data[8] = (aux ? 0x7F : 0x01); - packet.data[9] = packet.data[8]; + packet->data[8] = (aux ? 0x7F : 0x01); + packet->data[9] = packet->data[8]; - for(i=0; i<4; ++i) + for(uint8_t i=0; i<4; ++i) { - packet.data[10+i*2] = ((value&1) ? 0x7F : 0x01); + packet->data[10+i*2] = ((value&1) ? 0x7F : 0x01); value >>= 1; } - packet.repeat_count = 2; + packet->repeat_count = 2; // Duration of three trits - packet.repeat_delay = 96; - packet.final_delay = 224; + packet->repeat_delay = 96; + packet->final_delay = 224; + + return packet; } -static void motorola_old_packet(uint8_t addr, uint8_t aux, uint8_t value) +static OutputPacket *motorola_old_packet(uint8_t addr, uint8_t aux, uint8_t value) { - uint8_t i; + OutputPacket *packet = motorola_common_packet(addr, aux, value); - motorola_common_packet(addr, aux, value); + for(uint8_t i=0; i<4; ++i) + packet->data[11+i*2] = packet->data[10+i*2]; - for(i=0; i<4; ++i) - packet.data[11+i*2] = packet.data[10+i*2]; + return packet; } void motorola_locomotive_speed_packet(uint8_t addr, uint8_t aux, uint8_t speed) { motorola_old_packet(addr, aux, motorola_speed_to_value(speed)); - - packet.ready = 1; + output_send_packet(); } void motorola_locomotive_reverse_packet(uint8_t addr, uint8_t aux) { motorola_old_packet(addr, aux, 1); - - packet.ready = 1; + output_send_packet(); } void motorola_locomotive_speed_direction_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t dir) { - motorola_common_packet(addr, aux, motorola_speed_to_value(speed)); + OutputPacket *packet = motorola_common_packet(addr, aux, motorola_speed_to_value(speed)); - packet.data[11] = (dir ? 0x01 : 0x7F); - packet.data[13] = (dir ? 0x7F : 0x01); - packet.data[15] = (dir ? 0x01 : 0x7F); - packet.data[17] = 0x80-packet.data[16]; + packet->data[11] = (dir ? 0x01 : 0x7F); + packet->data[13] = (dir ? 0x7F : 0x01); + packet->data[15] = (dir ? 0x01 : 0x7F); + packet->data[17] = 0x80-packet->data[16]; - packet.ready = 1; + output_send_packet(); } void motorola_locomotive_speed_function_packet(uint8_t addr, uint8_t aux, uint8_t speed, uint8_t func, uint8_t state) { - uint8_t i; - uint8_t value; - - value = motorola_speed_to_value(speed); - motorola_common_packet(addr, aux, value); + uint8_t value = motorola_speed_to_value(speed); + OutputPacket *packet = motorola_common_packet(addr, aux, value); /* 001 -> 011 @@ -101,13 +98,13 @@ void motorola_locomotive_speed_function_packet(uint8_t addr, uint8_t aux, uint8_ if(func==value) func = ((value&8) ? 2 : 5) | (func&8); - for(i=0; i<4; ++i) + for(uint8_t i=0; i<4; ++i) { - packet.data[11+i*2] = ((func&1) ? 0x7F : 0x01); + packet->data[11+i*2] = ((func&1) ? 0x7F : 0x01); func >>= 1; } - packet.ready = 1; + output_send_packet(); } void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state) @@ -116,9 +113,84 @@ void motorola_solenoid_packet(uint8_t addr, uint8_t output, uint8_t state) if(state) value |= 8; - motorola_old_packet(addr, 0, value); - packet.repeat_delay >>= 1; - packet.bit_duration = 1; + OutputPacket *packet = motorola_old_packet(addr, 0, value); + packet->repeat_delay >>= 1; + packet->bit_duration = 1; + + output_send_packet(); +} + +uint8_t motorola_command(const uint8_t *cmd_buf, uint8_t cmd_length) +{ + if(cmd_buf[0]==MOTOROLA_SPEED || cmd_buf[0]==MOTOROLA_SPEED_DIRECTION || cmd_buf[0]==MOTOROLA_SPEED_FUNCTION) + { + if(cmd_length!=4) + return LENGTH_ERROR; + + uint8_t addr = cmd_buf[1]; + if(addr>80) + return INVALID_VALUE; + + if(cmd_buf[2]&0x0C) + return INVALID_VALUE; + uint8_t aux = cmd_buf[2]&0x01; + + uint8_t func = (cmd_buf[2]&0xF0)>>4; + if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION) + { + if(func<1 || func>4) + return INVALID_VALUE; + } + else if(cmd_buf[2]&0xFE) + return INVALID_VALUE; + uint8_t state = cmd_buf[2]&0x02; + + uint8_t speed = cmd_buf[3]&0x7F; + if(speed>14) + return INVALID_VALUE; + + uint8_t dir = !(cmd_buf[3]&0x80); + + if(cmd_buf[0]==MOTOROLA_SPEED) + motorola_locomotive_speed_packet(addr, aux, speed); + else if(cmd_buf[0]==MOTOROLA_SPEED_DIRECTION) + motorola_locomotive_speed_direction_packet(addr, aux, speed, dir); + else if(cmd_buf[0]==MOTOROLA_SPEED_FUNCTION) + motorola_locomotive_speed_function_packet(addr, aux, speed, func, state); + } + else if(cmd_buf[0]==MOTOROLA_REVERSE) + { + if(cmd_length!=3) + return LENGTH_ERROR; + + uint8_t addr = cmd_buf[1]; + if(addr>80) + return INVALID_VALUE; + + if(cmd_buf[2]&0xFE) + return INVALID_VALUE; + uint8_t aux = cmd_buf[2]&0x01; + + motorola_locomotive_reverse_packet(addr, aux); + } + else if(cmd_buf[0]==MOTOROLA_SOLENOID) + { + if(cmd_length!=3) + return LENGTH_ERROR; + + uint8_t addr = cmd_buf[1]; + if(addr>80) + return INVALID_VALUE; + + if(cmd_buf[2]&0x8E) + return INVALID_VALUE; + uint8_t output = (cmd_buf[2]&0x70)>>4; + uint8_t state = cmd_buf[2]&1; + + motorola_solenoid_packet(addr, output, state); + } + else + return INVALID_COMMAND; - packet.ready = 1; + return COMMAND_OK; }