Diff: FPGA_bus.cpp

Revision:

2:fd5c862b86db

Parent:

1:b819a72b3b5d

Child:

3:cf36c2d4208f

--- a/FPGA_bus.cpp	Wed Apr 17 14:32:22 2019 +0000
+++ b/FPGA_bus.cpp	Wed Apr 17 15:58:52 2019 +0000
@@ -200,14 +200,22 @@
 void FPGA_bus :: set_RC_pulse(uint32_t channel, uint32_t pulse_uS)
 {
     uint32_t nos_20nS_ticks = ((pulse_uS * nS_IN_uS)/FPGA_CLOCK_PERIOD_nS);
-    do_transaction(WRITE_REGISTER_CMD, (RC_0 + RC_SERVO_PERIOD), nos_20nS_ticks, &data, &status);
+    do_transaction(WRITE_REGISTER_CMD, (RC_0 + RC_SERVO_ON_TIME + channel), nos_20nS_ticks, &data, &status);
     global_FPGA_unit_error_flag = status;;    
 }
 
 void FPGA_bus ::enable_RC_channel(uint32_t channel)
 {
     do_transaction(READ_REGISTER_CMD, (RC_0 + RC_SERVO_CONFIG), NULL, &data, &status);
-    int32_t config = data || (0x01 << channel);
+    int32_t config = (data || (0x01 << channel)) + GLOBAL_RC_ENABLE;
+    do_transaction(WRITE_REGISTER_CMD, (RC_0 + RC_SERVO_CONFIG), config, &data, &status);
+    global_FPGA_unit_error_flag = config;   // = status;
+}
+
+void FPGA_bus ::disable_RC_channel(uint32_t channel)
+{
+    do_transaction(READ_REGISTER_CMD, (RC_0 + RC_SERVO_CONFIG), NULL, &data, &status);
+    int32_t config = data && ~(0x01 << channel);
     do_transaction(WRITE_REGISTER_CMD, (RC_0 + RC_SERVO_CONFIG), config, &data, &status);
     global_FPGA_unit_error_flag = status;
 }
\ No newline at end of file