Line data Source code
1 : #include "SIM_config.h" 2 : 3 : #if AP_SIM_LP5562_ENABLED 4 : 5 : #include "SIM_LP5562.h" 6 : 7 : using namespace SITL; 8 : 9 : #include <stdio.h> 10 : 11 2094 : void LP5562::init() 12 : { 13 2094 : add_register("ENABLE", LP5562DevReg::ENABLE, I2CRegisters::RegMode::RDWR); 14 2094 : add_register("OP_MODE", LP5562DevReg::OP_MODE, I2CRegisters::RegMode::RDWR); 15 : 16 2094 : add_register("B_CURRENT", LP5562DevReg::B_CURRENT, I2CRegisters::RegMode::RDWR); 17 2094 : add_register("G_CURRENT", LP5562DevReg::G_CURRENT, I2CRegisters::RegMode::RDWR); 18 2094 : add_register("R_CURRENT", LP5562DevReg::R_CURRENT, I2CRegisters::RegMode::RDWR); 19 : 20 2094 : add_register("B_PWM", LP5562DevReg::B_PWM, I2CRegisters::RegMode::RDWR); 21 2094 : add_register("G_PWM", LP5562DevReg::G_PWM, I2CRegisters::RegMode::RDWR); 22 2094 : add_register("R_PWM", LP5562DevReg::R_PWM, I2CRegisters::RegMode::RDWR); 23 : 24 2094 : add_register("CONFIG", LP5562DevReg::CONFIG, I2CRegisters::RegMode::RDWR); 25 : 26 2094 : add_register("STATUS", LP5562DevReg::STATUS, I2CRegisters::RegMode::RDONLY); 27 2094 : add_register("RESET", LP5562DevReg::RESET, I2CRegisters::RegMode::RDWR); 28 : 29 2094 : add_register("LED_MAP", LP5562DevReg::LED_MAP, I2CRegisters::RegMode::RDWR); 30 : 31 2094 : reset_registers(); 32 : 33 : // create objects for each of the channels to handle the dynamics 34 : // of updating each. The channel gets references to the relevant 35 : // configuration bytes. This seems over-kill for the way we 36 : // currently use this device but is a structure we use elsewhere. 37 : // Note that this assumed that the LED Map register is set to its 38 : // default value. 39 2094 : b = NEW_NOTHROW LEDChannel( 40 2094 : byte[(uint8_t)LP5562DevReg::B_PWM] 41 2094 : ); 42 2094 : g = NEW_NOTHROW LEDChannel( 43 2094 : byte[(uint8_t)LP5562DevReg::G_PWM] 44 2094 : ); 45 2094 : r = NEW_NOTHROW LEDChannel( 46 2094 : byte[(uint8_t)LP5562DevReg::R_PWM] 47 2094 : ); 48 : 49 2094 : rgbled.init(); 50 2094 : } 51 : 52 4103 : void LP5562::reset_registers() 53 : { 54 4103 : set_register(LP5562DevReg::LED_MAP, (uint8_t)0b00111001); 55 4103 : set_register(LP5562DevReg::B_CURRENT, (uint8_t)0xAF); 56 4103 : set_register(LP5562DevReg::G_CURRENT, (uint8_t)0xAF); 57 4103 : set_register(LP5562DevReg::R_CURRENT, (uint8_t)0xAF); 58 4103 : set_register(LP5562DevReg::RESET, (uint8_t)0); 59 4103 : } 60 : 61 178759942 : void LP5562::update(const class Aircraft &aircraft) 62 : { 63 : { // check the state of the reset register 64 : union { 65 : struct { 66 : uint8_t reset; 67 : } bits; 68 : uint8_t byte; 69 : } reset_reg; 70 178759942 : reset_reg.byte = get_register(LP5562DevReg::RESET); 71 : 72 : // 0xff in this register resets the device: 73 178759942 : if (reset_reg.bits.reset == 0xFF) { 74 2009 : reset_registers(); 75 : } 76 : } 77 : 78 : { // check the state of the enable register 79 : union { 80 : struct { 81 : uint8_t eng3_exc : 2; 82 : uint8_t eng2_exc : 2; 83 : uint8_t eng1_exc : 2; 84 : uint8_t chip_en : 1; 85 : uint8_t log_en : 1; 86 : } bits; 87 : uint8_t byte; 88 : } enable_reg; 89 178759942 : enable_reg.byte = get_register(LP5562DevReg::ENABLE); 90 : 91 : // we don't get fancy with the device: 92 178759942 : if (enable_reg.bits.eng3_exc || enable_reg.bits.eng2_exc || enable_reg.bits.eng1_exc) { 93 0 : AP_HAL::panic("engine execute bits set"); 94 : } 95 : 96 178759942 : if (!enable_reg.bits.chip_en) { 97 : // do nothing, maintain outputs; not sure what actual behaviour is 98 202090 : return; 99 : } 100 : } 101 : 102 : { // check the state of the config register 103 : union { 104 : struct { 105 : uint8_t internal_clock : 1; 106 : uint8_t autodetect : 1; 107 : uint8_t unused2 : 1; 108 : uint8_t unused3 : 1; 109 : uint8_t unused4 : 1; 110 : uint8_t pwrsav_en : 1; 111 : uint8_t pwm_hf : 1; 112 : } bits; 113 : uint8_t byte; 114 : } config_reg; 115 178557852 : config_reg.byte = get_register(LP5562DevReg::CONFIG); 116 : // we don't get fancy with the device. Until the driver sets 117 : // internal clock enabled we do nothing (assume no external clock) 118 178557852 : if (config_reg.bits.internal_clock != 1) { 119 2009 : fprintf(stderr, "Waiting for internal clock bits to be set (current=0x%02x)\n", config_reg.byte); 120 2009 : return; 121 : } 122 : } 123 : 124 : { // check the state of the LED Map register 125 : union { 126 : struct { 127 : uint8_t b_eng_sel : 2; 128 : uint8_t g_eng_sel : 2; 129 : uint8_t r_eng_sel : 2; 130 : uint8_t w_eng_sel : 2; 131 : } bits; 132 : uint8_t byte; 133 : } led_map_reg; 134 178555843 : led_map_reg.byte = get_register(LP5562DevReg::LED_MAP); 135 : // we don't get fancy with the device. We expect this to be 136 : // at its reset value 137 178555843 : if (led_map_reg.bits.b_eng_sel != 0x00 || 138 178555843 : led_map_reg.bits.g_eng_sel != 0x00 || 139 178555843 : led_map_reg.bits.r_eng_sel != 0x00 140 : ) { 141 0 : fprintf(stderr, "Waiting for LED mode to be set to take-from-register\n"); 142 0 : return; 143 : } 144 : } 145 : 146 : // update each channel. 147 178555843 : b->update(); 148 178555843 : g->update(); 149 178555843 : r->update(); 150 : 151 178555843 : rgbled.set_colours( 152 178555843 : r->current_value(), 153 178555843 : g->current_value(), 154 178555843 : b->current_value() 155 : ); 156 : } 157 : 158 535667529 : void LP5562::LEDChannel::update() 159 : { 160 : // nothing to update here ATM; the output value is just the input value 161 535667529 : } 162 : 163 : #endif
