void dl_parse_msg( void ) {
uint8_t msg_id = IdOfMsg(dl_buffer);
if (msg_id == DL_SET_ACTUATOR) {
uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
LED_TOGGLE(2);
if (servo_no < SERVOS_NB)
SetServo(servo_no, servo_value);
}
#ifdef DlSetting
else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_SETTING_index(dl_buffer);
float val = DL_SETTING_value(dl_buffer);
DlSetting(i, val);
LED_TOGGLE(2);
for (int j=0 ; j<8 ; j++) {
SetServo(j,actuators[j]);
}
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
#endif
}
void parse_mavpilot_msg( void )
{
if (intermcu_data.msg_class == MSG_INTERMCU_ID)
{
if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID)
{
#if COMMANDS_NB > 8
#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
#endif
for (int i=0; i< COMMANDS_NB; i++)
{
ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i));
}
#ifdef LINK_MCU_LED
LED_TOGGLE(LINK_MCU_LED);
#endif
inter_mcu_received_ap = TRUE;
}
else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID)
{
#if RADIO_CONTROL_NB_CHANNEL > 10
#error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
#endif
for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++)
{
fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i));
}
}
else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID)
{
ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
}
else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID)
{
fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf);
fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf);
fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf);
fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf);
fbw_state->current = MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf);
#ifdef LINK_MCU_LED
LED_TOGGLE(LINK_MCU_LED);
#endif
inter_mcu_received_fbw = TRUE;
}
}
}
/* Sets the actual actuator commands */
STATIC_INLINE void main_periodic(void)
{
/* Inter-MCU watchdog */
intermcu_periodic();
/* Safety check and set FBW mode */
fbw_safety_check();
#ifdef BOARD_PX4IO
//due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
//to prevent this situation:
if (intermcu.stable_px4_baud != PPRZ_BAUD) {
fbw_mode = FBW_MODE_FAILSAFE;
fbw_motors_on = false;
//signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
}
#endif
// TODO make module out of led blink?
#ifdef FBW_MODE_LED
static uint16_t dv = 0;
if (fbw_mode == FBW_MODE_FAILSAFE) {
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
LED_ON(FBW_MODE_LED);
}
#endif // FWB_MODE_LED
/* Set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
fbw_motors_on = false;
SetCommands(commands_failsafe);
}
/* If in auto copy autopilot motors on */
if (fbw_mode == FBW_MODE_AUTO) {
fbw_motors_on = autopilot_motors_on;
}
/* Set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
/* Periodic blinking */
RunOnceEvery(10, LED_PERIODIC());
}
void led_update(enum led_status ls)
{
static bool is_pwm = false;
static bool step_sign = false;
static const int pwm_max = 100;
static const int pwm_one_step = 10;
static int pwm_last = 0;
if (!is_pwm && ls == LST_NORMAL) {
LED_ENABLE_PWM_MODE();
is_pwm = true;
}
else if (is_pwm && ls != LST_NORMAL) {
LED_DISABLE_PWM_MODE();
is_pwm = false;
}
if (ls == LST_FAIL) {
LED_ON();
}
else if (ls == LST_NORMAL) {
pwm_last += (step_sign) ? -pwm_one_step : pwm_one_step;
if (0 > pwm_last || pwm_last > pwm_max) {
step_sign = !step_sign;
pwm_last += (step_sign) ? -pwm_one_step : pwm_one_step;
}
LED_PWM(pwm_last);
}
else { /* INIT */
LED_TOGGLE();
}
}
static inline void main_periodic_task( void ) {
LED_TOGGLE(1);
// DOWNLINK_SEND_TAKEOFF(&cpu_time_sec);
usb_serial_transmit( 'A' );
usb_serial_transmit( '\n' );
}
void parse_ins_msg(void)
{
struct link_device *dev = InsLinkDevice;
while (dev->char_available(dev->periph)) {
uint8_t ch = dev->get_byte(dev->periph);
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
if (CHIMU_DATA.m_MsgID == 0x03) {
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
ahrs_chimu.is_aligned = TRUE;
#if CHIMU_DOWNLINK_IMMEDIATE
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi,
&CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
#endif
}
}
}
}
int main()
{
LED_OUTPUT();
usi_init_master();
volatile TWRESULT status = usi_start_master(0x08, 1);
if (status != TWST_OK)
{
while(1)
{
LED_TOGGLE();
_delay_ms(100);
}
}
LED_HIGH();
for (uint8_t i = 0; i < 25; ++i)
{
volatile uint8_t data = usi_read_master(0);
}
usi_read_master(1);
usi_stop();
LED_LOW();
while(1);
return 0;
}
void buttons_leds_timer()
{
static uint8_t led_timer = 0;
if (led_timer > 0)
led_timer--;
switch (led_state)
{
case led_on:
led_timer = 0;
LED_ON();
break;
case led_blink_slow:
case led_blink_fast:
if (led_timer == 0)
{
led_timer = led_state == led_blink_fast ? HM_TIMER_TICKS_FROM_MS(150) : HM_TIMER_TICKS_FROM_MS(300);
LED_TOGGLE();
}
break;
default:
led_timer = 0;
LED_OFF();
}
}
void mpu9250_calibrate_gyro_offset(imu_calibrated_offset_t *imu_offset, uint16_t count)
{
imu_unscaled_data_t mpu9250_cache_unscaled_data;
imu_data_t mpu9250_cache_average_data;
mpu9250_cache_average_data.gyro[0] = 0.0;
mpu9250_cache_average_data.gyro[1] = 0.0;
mpu9250_cache_average_data.gyro[2] = 0.0;
uint16_t i = 0;
for (i = 0; i < count; i++) {
mpu9250_read_accel_temp_gyro(&mpu9250_cache_unscaled_data);
mpu9250_cache_average_data.gyro[0] += ((float)mpu9250_cache_unscaled_data.gyro[0]) / (float)count;
mpu9250_cache_average_data.gyro[1] += ((float)mpu9250_cache_unscaled_data.gyro[1]) / (float)count;
mpu9250_cache_average_data.gyro[2] += ((float)mpu9250_cache_unscaled_data.gyro[2]) / (float)count;
mpu9250_delay(100);
LED_TOGGLE(LED2);
}
imu_offset->gyro[0] = (int16_t)mpu9250_cache_average_data.gyro[0];
imu_offset->gyro[1] = (int16_t)mpu9250_cache_average_data.gyro[1];
imu_offset->gyro[2] = (int16_t)mpu9250_cache_average_data.gyro[2];
}
/* Process that reads from the USB port and writes to the UART port */
static void NORETURN usb_serial_process(void)
{
iptr_t type = proc_currentUserData();
KFile *in_fd = (type == USB_TO_SERIAL) ? &usb_port.fd : &ser_port.fd;
KFile *out_fd = (type == USB_TO_SERIAL) ? &ser_port.fd : &usb_port.fd;
while (1)
{
int c;
c = kfile_getc(in_fd);
if (UNLIKELY(c == EOF))
{
kfile_clearerr(in_fd);
continue;
}
kfile_putc(c, out_fd);
/*
* Toggle the STAT LED when some data passes through the
* usb-seral link
*/
LED_TOGGLE();
}
}
static inline void led_toggle(void)
{
#ifdef BOARD_LISA_L
LED_TOGGLE(7);
#endif
}
// FIXME : nb_tick rollover ???
//
// 97 days at 512hz
// 12 hours at 100khz
//
static inline void sys_tick_irq_handler(void) {
/* set match register for next interrupt */
T0MR0 += sys_time.resolution_cpu_ticks - 1;
sys_time.nb_tick++;
sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;
if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) {
sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec;
sys_time.nb_sec++;
#ifdef SYS_TIME_LED
LED_TOGGLE(SYS_TIME_LED);
#endif
}
for (unsigned int i=0; i<SYS_TIME_NB_TIMER; i++) {
if (sys_time.timer[i].in_use &&
sys_time.nb_tick >= sys_time.timer[i].end_time) {
sys_time.timer[i].end_time += sys_time.timer[i].duration;
sys_time.timer[i].elapsed = TRUE;
if (sys_time.timer[i].cb) {
sys_time.timer[i].cb(i);
}
}
}
}
void tmr_test(void)
{
// Create a timer object
tmr_t tmr;
// Initialise PIO
BIT_SET_HI(PORT_LED_O, BIT_LED_O);
BIT_SET_HI(DDR_LED_O, BIT_LED_O);
// Initialise module
pit_init();
// Enable global interrupts
sei();
// Start timer with a 1s timeout
tmr_start(&tmr, TMR_MS_TO_TICKS(1000));
for(;;)
{
// Wait until timer has expired
while(!tmr_has_expired(&tmr))
{
;
}
// Restart timer
tmr_restart(&tmr);
// Toggle LED
LED_TOGGLE();
}
}
/*!
* @brief Main function
*/
int main(void)
{
volatile uint32_t i;
uint32_t sysFreq;
/* Structure for OSC configuration */
osc_config_t oscConfig;
oscConfig.freq = BOARD_XTAL0_CLK_HZ;
oscConfig.capLoad = 0U;
oscConfig.workMode = kOSC_ModeOscLowPower;
oscConfig.oscerConfig.enableMode = kOSC_ErClkEnable;
BOARD_InitPins();
CLOCK_InitOsc0(&oscConfig);
CLOCK_SetXtal0Freq(BOARD_XTAL0_CLK_HZ);
/* Set clock divider to safe value to switch mode */
CLOCK_SetSimSafeDivs();
#if (defined(FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE) && FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE)
/* Calculate frdiv */
if (!APP_GetAvailableFrdiv())
{
while (1)
{
}
}
#endif /* FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE || FSL_FEATURE_MCG_USE_PLLREFSEL */
/* Configure pll */
if (!APP_GetAvailablePllConfig(&g_pllConfig))
{
while (1)
{
}
}
APP_BootToPeeExample();
/* Change clock PEE -> PBE -> BLPE */
APP_ChangePeeToBlpeExample();
/* Change clock BLPE -> PBE -> PEE */
APP_ChangeBlpeToPeeExample();
/* Get System clock to blink a LED */
sysFreq = CLOCK_GetFreq(kCLOCK_CoreSysClk) / 20U;
/* Enable a LED */
LED_INIT();
/* Blink a LED */
while (1)
{
for (i = 0; i < sysFreq; i++)
{
__NOP();
}
LED_TOGGLE();
}
}
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
}
else if(CHIMU_DATA.m_MsgID==0x02)
{
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
}
}
}
}
static inline void main_periodic_task(void)
{
LED_TOGGLE(1);
UART0PrintString("demo3 running since ");
UART0PrintHex32(sys_time.nb_sec);
UART0PrintString(" seconds\n");
}
void baro_event(void)
{
if (sys_time.nb_sec > 1) {
ms5611_spi_event(&bb_ms5611);
if (bb_ms5611.data_available) {
float pressure = (float)bb_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = false;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
#endif
#if DEBUG
float fbaroms = bb_ms5611.data.pressure / 100.;
DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
&bb_ms5611.data.d1, &bb_ms5611.data.d2,
&fbaroms, &temp);
#endif
}
}
}
static inline void main_periodic_task(void)
{
LED_TOGGLE(1);
DOWNLINK_SEND_TAKEOFF(&motor_power);
wt_baro_periodic();
DOWNLINK_SEND_DEBUG(3, buf_input);
}
void booz_gps_skytraq_read_message(void) {
DEBUG_S1_ON();
if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
booz_gps_state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf);
booz_gps_state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(booz_gps_skytraq.msg_buf);
booz_gps_state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(booz_gps_skytraq.msg_buf);
booz_gps_state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(booz_gps_skytraq.msg_buf);
booz_gps_state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(booz_gps_skytraq.msg_buf);
booz_gps_state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(booz_gps_skytraq.msg_buf);
booz_gps_state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(booz_gps_skytraq.msg_buf);
booz_gps_state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(booz_gps_skytraq.msg_buf);
booz_gps_state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(booz_gps_skytraq.msg_buf);
booz_gps_state.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(booz_gps_skytraq.msg_buf);
// pacc;
// sacc;
// booz_gps_state.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(booz_gps_skytraq.msg_buf);
booz_gps_state.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(booz_gps_skytraq.msg_buf);
booz_gps_state.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(booz_gps_skytraq.msg_buf);
booz_gps_state.tow = SKYTRAQ_NAVIGATION_DATA_TOW(booz_gps_skytraq.msg_buf);
DEBUG_S2_TOGGLE();
#ifdef GPS_LED
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
LED_ON(GPS_LED);
}
else {
LED_TOGGLE(GPS_LED);
}
#endif
}
DEBUG_S1_OFF();
}
void link_mcu_send( void )
{
#ifdef LINK_MCU_LED
LED_TOGGLE(LINK_MCU_LED);
#endif
InterMcuSend_INTERMCU_COMMAND( ap_state->commands );
InterMcuSend_INTERMCU_TRIM( ap_state->command_roll_trim, ap_state->command_pitch_trim );
}
static inline void main_periodic_task(void)
{
RunOnceEvery(100, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
if (sys_time.nb_sec > 1) {
lis302dl_spi_periodic(&lis302);
#if USE_LED_5
RunOnceEvery(10, LED_TOGGLE(5););
static inline void main_periodic_task( void ) {
LED_TOGGLE(1);
//uint16_t time_sec = sys_time.nb_sec;
// DOWNLINK_SEND_TAKEOFF(&time_sec);
usb_serial_transmit( 'A' );
usb_serial_transmit( '\n' );
}
/* When the DSM timer ends */
void on_timer(void) {
// Sen the packet
cyrf_send(packet);
LED_TOGGLE(2);
// Set timeout for next send
timer_dsm_set(10000);
}
static inline void main_periodic_task( void ) {
static uint16_t i = 0;
RunOnceEvery(100, {
LED_TOGGLE(3);
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
});
static void checkPx4RebootCommand(uint8_t b)
{
if (intermcu.stable_px4_baud == CHANGING_BAUD && sys_time_check_and_ack_timer(px4bl_tid)) {
//to prevent a short intermcu comm loss, give some time to changing the baud
sys_time_cancel_timer(px4bl_tid);
intermcu.stable_px4_baud = PPRZ_BAUD;
} else if (intermcu.stable_px4_baud == PX4_BAUD) {
if (sys_time_check_and_ack_timer(px4bl_tid)) {
//time out the possibility to reboot to the px4 bootloader, to prevent unwanted restarts during flight
sys_time_cancel_timer(px4bl_tid);
//for unknown reasons, 1500000 baud does not work reliably after prolonged times.
//I suspect a temperature related issue, combined with the fbw f1 crystal which is out of specs
//After a initial period on 1500000, revert to 230400
//We still start at 1500000 to remain compatible with original PX4 firmware. (which always runs at 1500000)
uart_periph_set_baudrate(intermcu.device->periph, B230400);
intermcu.stable_px4_baud = CHANGING_BAUD;
px4bl_tid = sys_time_register_timer(1.0, NULL);
return;
}
#ifdef SYS_TIME_LED
LED_ON(SYS_TIME_LED);
#endif
if (b == px4RebootSequence[px4RebootSequenceCount]) {
px4RebootSequenceCount++;
} else {
px4RebootSequenceCount = 0;
}
if (px4RebootSequenceCount >= 6) { // 6 = length of rebootSequence + 1
px4RebootSequenceCount = 0; // should not be necessary...
//send some magic back
//this is the same as the Pixhawk IO code would send
intermcu.device->put_byte(intermcu.device->periph, 0, 0x00);
intermcu.device->put_byte(intermcu.device->periph, 0, 0xe5);
intermcu.device->put_byte(intermcu.device->periph, 0, 0x32);
intermcu.device->put_byte(intermcu.device->periph, 0, 0x0a);
intermcu.device->put_byte(intermcu.device->periph, 0,
0x66); // dummy byte, seems to be necessary otherwise one byte is missing at the fmu side...
while (((struct uart_periph *)(intermcu.device->periph))->tx_running) {
// tx_running is volatile now, so LED_TOGGLE not necessary anymore
#ifdef SYS_TIME_LED
LED_TOGGLE(SYS_TIME_LED);
#endif
}
#ifdef SYS_TIME_LED
LED_OFF(SYS_TIME_LED);
#endif
scb_reset_system();
}
}
}
void led_update(enum led_status ls)
{
if (ls == LST_FAIL) {
LED_FAIL_TOGGLE();
LED_OFF();
LED_NORMAL_OFF();
}
else if (ls == LST_NORMAL) {
LED_TOGGLE();
LED_NORMAL_ON();
LED_FAIL_OFF();
}
else { /* INIT */
LED_TOGGLE();
LED_NORMAL_OFF();
LED_FAIL_OFF();
}
}
void event_i2c_abuse_test(void)
{
if (i2c_idle(&I2C_ABUSE_PORT))
{
LED_ON(5); // green = idle
LED_OFF(4);
}
else
{
LED_ON(4); // red = busy
LED_OFF(5);
}
// Wait for I2C transaction object to be released by the I2C driver before changing anything
if ((i2c_abuse_test_counter < 12) && (i2c_abuse_test_counter > 3))
{
if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess))
{
//i2c_test2.slave_addr = 0x90;
i2c_test2.type = I2CTransRx;
i2c_test2.slave_addr = 0x92;
i2c_test2.len_r = 2;
i2c_submit(&I2C_ABUSE_PORT,&i2c_test2);
}
}
if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
{
if (i2c_abuse_test_counter < 16)
{
i2c_abuse_test_counter++;
}
else
{
// wait until ready:
if (i2c_idle(&I2C_ABUSE_PORT))
{
i2c_abuse_test_counter = 1;
i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
i2c_abuse_test_bitrate += 17000;
if (i2c_abuse_test_bitrate > 410000)
{
i2c_abuse_test_bitrate -= 410000;
}
}
}
if (i2c_abuse_test_counter < 16)
{
RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED));
i2c_abuse_send_transaction( i2c_abuse_test_counter );
}
}
}
void dl_parse_msg( void ) {
uint8_t msg_id = IdOfMsg(dl_buffer);
if (msg_id == DL_SET_ACTUATOR) {
uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
LED_TOGGLE(2);
if (servo_no < ACTUATORS_NB)
//SetServo(servo_no, servo_value);
}
void bat_checker_periodic(void) {
if (electrical.bat_critical)
LED_ON(BAT_CHECKER_LED);
else if (electrical.bat_low)
LED_TOGGLE(BAT_CHECKER_LED);
else
LED_OFF(BAT_CHECKER_LED);
}
void humid_sht_periodic(void) {
uint8_t error=0, checksum;
if (humid_sht_status == SHT_IDLE) {
/* init humidity read */
s_connectionreset();
s_start_measure(HUMI);
humid_sht_status = SHT_MEASURING_HUMID;
}
else if (humid_sht_status == SHT_MEASURING_HUMID) {
/* get data */
error += s_read_measure(&humidsht, &checksum);
if (error != 0) {
s_connectionreset();
s_start_measure(HUMI); //restart
LED_TOGGLE(2);
}
else {
error += s_start_measure(TEMP);
humid_sht_status = SHT_MEASURING_TEMP;
}
}
else if (humid_sht_status == SHT_MEASURING_TEMP) {
/* get data */
error += s_read_measure(&tempsht, &checksum);
if (error != 0) {
s_connectionreset();
s_start_measure(TEMP); //restart
LED_TOGGLE(2);
}
else {
calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht);
humid_sht_available = TRUE;
s_connectionreset();
s_start_measure(HUMI);
humid_sht_status = SHT_MEASURING_HUMID;
DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht);
humid_sht_available = FALSE;
}
}
}
