Comms between MAX 10 FPGA and ST uP
FPGA_bus.cpp@23:4b391cfd4f2d, 2020-06-26 (annotated)
- Committer:
- jimherd
- Date:
- Fri Jun 26 11:02:30 2020 +0000
- Revision:
- 23:4b391cfd4f2d
- Parent:
- 22:c47d4177d59c
- Child:
- 25:9cdeb5267a47
"soft_bus_check" changed to use decrementing timeout counter rather than fixed delay.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jimherd | 0:9600ed6fd725 | 1 | /* |
jimherd | 0:9600ed6fd725 | 2 | * FPGA_bus : 8-bit bi-directional bus between uP and FPGA |
jimherd | 0:9600ed6fd725 | 3 | */ |
jimherd | 0:9600ed6fd725 | 4 | #include "mbed.h" |
jimherd | 0:9600ed6fd725 | 5 | #include "FPGA_bus.h" |
jimherd | 0:9600ed6fd725 | 6 | |
jimherd | 0:9600ed6fd725 | 7 | /** create a FPGA_bus object connecting uP to FPGA |
jimherd | 0:9600ed6fd725 | 8 | * |
jimherd | 0:9600ed6fd725 | 9 | * @param nos_PWM Number of PWM channels (default = 4) |
jimherd | 0:9600ed6fd725 | 10 | * @param nos_QE Number of Quadrature Encoder channels (default = 4) |
jimherd | 0:9600ed6fd725 | 11 | * @param nos_servo Number of RC servo channels (default = 8) |
jimherd | 0:9600ed6fd725 | 12 | * |
jimherd | 0:9600ed6fd725 | 13 | * Notes |
jimherd | 0:9600ed6fd725 | 14 | * You can only change the defaults by recompiling the SystemVerilog code |
jimherd | 0:9600ed6fd725 | 15 | * on the FPGA. |
jimherd | 7:c0bef9c1f5d5 | 16 | */ |
jimherd | 7:c0bef9c1f5d5 | 17 | |
jimherd | 20:aacf2ebd93ff | 18 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 19 | |
jimherd | 14:b56473e54f6f | 20 | FPGA_bus::FPGA_bus(int nos_PWM /* = NOS_PWM_CHANNELS */, |
jimherd | 14:b56473e54f6f | 21 | int nos_QE /* = NOS_QE_CHANNELS */, |
jimherd | 14:b56473e54f6f | 22 | int nos_servo /* = NOS_RC_CHANNELS */ ) |
jimherd | 0:9600ed6fd725 | 23 | |
jimherd | 14:b56473e54f6f | 24 | : |
jimherd | 14:b56473e54f6f | 25 | _nos_PWM_units(nos_PWM), |
jimherd | 14:b56473e54f6f | 26 | _nos_QE_units(nos_QE), |
jimherd | 14:b56473e54f6f | 27 | _nos_servo_units(nos_servo), |
jimherd | 14:b56473e54f6f | 28 | |
jimherd | 14:b56473e54f6f | 29 | async_uP_start(ASYNC_UP_START_PIN), |
jimherd | 0:9600ed6fd725 | 30 | async_uP_handshake_1(ASYNC_UP_HANDSHAKE_1_PIN), |
jimherd | 0:9600ed6fd725 | 31 | async_uP_RW(ASYNC_UP_RW_PIN), |
jimherd | 0:9600ed6fd725 | 32 | async_uP_reset(ASYNC_UP_RESET_PIN), |
jimherd | 0:9600ed6fd725 | 33 | uP_ack(ASYNC_UP_ACK_PIN), |
jimherd | 9:6fe95fb0c7ea | 34 | uP_handshake_2(ASYNC_UP_HANDSHAKE_2_PIN), |
jimherd | 14:b56473e54f6f | 35 | log_pin(LOG_PIN) |
jimherd | 15:6420b52d30cc | 36 | |
jimherd | 14:b56473e54f6f | 37 | { |
jimherd | 14:b56473e54f6f | 38 | async_uP_start = LOW; |
jimherd | 15:6420b52d30cc | 39 | PWM_base = 0; |
jimherd | 15:6420b52d30cc | 40 | QE_base = 0; |
jimherd | 15:6420b52d30cc | 41 | RC_base = 0; |
jimherd | 7:c0bef9c1f5d5 | 42 | } |
jimherd | 0:9600ed6fd725 | 43 | |
jimherd | 20:aacf2ebd93ff | 44 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 45 | // do_reset : reset FPGA system |
jimherd | 20:aacf2ebd93ff | 46 | // ======== |
jimherd | 20:aacf2ebd93ff | 47 | // |
jimherd | 20:aacf2ebd93ff | 48 | // Generate a 20uS LOW going pulse to set FPGA into initial state |
jimherd | 0:9600ed6fd725 | 49 | |
jimherd | 20:aacf2ebd93ff | 50 | void FPGA_bus:: do_reset(void) |
jimherd | 0:9600ed6fd725 | 51 | { |
jimherd | 20:aacf2ebd93ff | 52 | async_uP_reset = LOW; // generate low reset pulse |
jimherd | 20:aacf2ebd93ff | 53 | wait_us(20); |
jimherd | 20:aacf2ebd93ff | 54 | async_uP_reset = HIGH; |
jimherd | 20:aacf2ebd93ff | 55 | wait_us(20); |
jimherd | 20:aacf2ebd93ff | 56 | } |
jimherd | 20:aacf2ebd93ff | 57 | |
jimherd | 20:aacf2ebd93ff | 58 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 59 | // initialise : Configure uP to FPGA data bus |
jimherd | 20:aacf2ebd93ff | 60 | // ========== |
jimherd | 20:aacf2ebd93ff | 61 | // |
jimherd | 20:aacf2ebd93ff | 62 | // Do the following |
jimherd | 20:aacf2ebd93ff | 63 | // 1. Set output signals to known values |
jimherd | 20:aacf2ebd93ff | 64 | // 2. Prod FPGA to check that it is responding correctly |
jimherd | 20:aacf2ebd93ff | 65 | // Code will make 3 attempts before returning with an ERROR |
jimherd | 20:aacf2ebd93ff | 66 | // 3. Read register ZERO to get number of units in FPGA |
jimherd | 20:aacf2ebd93ff | 67 | // This data is used to initialise the correct register pointers |
jimherd | 15:6420b52d30cc | 68 | |
jimherd | 20:aacf2ebd93ff | 69 | int32_t FPGA_bus::initialise(void) |
jimherd | 20:aacf2ebd93ff | 70 | { |
jimherd | 20:aacf2ebd93ff | 71 | int32_t status; |
jimherd | 20:aacf2ebd93ff | 72 | |
jimherd | 20:aacf2ebd93ff | 73 | status = NO_ERROR; |
jimherd | 15:6420b52d30cc | 74 | |
jimherd | 0:9600ed6fd725 | 75 | // GPIOC Periph clock enable |
jimherd | 0:9600ed6fd725 | 76 | |
jimherd | 0:9600ed6fd725 | 77 | ENABLE_GPIO_SUBSYSTEM; |
jimherd | 0:9600ed6fd725 | 78 | wait_us(2); |
jimherd | 0:9600ed6fd725 | 79 | |
jimherd | 0:9600ed6fd725 | 80 | async_uP_start = LOW; |
jimherd | 0:9600ed6fd725 | 81 | async_uP_reset = HIGH; |
jimherd | 0:9600ed6fd725 | 82 | async_uP_handshake_1 = LOW; |
jimherd | 0:9600ed6fd725 | 83 | async_uP_RW = LOW; |
jimherd | 0:9600ed6fd725 | 84 | |
jimherd | 20:aacf2ebd93ff | 85 | do_reset(); |
jimherd | 20:aacf2ebd93ff | 86 | // |
jimherd | 20:aacf2ebd93ff | 87 | // Test to see if FPGA is alive and well |
jimherd | 20:aacf2ebd93ff | 88 | // |
jimherd | 22:c47d4177d59c | 89 | status = hard_check_bus(); |
jimherd | 20:aacf2ebd93ff | 90 | if (status != NO_ERROR) { |
jimherd | 20:aacf2ebd93ff | 91 | global_FPGA_unit_error_flag = status; |
jimherd | 20:aacf2ebd93ff | 92 | return status; |
jimherd | 20:aacf2ebd93ff | 93 | } |
jimherd | 20:aacf2ebd93ff | 94 | // |
jimherd | 20:aacf2ebd93ff | 95 | // Seems OK, now read register 0 and get basic system parameters |
jimherd | 20:aacf2ebd93ff | 96 | // |
jimherd | 20:aacf2ebd93ff | 97 | wait_ms(10); |
jimherd | 20:aacf2ebd93ff | 98 | get_SYS_data(); |
jimherd | 1:b819a72b3b5d | 99 | |
jimherd | 20:aacf2ebd93ff | 100 | if (global_FPGA_unit_error_flag != NO_ERROR){ |
jimherd | 20:aacf2ebd93ff | 101 | return global_FPGA_unit_error_flag; |
jimherd | 20:aacf2ebd93ff | 102 | } |
jimherd | 1:b819a72b3b5d | 103 | global_FPGA_unit_error_flag = NO_ERROR; |
jimherd | 9:6fe95fb0c7ea | 104 | log_pin = LOW; |
jimherd | 20:aacf2ebd93ff | 105 | return NO_ERROR; |
jimherd | 0:9600ed6fd725 | 106 | } |
jimherd | 20:aacf2ebd93ff | 107 | |
jimherd | 20:aacf2ebd93ff | 108 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 109 | // do_start : generate signals to start of bus transaction |
jimherd | 20:aacf2ebd93ff | 110 | // ======== |
jimherd | 20:aacf2ebd93ff | 111 | |
jimherd | 0:9600ed6fd725 | 112 | void FPGA_bus::do_start(void) |
jimherd | 0:9600ed6fd725 | 113 | { |
jimherd | 0:9600ed6fd725 | 114 | async_uP_start = HIGH; |
jimherd | 0:9600ed6fd725 | 115 | async_uP_handshake_1 = LOW; |
jimherd | 0:9600ed6fd725 | 116 | async_uP_start = LOW; |
jimherd | 0:9600ed6fd725 | 117 | } |
jimherd | 0:9600ed6fd725 | 118 | |
jimherd | 20:aacf2ebd93ff | 119 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 120 | // do_end : do actions to complete bus transaction |
jimherd | 20:aacf2ebd93ff | 121 | // ====== |
jimherd | 20:aacf2ebd93ff | 122 | |
jimherd | 0:9600ed6fd725 | 123 | void FPGA_bus::do_end(void) |
jimherd | 0:9600ed6fd725 | 124 | { |
jimherd | 0:9600ed6fd725 | 125 | while (uP_ack == HIGH) |
jimherd | 0:9600ed6fd725 | 126 | ; |
jimherd | 0:9600ed6fd725 | 127 | async_uP_start = LOW; |
jimherd | 0:9600ed6fd725 | 128 | } |
jimherd | 0:9600ed6fd725 | 129 | |
jimherd | 20:aacf2ebd93ff | 130 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 131 | // write_byte : write a byte of data to the FPGA |
jimherd | 20:aacf2ebd93ff | 132 | // ========== |
jimherd | 20:aacf2ebd93ff | 133 | |
jimherd | 0:9600ed6fd725 | 134 | void FPGA_bus::write_byte(uint32_t byte_value) |
jimherd | 0:9600ed6fd725 | 135 | { |
jimherd | 0:9600ed6fd725 | 136 | SET_BUS_OUTPUT; |
jimherd | 0:9600ed6fd725 | 137 | OUTPUT_BYTE_TO_BUS(byte_value); |
jimherd | 0:9600ed6fd725 | 138 | async_uP_RW = WRITE_BUS; |
jimherd | 0:9600ed6fd725 | 139 | async_uP_handshake_1 = HIGH; |
jimherd | 0:9600ed6fd725 | 140 | while (uP_handshake_2 == LOW) |
jimherd | 0:9600ed6fd725 | 141 | ; |
jimherd | 0:9600ed6fd725 | 142 | async_uP_handshake_1 = LOW; |
jimherd | 0:9600ed6fd725 | 143 | while (uP_handshake_2 == HIGH) |
jimherd | 0:9600ed6fd725 | 144 | ; |
jimherd | 0:9600ed6fd725 | 145 | } |
jimherd | 0:9600ed6fd725 | 146 | |
jimherd | 20:aacf2ebd93ff | 147 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 148 | // read_byte : read a byte of data from the FPGA |
jimherd | 20:aacf2ebd93ff | 149 | // ========= |
jimherd | 20:aacf2ebd93ff | 150 | |
jimherd | 0:9600ed6fd725 | 151 | uint32_t FPGA_bus::read_byte(void) |
jimherd | 0:9600ed6fd725 | 152 | { |
jimherd | 0:9600ed6fd725 | 153 | SET_BUS_INPUT; |
jimherd | 0:9600ed6fd725 | 154 | async_uP_RW = READ_BUS; |
jimherd | 0:9600ed6fd725 | 155 | while (uP_handshake_2 == LOW) |
jimherd | 0:9600ed6fd725 | 156 | ; |
jimherd | 0:9600ed6fd725 | 157 | data = INPUT_BYTE_FROM_BUS; |
jimherd | 0:9600ed6fd725 | 158 | async_uP_handshake_1 = HIGH; |
jimherd | 0:9600ed6fd725 | 159 | while (uP_handshake_2 == HIGH) |
jimherd | 0:9600ed6fd725 | 160 | ; |
jimherd | 0:9600ed6fd725 | 161 | async_uP_handshake_1 = LOW; |
jimherd | 0:9600ed6fd725 | 162 | return data; |
jimherd | 0:9600ed6fd725 | 163 | } |
jimherd | 0:9600ed6fd725 | 164 | |
jimherd | 20:aacf2ebd93ff | 165 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 166 | // do write : write a SIX byte command to FPGA |
jimherd | 20:aacf2ebd93ff | 167 | // ======== |
jimherd | 20:aacf2ebd93ff | 168 | |
jimherd | 0:9600ed6fd725 | 169 | void FPGA_bus::do_write(uint32_t command, |
jimherd | 0:9600ed6fd725 | 170 | uint32_t register_address, |
jimherd | 0:9600ed6fd725 | 171 | uint32_t register_data) |
jimherd | 0:9600ed6fd725 | 172 | { |
jimherd | 0:9600ed6fd725 | 173 | write_byte(command); |
jimherd | 0:9600ed6fd725 | 174 | write_byte(register_address); |
jimherd | 0:9600ed6fd725 | 175 | write_byte(register_data); |
jimherd | 0:9600ed6fd725 | 176 | write_byte(register_data>>8); |
jimherd | 0:9600ed6fd725 | 177 | write_byte(register_data>>16); |
jimherd | 0:9600ed6fd725 | 178 | write_byte(register_data>>24); |
jimherd | 0:9600ed6fd725 | 179 | } |
jimherd | 0:9600ed6fd725 | 180 | |
jimherd | 20:aacf2ebd93ff | 181 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 182 | // do_read : Read the results of FPGA command execution |
jimherd | 20:aacf2ebd93ff | 183 | // ======= |
jimherd | 20:aacf2ebd93ff | 184 | |
jimherd | 0:9600ed6fd725 | 185 | void FPGA_bus::do_read(received_packet_t *buffer) |
jimherd | 0:9600ed6fd725 | 186 | { |
jimherd | 0:9600ed6fd725 | 187 | for (int i=0; i < (NOS_RECEIVED_PACKET_WORDS<<2) ; i++) { |
jimherd | 0:9600ed6fd725 | 188 | buffer->byte_data[i] = (uint8_t)read_byte(); |
jimherd | 0:9600ed6fd725 | 189 | } |
jimherd | 0:9600ed6fd725 | 190 | } |
jimherd | 0:9600ed6fd725 | 191 | |
jimherd | 20:aacf2ebd93ff | 192 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 193 | // do_transaction : execute complete command and get results |
jimherd | 20:aacf2ebd93ff | 194 | // ============== |
jimherd | 20:aacf2ebd93ff | 195 | |
jimherd | 0:9600ed6fd725 | 196 | void FPGA_bus::do_transaction(uint32_t command, |
jimherd | 0:9600ed6fd725 | 197 | uint32_t register_address, |
jimherd | 0:9600ed6fd725 | 198 | uint32_t register_data, |
jimherd | 0:9600ed6fd725 | 199 | uint32_t *data, |
jimherd | 0:9600ed6fd725 | 200 | uint32_t *status) |
jimherd | 0:9600ed6fd725 | 201 | { |
jimherd | 9:6fe95fb0c7ea | 202 | log_pin = HIGH; |
jimherd | 0:9600ed6fd725 | 203 | do_start(); |
jimherd | 0:9600ed6fd725 | 204 | do_write(command, register_address, register_data); |
jimherd | 0:9600ed6fd725 | 205 | do_read(&in_pkt); |
jimherd | 0:9600ed6fd725 | 206 | do_end(); |
jimherd | 9:6fe95fb0c7ea | 207 | log_pin = LOW; |
jimherd | 0:9600ed6fd725 | 208 | *data = in_pkt.word_data[0]; |
jimherd | 0:9600ed6fd725 | 209 | *status = in_pkt.word_data[1]; |
jimherd | 0:9600ed6fd725 | 210 | } |
jimherd | 0:9600ed6fd725 | 211 | |
jimherd | 20:aacf2ebd93ff | 212 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 213 | // set_PWM_period : set PWM period given frequency |
jimherd | 20:aacf2ebd93ff | 214 | // ============== |
jimherd | 20:aacf2ebd93ff | 215 | |
jimherd | 1:b819a72b3b5d | 216 | void FPGA_bus::set_PWM_period(uint32_t channel, float frequency) |
jimherd | 0:9600ed6fd725 | 217 | { |
jimherd | 16:d69a36a541c5 | 218 | uint32_t register_address = ((PWM_base + (channel * NOS_PWM_REGISTERS)) + PWM_PERIOD); |
jimherd | 0:9600ed6fd725 | 219 | uint32_t period_value = (uint32_t)(1000000/(20 * frequency)); |
jimherd | 4:e5d36eee9245 | 220 | do_transaction(WRITE_REGISTER_CMD, register_address, period_value, &data, &status); |
jimherd | 0:9600ed6fd725 | 221 | sys_data.PWM_period_value[channel] = period_value; |
jimherd | 17:928b755cba80 | 222 | do_transaction(READ_REGISTER_CMD, (1 + (channel * NOS_PWM_REGISTERS)) , NULL, &data, &status); |
jimherd | 1:b819a72b3b5d | 223 | global_FPGA_unit_error_flag = status; |
jimherd | 0:9600ed6fd725 | 224 | } |
jimherd | 0:9600ed6fd725 | 225 | |
jimherd | 20:aacf2ebd93ff | 226 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 227 | // set_PWM_duty : set puty time at % of period |
jimherd | 20:aacf2ebd93ff | 228 | // ============ |
jimherd | 20:aacf2ebd93ff | 229 | |
jimherd | 1:b819a72b3b5d | 230 | void FPGA_bus::set_PWM_duty(uint32_t channel, float percentage) |
jimherd | 0:9600ed6fd725 | 231 | { |
jimherd | 16:d69a36a541c5 | 232 | uint32_t register_address = ((PWM_base + (channel * NOS_PWM_REGISTERS)) + PWM_ON_TIME); |
jimherd | 0:9600ed6fd725 | 233 | uint32_t duty_value = (uint32_t)((sys_data.PWM_period_value[channel] * percentage) / 100); |
jimherd | 4:e5d36eee9245 | 234 | do_transaction(WRITE_REGISTER_CMD, register_address , duty_value, &data, &status); |
jimherd | 0:9600ed6fd725 | 235 | sys_data.PWM_duty_value[channel] = duty_value; |
jimherd | 1:b819a72b3b5d | 236 | global_FPGA_unit_error_flag = status;; |
jimherd | 0:9600ed6fd725 | 237 | } |
jimherd | 0:9600ed6fd725 | 238 | |
jimherd | 20:aacf2ebd93ff | 239 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 240 | // PWM_enable : enable PWM output |
jimherd | 20:aacf2ebd93ff | 241 | // ========== |
jimherd | 20:aacf2ebd93ff | 242 | |
jimherd | 1:b819a72b3b5d | 243 | void FPGA_bus::PWM_enable(uint32_t channel) |
jimherd | 0:9600ed6fd725 | 244 | { |
jimherd | 16:d69a36a541c5 | 245 | uint32_t register_address = ((PWM_base + (channel * NOS_PWM_REGISTERS)) + PWM_CONFIG); |
jimherd | 4:e5d36eee9245 | 246 | do_transaction(WRITE_REGISTER_CMD, register_address , 1, &data, &status); |
jimherd | 1:b819a72b3b5d | 247 | global_FPGA_unit_error_flag = status;; |
jimherd | 0:9600ed6fd725 | 248 | } |
jimherd | 0:9600ed6fd725 | 249 | |
jimherd | 20:aacf2ebd93ff | 250 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 251 | // PWM_config : program PWM configuration register |
jimherd | 20:aacf2ebd93ff | 252 | // ========== |
jimherd | 20:aacf2ebd93ff | 253 | |
jimherd | 1:b819a72b3b5d | 254 | void FPGA_bus::PWM_config(uint32_t channel, uint32_t config_value) |
jimherd | 0:9600ed6fd725 | 255 | { |
jimherd | 16:d69a36a541c5 | 256 | uint32_t register_address = ((PWM_base + (channel * NOS_PWM_REGISTERS)) + PWM_CONFIG); |
jimherd | 4:e5d36eee9245 | 257 | do_transaction(WRITE_REGISTER_CMD, register_address , config_value, &data, &status); |
jimherd | 0:9600ed6fd725 | 258 | sys_data.PWM_duty_value[channel] = config_value; |
jimherd | 1:b819a72b3b5d | 259 | global_FPGA_unit_error_flag = status;; |
jimherd | 1:b819a72b3b5d | 260 | } |
jimherd | 1:b819a72b3b5d | 261 | |
jimherd | 20:aacf2ebd93ff | 262 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 263 | // set_RC_period : set RC period to 20mS for all servos |
jimherd | 20:aacf2ebd93ff | 264 | // ============= |
jimherd | 20:aacf2ebd93ff | 265 | // |
jimherd | 20:aacf2ebd93ff | 266 | // No parameter therefore assume 20mS period |
jimherd | 20:aacf2ebd93ff | 267 | // |
jimherd | 20:aacf2ebd93ff | 268 | // FPGA clock = 20nS => 20mS = 1,000,000 clocks counts |
jimherd | 20:aacf2ebd93ff | 269 | |
jimherd | 4:e5d36eee9245 | 270 | void FPGA_bus::set_RC_period(void) |
jimherd | 1:b819a72b3b5d | 271 | { |
jimherd | 16:d69a36a541c5 | 272 | do_transaction(WRITE_REGISTER_CMD, (RC_base + RC_SERVO_PERIOD), 1000000, &data, &status); |
jimherd | 6:e68defb7b775 | 273 | global_FPGA_unit_error_flag = status; |
jimherd | 1:b819a72b3b5d | 274 | } |
jimherd | 1:b819a72b3b5d | 275 | |
jimherd | 20:aacf2ebd93ff | 276 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 277 | // set_RC_period : set RC pulse in units of uS for all servos |
jimherd | 20:aacf2ebd93ff | 278 | // ============= |
jimherd | 20:aacf2ebd93ff | 279 | // |
jimherd | 20:aacf2ebd93ff | 280 | |
jimherd | 4:e5d36eee9245 | 281 | void FPGA_bus::set_RC_period(uint32_t duty_uS) |
jimherd | 1:b819a72b3b5d | 282 | { |
jimherd | 1:b819a72b3b5d | 283 | uint32_t nos_20nS_ticks = ((duty_uS * nS_IN_uS)/FPGA_CLOCK_PERIOD_nS); |
jimherd | 16:d69a36a541c5 | 284 | do_transaction(WRITE_REGISTER_CMD, (RC_base + RC_SERVO_PERIOD), nos_20nS_ticks, &data, &status); |
jimherd | 6:e68defb7b775 | 285 | global_FPGA_unit_error_flag = status; |
jimherd | 1:b819a72b3b5d | 286 | } |
jimherd | 1:b819a72b3b5d | 287 | |
jimherd | 20:aacf2ebd93ff | 288 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 289 | // set_RC_pulse : set RC pulse width in uS for a particular servo |
jimherd | 20:aacf2ebd93ff | 290 | // ============ |
jimherd | 20:aacf2ebd93ff | 291 | // |
jimherd | 20:aacf2ebd93ff | 292 | |
jimherd | 1:b819a72b3b5d | 293 | void FPGA_bus :: set_RC_pulse(uint32_t channel, uint32_t pulse_uS) |
jimherd | 1:b819a72b3b5d | 294 | { |
jimherd | 1:b819a72b3b5d | 295 | uint32_t nos_20nS_ticks = ((pulse_uS * nS_IN_uS)/FPGA_CLOCK_PERIOD_nS); |
jimherd | 16:d69a36a541c5 | 296 | do_transaction(WRITE_REGISTER_CMD, (RC_base + RC_SERVO_ON_TIME + channel), nos_20nS_ticks, &data, &status); |
jimherd | 1:b819a72b3b5d | 297 | global_FPGA_unit_error_flag = status;; |
jimherd | 1:b819a72b3b5d | 298 | } |
jimherd | 1:b819a72b3b5d | 299 | |
jimherd | 20:aacf2ebd93ff | 300 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 301 | // enable_RC_channel : enable RC servo channel output |
jimherd | 20:aacf2ebd93ff | 302 | // ================= |
jimherd | 20:aacf2ebd93ff | 303 | |
jimherd | 4:e5d36eee9245 | 304 | void FPGA_bus::enable_RC_channel(uint32_t channel) |
jimherd | 1:b819a72b3b5d | 305 | { |
jimherd | 16:d69a36a541c5 | 306 | do_transaction(READ_REGISTER_CMD, (RC_base + RC_SERVO_CONFIG), NULL, &data, &status); |
jimherd | 2:fd5c862b86db | 307 | int32_t config = (data || (0x01 << channel)) + GLOBAL_RC_ENABLE; |
jimherd | 16:d69a36a541c5 | 308 | do_transaction(WRITE_REGISTER_CMD, (RC_base + RC_SERVO_CONFIG), config, &data, &status); |
jimherd | 3:cf36c2d4208f | 309 | global_FPGA_unit_error_flag = status; |
jimherd | 2:fd5c862b86db | 310 | } |
jimherd | 2:fd5c862b86db | 311 | |
jimherd | 20:aacf2ebd93ff | 312 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 313 | // disable_RC_channel : disable RC servo channel output |
jimherd | 20:aacf2ebd93ff | 314 | // ================== |
jimherd | 20:aacf2ebd93ff | 315 | |
jimherd | 4:e5d36eee9245 | 316 | void FPGA_bus::disable_RC_channel(uint32_t channel) |
jimherd | 2:fd5c862b86db | 317 | { |
jimherd | 16:d69a36a541c5 | 318 | do_transaction(READ_REGISTER_CMD, (RC_base + RC_SERVO_CONFIG), NULL, &data, &status); |
jimherd | 4:e5d36eee9245 | 319 | uint32_t config = data && ~(0x01 << channel); |
jimherd | 16:d69a36a541c5 | 320 | do_transaction(WRITE_REGISTER_CMD, (RC_base + RC_SERVO_CONFIG), config, &data, &status); |
jimherd | 1:b819a72b3b5d | 321 | global_FPGA_unit_error_flag = status; |
jimherd | 4:e5d36eee9245 | 322 | } |
jimherd | 4:e5d36eee9245 | 323 | |
jimherd | 20:aacf2ebd93ff | 324 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 325 | // QE_config : configure quadrature encoder unit |
jimherd | 20:aacf2ebd93ff | 326 | // ========= |
jimherd | 20:aacf2ebd93ff | 327 | |
jimherd | 9:6fe95fb0c7ea | 328 | void FPGA_bus::QE_config(uint32_t channel, uint32_t config_value) |
jimherd | 9:6fe95fb0c7ea | 329 | { |
jimherd | 16:d69a36a541c5 | 330 | uint32_t register_address = ((PWM_base + (channel * NOS_PWM_REGISTERS)) + PWM_CONFIG); |
jimherd | 9:6fe95fb0c7ea | 331 | do_transaction(WRITE_REGISTER_CMD, register_address , config_value, &data, &status); |
jimherd | 9:6fe95fb0c7ea | 332 | global_FPGA_unit_error_flag = status;; |
jimherd | 9:6fe95fb0c7ea | 333 | } |
jimherd | 9:6fe95fb0c7ea | 334 | |
jimherd | 20:aacf2ebd93ff | 335 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 336 | // enable_speed_measure : enable speed calculation of encoder channel |
jimherd | 20:aacf2ebd93ff | 337 | // ==================== |
jimherd | 20:aacf2ebd93ff | 338 | |
jimherd | 9:6fe95fb0c7ea | 339 | void FPGA_bus::enable_speed_measure(uint32_t channel, uint32_t config_value, uint32_t phase_time) |
jimherd | 4:e5d36eee9245 | 340 | { |
jimherd | 16:d69a36a541c5 | 341 | uint32_t register_base = (QE_base + (channel * NOS_QE_REGISTERS)); |
jimherd | 9:6fe95fb0c7ea | 342 | do_transaction(WRITE_REGISTER_CMD, (register_base + QE_SIM_PHASE_TIME), phase_time, &data, &status); |
jimherd | 5:64c677e9995c | 343 | global_FPGA_unit_error_flag = status; |
jimherd | 10:56a045a02047 | 344 | if (status != NO_ERROR) { |
jimherd | 10:56a045a02047 | 345 | return; |
jimherd | 10:56a045a02047 | 346 | } |
jimherd | 10:56a045a02047 | 347 | do_transaction(WRITE_REGISTER_CMD, (register_base + QE_CONFIG), config_value, &data, &status); |
jimherd | 10:56a045a02047 | 348 | global_FPGA_unit_error_flag = status; |
jimherd | 5:64c677e9995c | 349 | } |
jimherd | 5:64c677e9995c | 350 | |
jimherd | 20:aacf2ebd93ff | 351 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 352 | // read_speed_measure : read current speed measurement |
jimherd | 20:aacf2ebd93ff | 353 | // ================== |
jimherd | 20:aacf2ebd93ff | 354 | |
jimherd | 5:64c677e9995c | 355 | uint32_t FPGA_bus::read_speed_measure(uint32_t channel) |
jimherd | 5:64c677e9995c | 356 | { |
jimherd | 16:d69a36a541c5 | 357 | uint32_t register_address = ((QE_base + (channel * NOS_QE_REGISTERS)) + QE_SPEED_BUFFER); |
jimherd | 5:64c677e9995c | 358 | do_transaction(READ_REGISTER_CMD, register_address, NULL, &data, &status); |
jimherd | 5:64c677e9995c | 359 | global_FPGA_unit_error_flag = status; |
jimherd | 5:64c677e9995c | 360 | return data; |
jimherd | 6:e68defb7b775 | 361 | } |
jimherd | 6:e68defb7b775 | 362 | |
jimherd | 20:aacf2ebd93ff | 363 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 364 | // read_count_measure : read encoder counter register |
jimherd | 20:aacf2ebd93ff | 365 | // ================== |
jimherd | 20:aacf2ebd93ff | 366 | |
jimherd | 10:56a045a02047 | 367 | uint32_t FPGA_bus::read_count_measure(uint32_t channel) |
jimherd | 10:56a045a02047 | 368 | { |
jimherd | 16:d69a36a541c5 | 369 | uint32_t register_address = ((QE_base + (channel * NOS_QE_REGISTERS)) + QE_COUNT_BUFFER); |
jimherd | 10:56a045a02047 | 370 | do_transaction(READ_REGISTER_CMD, register_address, NULL, &data, &status); |
jimherd | 10:56a045a02047 | 371 | global_FPGA_unit_error_flag = status; |
jimherd | 10:56a045a02047 | 372 | return data; |
jimherd | 10:56a045a02047 | 373 | } |
jimherd | 10:56a045a02047 | 374 | |
jimherd | 20:aacf2ebd93ff | 375 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 376 | // get_SYS_data : read system data from register 0 |
jimherd | 20:aacf2ebd93ff | 377 | // ============ |
jimherd | 20:aacf2ebd93ff | 378 | // |
jimherd | 20:aacf2ebd93ff | 379 | // Notes |
jimherd | 20:aacf2ebd93ff | 380 | // Register data has information to define where the subsystem registers |
jimherd | 20:aacf2ebd93ff | 381 | // are located. The software will therefore be independent of the |
jimherd | 20:aacf2ebd93ff | 382 | // number of subsystems in the FPGA |
jimherd | 20:aacf2ebd93ff | 383 | |
jimherd | 6:e68defb7b775 | 384 | uint32_t FPGA_bus::get_SYS_data(void) |
jimherd | 6:e68defb7b775 | 385 | { |
jimherd | 6:e68defb7b775 | 386 | do_transaction(READ_REGISTER_CMD, SYS_DATA_REG_ADDR, NULL, &data, &status); |
jimherd | 6:e68defb7b775 | 387 | sys_data.major_version = (data & 0x0000000F); |
jimherd | 20:aacf2ebd93ff | 388 | sys_data.minor_version = (data >> 4) & 0x0000000F; |
jimherd | 20:aacf2ebd93ff | 389 | sys_data.number_of_PWM_channels = (data >> 8) & 0x0000000F; |
jimherd | 20:aacf2ebd93ff | 390 | sys_data.number_of_QE_channels = (data >> 12) & 0x0000000F; |
jimherd | 20:aacf2ebd93ff | 391 | sys_data.number_of_RC_channels = (data >> 16) & 0x0000000F; |
jimherd | 20:aacf2ebd93ff | 392 | |
jimherd | 20:aacf2ebd93ff | 393 | update_FPGA_register_pointers(); |
jimherd | 6:e68defb7b775 | 394 | |
jimherd | 6:e68defb7b775 | 395 | global_FPGA_unit_error_flag = status; |
jimherd | 6:e68defb7b775 | 396 | return data; |
jimherd | 15:6420b52d30cc | 397 | } |
jimherd | 20:aacf2ebd93ff | 398 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 20:aacf2ebd93ff | 399 | // update_FPGA_register_pointers : set pointer to FPGA register bank |
jimherd | 20:aacf2ebd93ff | 400 | // ============================= |
jimherd | 20:aacf2ebd93ff | 401 | // |
jimherd | 20:aacf2ebd93ff | 402 | // Accessing the units within the FPGA is by reading and writing to registers. |
jimherd | 20:aacf2ebd93ff | 403 | // This block of registers is a contiguous block starting at ZERO, up to |
jimherd | 20:aacf2ebd93ff | 404 | // a maximum of 255. This implies that if the FPGA is configured with more |
jimherd | 20:aacf2ebd93ff | 405 | // PWM units (say), then the addresses of the later units are changed. |
jimherd | 20:aacf2ebd93ff | 406 | // |
jimherd | 20:aacf2ebd93ff | 407 | // To overcome this problem, register ZERO contains a note of the number of |
jimherd | 20:aacf2ebd93ff | 408 | // PWM, QE, abd RC servo units. This address is fixed and is read to |
jimherd | 20:aacf2ebd93ff | 409 | // calculate the relevant register address pointers. |
jimherd | 20:aacf2ebd93ff | 410 | // |
jimherd | 15:6420b52d30cc | 411 | |
jimherd | 15:6420b52d30cc | 412 | void FPGA_bus::update_FPGA_register_pointers(void) { |
jimherd | 15:6420b52d30cc | 413 | |
jimherd | 15:6420b52d30cc | 414 | PWM_base = 1; |
jimherd | 20:aacf2ebd93ff | 415 | QE_base = ((NOS_PWM_REGISTERS * sys_data.number_of_PWM_channels) + PWM_base); |
jimherd | 20:aacf2ebd93ff | 416 | RC_base = ((NOS_QE_REGISTERS * sys_data.number_of_QE_channels) + QE_base); |
jimherd | 20:aacf2ebd93ff | 417 | } |
jimherd | 20:aacf2ebd93ff | 418 | |
jimherd | 20:aacf2ebd93ff | 419 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 22:c47d4177d59c | 420 | // hard_check_bus : verify that connection to FPGA is working (uses RESET) |
jimherd | 22:c47d4177d59c | 421 | // ============== |
jimherd | 20:aacf2ebd93ff | 422 | // |
jimherd | 20:aacf2ebd93ff | 423 | // Notes |
jimherd | 20:aacf2ebd93ff | 424 | // Timeout on a first handshake after a RESET. Uses first part of |
jimherd | 20:aacf2ebd93ff | 425 | // 'write_byte' handshake to test bus |
jimherd | 20:aacf2ebd93ff | 426 | // Return |
jimherd | 22:c47d4177d59c | 427 | // NO_ERROR = bus active and replying correctly |
jimherd | 22:c47d4177d59c | 428 | // BUS_FAIL_1 = unable to get response from FPGA through bus |
jimherd | 22:c47d4177d59c | 429 | // BUS_FAIL_2 = unable to get response from FPGA through bus |
jimherd | 20:aacf2ebd93ff | 430 | // |
jimherd | 22:c47d4177d59c | 431 | int32_t FPGA_bus::hard_check_bus(void) |
jimherd | 20:aacf2ebd93ff | 432 | { |
jimherd | 21:6b2b7a0e2d9a | 433 | do_reset(); |
jimherd | 21:6b2b7a0e2d9a | 434 | wait_us(uS_DELAY_BEFORE_TEST_HANDSHAKE); |
jimherd | 20:aacf2ebd93ff | 435 | |
jimherd | 21:6b2b7a0e2d9a | 436 | if (uP_handshake_2 == HIGH) { |
jimherd | 21:6b2b7a0e2d9a | 437 | return BUS_FAIL_1; // handshake line in wrong init state |
jimherd | 20:aacf2ebd93ff | 438 | } |
jimherd | 21:6b2b7a0e2d9a | 439 | |
jimherd | 21:6b2b7a0e2d9a | 440 | do_start(); |
jimherd | 21:6b2b7a0e2d9a | 441 | SET_BUS_INPUT; |
jimherd | 21:6b2b7a0e2d9a | 442 | async_uP_RW = READ_BUS; |
jimherd | 21:6b2b7a0e2d9a | 443 | SET_BUS_OUTPUT; |
jimherd | 21:6b2b7a0e2d9a | 444 | OUTPUT_BYTE_TO_BUS(0); |
jimherd | 21:6b2b7a0e2d9a | 445 | async_uP_RW = WRITE_BUS; |
jimherd | 21:6b2b7a0e2d9a | 446 | async_uP_handshake_1 = HIGH; |
jimherd | 21:6b2b7a0e2d9a | 447 | wait_us(uS_DELAY_BEFORE_TEST_HANDSHAKE); |
jimherd | 21:6b2b7a0e2d9a | 448 | |
jimherd | 21:6b2b7a0e2d9a | 449 | if (uP_handshake_2 == LOW) { |
jimherd | 21:6b2b7a0e2d9a | 450 | return BUS_FAIL_2; |
jimherd | 21:6b2b7a0e2d9a | 451 | } |
jimherd | 20:aacf2ebd93ff | 452 | // |
jimherd | 20:aacf2ebd93ff | 453 | // set system back to original state |
jimherd | 20:aacf2ebd93ff | 454 | // |
jimherd | 20:aacf2ebd93ff | 455 | async_uP_start = LOW; |
jimherd | 20:aacf2ebd93ff | 456 | async_uP_reset = HIGH; |
jimherd | 20:aacf2ebd93ff | 457 | async_uP_handshake_1 = LOW; |
jimherd | 20:aacf2ebd93ff | 458 | async_uP_RW = LOW; |
jimherd | 20:aacf2ebd93ff | 459 | do_reset(); |
jimherd | 21:6b2b7a0e2d9a | 460 | return NO_ERROR; |
jimherd | 20:aacf2ebd93ff | 461 | } |
jimherd | 20:aacf2ebd93ff | 462 | |
jimherd | 22:c47d4177d59c | 463 | ////////////////////////////////////////////////////////////////////////// |
jimherd | 22:c47d4177d59c | 464 | // soft_check_bus : verify that connection to FPGA is working (no RESET) |
jimherd | 22:c47d4177d59c | 465 | // ============== |
jimherd | 22:c47d4177d59c | 466 | // |
jimherd | 22:c47d4177d59c | 467 | // Notes |
jimherd | 22:c47d4177d59c | 468 | // FPGA-uP bus FSM special cased to accept a handshake_2 signal BEFORE |
jimherd | 22:c47d4177d59c | 469 | // a START signal. If handshake replies are not recieved in a timely |
jimherd | 22:c47d4177d59c | 470 | // manner, error codes are returned. |
jimherd | 22:c47d4177d59c | 471 | // Return |
jimherd | 22:c47d4177d59c | 472 | // NO_ERROR = bus active and replying correctly |
jimherd | 22:c47d4177d59c | 473 | // BUS_FAIL_1 = handshake line in wrong init state |
jimherd | 22:c47d4177d59c | 474 | // BUS_FAIL_2 = no handshake response |
jimherd | 22:c47d4177d59c | 475 | // BUS_FAIL_3 = handshake failed to terminate correctly |
jimherd | 22:c47d4177d59c | 476 | // |
jimherd | 23:4b391cfd4f2d | 477 | |
jimherd | 22:c47d4177d59c | 478 | int32_t FPGA_bus::soft_check_bus(void) |
jimherd | 22:c47d4177d59c | 479 | { |
jimherd | 23:4b391cfd4f2d | 480 | uint32_t timeout_counter; |
jimherd | 22:c47d4177d59c | 481 | |
jimherd | 23:4b391cfd4f2d | 482 | timeout_counter = HANDSHAKE_TIMEOUT_COUNT; |
jimherd | 23:4b391cfd4f2d | 483 | while (uP_handshake_2 == HIGH) { |
jimherd | 23:4b391cfd4f2d | 484 | timeout_counter--; |
jimherd | 23:4b391cfd4f2d | 485 | if (timeout_counter == 0) { |
jimherd | 23:4b391cfd4f2d | 486 | return BUS_FAIL_1; // handshake line in wrong init state |
jimherd | 23:4b391cfd4f2d | 487 | } |
jimherd | 22:c47d4177d59c | 488 | } |
jimherd | 22:c47d4177d59c | 489 | async_uP_handshake_1 = HIGH; |
jimherd | 23:4b391cfd4f2d | 490 | timeout_counter = HANDSHAKE_TIMEOUT_COUNT; |
jimherd | 23:4b391cfd4f2d | 491 | while (uP_handshake_2 == LOW) { |
jimherd | 23:4b391cfd4f2d | 492 | timeout_counter--; |
jimherd | 23:4b391cfd4f2d | 493 | if (timeout_counter == 0) { |
jimherd | 23:4b391cfd4f2d | 494 | async_uP_handshake_1 = LOW; |
jimherd | 23:4b391cfd4f2d | 495 | return BUS_FAIL_1; // no handshake response |
jimherd | 23:4b391cfd4f2d | 496 | } |
jimherd | 22:c47d4177d59c | 497 | } |
jimherd | 22:c47d4177d59c | 498 | async_uP_handshake_1 = LOW; |
jimherd | 23:4b391cfd4f2d | 499 | timeout_counter = HANDSHAKE_TIMEOUT_COUNT; |
jimherd | 23:4b391cfd4f2d | 500 | while (uP_handshake_2 == HIGH) { |
jimherd | 23:4b391cfd4f2d | 501 | timeout_counter--; |
jimherd | 23:4b391cfd4f2d | 502 | if (timeout_counter == 0) { |
jimherd | 23:4b391cfd4f2d | 503 | return BUS_FAIL_2; // handshake failed to terminate correctly |
jimherd | 23:4b391cfd4f2d | 504 | } |
jimherd | 23:4b391cfd4f2d | 505 | } |
jimherd | 22:c47d4177d59c | 506 | return NO_ERROR; |
jimherd | 22:c47d4177d59c | 507 | } |
jimherd | 22:c47d4177d59c | 508 | |
jimherd | 20:aacf2ebd93ff | 509 |