You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
46 lines
1.4 KiB
46 lines
1.4 KiB
#include "SIM_ToshibaLED.h" |
|
|
|
#include <stdio.h> |
|
|
|
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register |
|
#define TOSHIBA_LED_PWM1 0x02 // pwm1 register |
|
#define TOSHIBA_LED_PWM2 0x03 // pwm2 register |
|
#define TOSHIBA_LED_ENABLE 0x04 // enable register |
|
|
|
int SITL::ToshibaLED::rdwr(I2C::i2c_rdwr_ioctl_data *&data) |
|
{ |
|
if (data->nmsgs != 1) { |
|
// this is really just because it is unexpected from the |
|
// ArduPilot code, rather than being incorrect from a |
|
// simulated device perspective. |
|
AP_HAL::panic("Reading from Toshiba LED?!"); |
|
} |
|
const struct I2C::i2c_msg &msg = data->msgs[0]; |
|
const uint8_t reg = msg.buf[0]; |
|
const uint8_t val = msg.buf[1]; |
|
switch(reg) { |
|
case TOSHIBA_LED_PWM0: |
|
// ::fprintf(stderr, "ToshibaLED: pwm0=%u %u %u\n", msg.buf[1], msg.buf[2], msg.buf[3]); |
|
_pwm0 = val; |
|
break; |
|
case TOSHIBA_LED_PWM1: |
|
// ::fprintf(stderr, "ToshibaLED: pwm1=%u\n", val); |
|
_pwm1 = val; |
|
break; |
|
case TOSHIBA_LED_PWM2: |
|
// ::fprintf(stderr, "ToshibaLED: pwm2=%u\n", val); |
|
_pwm2 = val; |
|
break; |
|
case TOSHIBA_LED_ENABLE: |
|
if (val != 0x03) { |
|
AP_HAL::panic("Unexpected enable value (%u)", val); |
|
} |
|
// ::fprintf(stderr, "ToshibaLED: enabling\n"); |
|
_enabled = true; |
|
break; |
|
default: |
|
AP_HAL::panic("Unexpected register (%u)", reg); |
|
} |
|
// kill(0, SIGTRAP); |
|
return -1; |
|
}
|
|
|