int main(void) {
// ==== Init clock system ====
uint8_t ClkResult = Clk.SwitchToHSE();
if(ClkResult == 0) Clk.HSIDisable();
Clk.SetupBusDividers(ahbDiv1, apbDiv1, apbDiv1);
Clk.UpdateFreqValues();
// ==== Init OS ====
halInit();
chSysInit();
// ==== Init Hard & Soft ====
Init();
if(ClkResult != 0) Uart.Printf("Clk Failure\r");
ir.RegisterEvtTxEnd(&EvtLstnrIrTxEnd, EVENT_MASK(0));
while(TRUE) {
uint16_t w = ID;
w <<= 8;
w |= 0x04;
//Uart.Printf("%X\r", w);
ir.TransmitWord(w
#if IR_DAC
, dc1000mA
#endif
#if IR_PWM
, 100
#endif
);
chEvtWaitAny(EVENT_MASK(0));
// Transmission completed
//GoSleep();
Uart.Printf("TxE\r");
chThdSleepMilliseconds(999);
}
}
bool Node::remoteSubscribe(RemotePublisher * pub, LocalSubscriber * sub) {
LocalSubscriber * p = _subscribers;
eventid_t eid = 0;
if (p == NULL) {
_subscribers = sub;
sub->subscribe(pub, EVENT_MASK(eid));
return true;
}
// FIXME giusto farlo col sizeof()?
while ((++eid < sizeof(eventid_t) * 4) && (p->next() != NULL)) {
// FIXME ?
p = p->next();
}
if (eid == 32) {
return false;
}
p->next(sub);
sub->subscribe(pub, EVENT_MASK(eid));
return true;
}
static THD_FUNCTION(thSerEcho, arg)
{
(void)arg;
chRegSetThreadName("SerEcho");
event_listener_t elSerData;
eventflags_t flags;
chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (!chThdShouldTerminateX())
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(&SD1, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argc;
(void)argv;
static const SerialConfig sc = {
9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0};
sdStart(&SD1, &sc);
EventListener elSerData;
flagsmask_t flags;
chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (TRUE)
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(chp, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
static msg_t PollIMUThread(void *arg){
(void)arg;
chRegSetThreadName("PollIMU");
struct EventListener self_el;
chEvtRegister(&imu_event, &self_el, 2);
while (TRUE) {
chEvtWaitAll(EVENT_MASK(0) && EVENT_MASK(4));
mpu_i2c_read_data(0x3B, 14); // Read accelerometer, temperature and gyro data
chEvtBroadcastFlags(&imu_event, EVENT_MASK(2));
}
return 0;
}
void Node::spin(systime_t timeout) {
eventmask_t mask;
eventid_t eid;
LocalSubscriber * sub;
mask = chEvtWaitAnyTimeout(ALL_EVENTS, timeout);
if (mask == 0) {
return;
}
eid = 0;
sub = _subscribers;
while (sub && mask) {
if (mask & EVENT_MASK(eid)) {
mask &= ~EVENT_MASK(eid);
if (sub->callback()) {
BaseMessage * msg;
while ((msg = sub->get()) != NULL) {
sub->callback(msg);
sub->release(msg);
}
}
}
eid++;
sub = sub->next();
}
}
static msg_t tUsbRx(void *arg) {
(void)arg;
chRegSetThreadName("usbRx");
enum {UsbRxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};
usbPacket *usbBufp;
EventListener elUsbRxComplete;
EventListener elUsbReset;
EventListener elUsbConfigured;
eventmask_t activeEvents;
chEvtRegister(&esUsbRxComplete, &elUsbRxComplete, UsbRxComleteID);
chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);
// Wait for the USB system to be configured.
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
while (TRUE) {
// Allocate buffer space for reception of package in the sysctrl mempool
usbBufp = usbAllocMailboxBuffer();
// Prepare receive operation and initiate the usb system to listen
usbPrepareReceive(usbp, EP_OUT, usbBufp->packet, 64);
chSysLock();
usbStartReceiveI(usbp, EP_OUT);
chSysUnlock();
// Wait for events from the USB system
activeEvents = chEvtWaitAny(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
if (activeEvents == EVENT_MASK(UsbResetID)) {
// If the system was reset, clean up and wait for new configure.
usbFreeMailboxBuffer (usbBufp);
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
}
else {
// Post pagckage to sysctrl if receive was successful
usbBufp->size = ep2outstate.rxcnt;
chMBPost (&usbRXMailbox, (msg_t)usbBufp, TIME_INFINITE);
}
}
return 0;
}
/* прием и обработка комманд с земли*/
void process_cmd(mavlink_command_long_t *mavlink_command_long_struct){
/* all this flags defined in MAV_CMD enum */
switch(mavlink_command_long_struct->command){
case MAV_CMD_DO_SET_MODE:
/* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */
mavlink_system_struct.mode = mavlink_command_long_struct->param1;
command_accepted();
break;
/*
* (пере)запуск калибровки
*/
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
command_denied();
return;
}
else{
handle_calibration_cmd(mavlink_command_long_struct);
}
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
/* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT){
command_denied();
return;
}
else{
chEvtBroadcastFlags(&init_event, EVENT_MASK(SIGHALT_EVID));
command_accepted();
}
break;
/**
* Команды для загрузки/вычитки параметров из EEPROM
*/
case MAV_CMD_PREFLIGHT_STORAGE:
if (mavlink_system_struct.mode != MAV_MODE_PREFLIGHT)
return;
if (mavlink_command_long_struct->param1 == 0)
load_params_from_eeprom();
else if (mavlink_command_long_struct->param1 == 1)
save_params_to_eeprom();
if (mavlink_command_long_struct->param2 == 0)
load_mission_from_eeprom();
else if (mavlink_command_long_struct->param2 == 1)
save_mission_to_eeprom();
break;
default:
return;
break;
}
}
void mpu6050_interrupt_handler(EXTDriver *extp, expchannel_t channel) {
(void)extp;
(void)channel;
chSysLockFromIsr();
chEvtBroadcastFlagsI(&imu_event, EVENT_MASK(0));
chSysUnlockFromIsr();
}
static THD_FUNCTION( esp8266, arg )
{
(void)arg;
chRegSetThreadName( "esp8266" );
event_listener_t serialListener;
/* Registering on the serial driver 6 as event 1, interested in
* error flags and data-available only, other flags will not wakeup
* the thread.
*/
chEvtRegisterMaskWithFlags(
(struct event_source_t *)chnGetEventSource( &SD6 ),
&serialListener,
EVENT_MASK( 1 ),
SD_FRAMING_ERROR | SD_PARITY_ERROR |
CHN_INPUT_AVAILABLE );
while( true )
{
// Waiting for any of the events we're registered on.
eventmask_t evt = chEvtWaitAny( ALL_EVENTS );
// Serving events.
if( evt & EVENT_MASK(1) )
{
/* Event from the serial interface, getting serial
* flags as first thing.
*/
eventflags_t flags = chEvtGetAndClearFlags( &serialListener );
//Handling errors first.
if( flags & (SD_FRAMING_ERROR | SD_PARITY_ERROR) )
{
DPRINT( 4, KRED "FRAMING/PARITY ERROR" );
}
if( flags & CHN_INPUT_AVAILABLE )
{
char c;
c = sdGet( &SD6 );
sdPut( &SD3, c );
}
}
}
}
static THD_FUNCTION(uart_thread, arg)
{
(void)arg;
chRegSetThreadName("UART thread");
// KPA.
event_listener_t kpa_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD1), &kpa_event_listener, EVENT_MASK(1), CHN_INPUT_AVAILABLE);
struct writer kpa_writer;
writer_init(&kpa_writer, &SD1, uart_write);
// FBV.
event_listener_t fbv_event_listener;
chEvtRegisterMaskWithFlags((event_source_t*)chnGetEventSource(&SD6), &fbv_event_listener, EVENT_MASK(2), CHN_INPUT_AVAILABLE);
struct writer fbv_writer;
writer_init(&fbv_writer, &SD6, uart_write);
// Bridge-
struct bridge bridge;
bridge_init(&bridge, &kpa_writer, &fbv_writer);
uint8_t byte = 0;
while (true)
{
eventflags_t evt = chEvtWaitAnyTimeout(EVENT_MASK(1) | EVENT_MASK(2), MS2ST(1));
if (evt & EVENT_MASK(1))
{
chEvtGetAndClearFlags(&kpa_event_listener);
while (readByte(&SD1, &byte))
bridge_update_kpa(&bridge, byte);
}
if (evt & EVENT_MASK(2))
{
chEvtGetAndClearFlags(&fbv_event_listener);
while (readByte(&SD6, &byte))
bridge_update_fbv(&bridge, byte);
}
bridge_update_time(&bridge, clock_get_ms());
}
}
static THD_FUNCTION(serialThread, arg) {
(void)arg;
event_listener_t new_data_listener;
event_listener_t sd1_listener;
event_listener_t sd2_listener;
chEvtRegister(&new_data_event, &new_data_listener, 0);
eventflags_t events = CHN_INPUT_AVAILABLE
| SD_PARITY_ERROR | SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR | SD_BREAK_DETECTED;
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD1),
&sd1_listener,
EVENT_MASK(1),
events);
chEvtRegisterMaskWithFlags(chnGetEventSource(&SD2),
&sd2_listener,
EVENT_MASK(2),
events);
bool need_wait = false;
while(true) {
eventflags_t flags1 = 0;
eventflags_t flags2 = 0;
if (need_wait) {
eventmask_t mask = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
if (mask & EVENT_MASK(1)) {
flags1 = chEvtGetAndClearFlags(&sd1_listener);
print_error("DOWNLINK", flags1, &SD1);
}
if (mask & EVENT_MASK(2)) {
flags2 = chEvtGetAndClearFlags(&sd2_listener);
print_error("UPLINK", flags2, &SD2);
}
}
// Always stay as master, even if the USB goes into sleep mode
is_master |= usbGetDriverStateI(&USBD1) == USB_ACTIVE;
router_set_master(is_master);
need_wait = true;
need_wait &= read_from_serial(&SD2, UP_LINK) == 0;
need_wait &= read_from_serial(&SD1, DOWN_LINK) == 0;
update_transport();
}
}
void GKI_destroy_task(UINT8 task_id)
{
#if ( FALSE == GKI_PTHREAD_JOINABLE )
int i = 0;
#else
int result;
#endif
if (gki_cb.com.OSRdyTbl[task_id] != TASK_DEAD)
{
gki_cb.com.OSRdyTbl[task_id] = TASK_DEAD;
/* paranoi settings, make sure that we do not execute any mailbox events */
gki_cb.com.OSWaitEvt[task_id] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);
#if (GKI_NUM_TIMERS > 0)
gki_cb.com.OSTaskTmr0R[task_id] = 0;
gki_cb.com.OSTaskTmr0 [task_id] = 0;
#endif
#if (GKI_NUM_TIMERS > 1)
gki_cb.com.OSTaskTmr1R[task_id] = 0;
gki_cb.com.OSTaskTmr1 [task_id] = 0;
#endif
#if (GKI_NUM_TIMERS > 2)
gki_cb.com.OSTaskTmr2R[task_id] = 0;
gki_cb.com.OSTaskTmr2 [task_id] = 0;
#endif
#if (GKI_NUM_TIMERS > 3)
gki_cb.com.OSTaskTmr3R[task_id] = 0;
gki_cb.com.OSTaskTmr3 [task_id] = 0;
#endif
GKI_send_event(task_id, EVENT_MASK(GKI_SHUTDOWN_EVT));
#if ( FALSE == GKI_PTHREAD_JOINABLE )
i = 0;
while ((gki_cb.com.OSWaitEvt[task_id] != 0) && (++i < 10))
usleep(100 * 1000);
#else
result = pthread_join( gki_cb.os.thread_id[task_id], NULL );
if ( result < 0 )
{
GKI_ERROR_LOG( "pthread_join() FAILED: result: %d", result );
}
#endif
GKI_exit_task(task_id);
GKI_INFO( "GKI_shutdown(): task [%s] terminated\n", gki_cb.com.OSTName[task_id]);
}
}
/*******************************************************************************
**
** Function GKI_isend_msg
**
** Description Called from interrupt context to send a buffer to a task
**
** Returns Nothing
**
*******************************************************************************/
void GKI_isend_msg (UINT8 task_id, UINT8 mbox, void *msg)
{
BUFFER_HDR_T *p_hdr;
tGKI_COM_CB *p_cb = &gki_cb.com;
/* If task non-existant or not started, drop buffer */
if ((task_id >= GKI_MAX_TASKS) || (mbox >= NUM_TASK_MBOX) || (p_cb->OSRdyTbl[task_id] == TASK_DEAD))
{
GKI_exception(GKI_ERROR_SEND_MSG_BAD_DEST, "Sending to unknown dest");
GKI_freebuf (msg);
return;
}
#if (GKI_ENABLE_BUF_CORRUPTION_CHECK == TRUE)
if (gki_chk_buf_damage(msg))
{
GKI_exception(GKI_ERROR_BUF_CORRUPTED, "Send - Buffer corrupted");
return;
}
#endif
#if (GKI_ENABLE_OWNER_CHECK == TRUE)
if (gki_chk_buf_owner(msg))
{
GKI_exception(GKI_ERROR_NOT_BUF_OWNER, "Send by non-owner");
return;
}
#endif
p_hdr = (BUFFER_HDR_T *) ((UINT8 *) msg - BUFFER_HDR_SIZE);
if (p_hdr->status != BUF_STATUS_UNLINKED)
{
GKI_exception(GKI_ERROR_SEND_MSG_BUF_LINKED, "Send - buffer linked");
return;
}
if (p_cb->OSTaskQFirst[task_id][mbox])
p_cb->OSTaskQLast[task_id][mbox]->p_next = p_hdr;
else
p_cb->OSTaskQFirst[task_id][mbox] = p_hdr;
p_cb->OSTaskQLast[task_id][mbox] = p_hdr;
p_hdr->p_next = NULL;
p_hdr->status = BUF_STATUS_QUEUED;
p_hdr->task_id = task_id;
GKI_isend_event(task_id, (UINT16)EVENT_MASK(mbox));
return;
}
/*! \brief ADIS DIO1 thread
*
* For burst mode transactions t_readrate is 1uS
*
*/
static msg_t Thread_adis_dio1(void *arg) {
(void)arg;
static const evhandler_t evhndl_dio1[] = {
adis_burst_read_handler,
//adis_read_id_handler,
adis_spi_cb_txdone_handler,
adis_release_bus
};
struct EventListener evl_dio;
struct EventListener evl_spi_ev;
struct EventListener evl_spi_release;
chRegSetThreadName("adis_dio");
chEvtRegister(&adis_dio1_event, &evl_dio, 0);
chEvtRegister(&adis_spi_cb_txdone_event, &evl_spi_ev, 1);
chEvtRegister(&adis_spi_cb_releasebus, &evl_spi_release, 2);
while (TRUE) {
chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50)));
}
return -1;
}
static msg_t batterySurveyThd(void *arg)
{
(void)arg;
chRegSetThreadName ("battery survey");
chEvtRegister(&powerOutageSource, &powerOutageListener, 1);
chThdSleepMilliseconds (2000);
register_adc_watchdog((uint32_t) ADC1, 4,
V_ALERT, 0xfff, &powerOutageIsr);
chEvtWaitOne(EVENT_MASK(1));
chibios_logFinish ();
chThdExit(0);
return 0;
}
/**
* @brief Invokes the event handlers associated to an event flags mask.
*
* @param[in] events mask of events to be dispatched
* @param[in] handlers an array of @p evhandler_t. The array must have size
* equal to the number of bits in eventmask_t.
*
* @api
*/
void chEvtDispatch(const evhandler_t *handlers, eventmask_t events) {
eventid_t eid;
chDbgCheck(handlers != NULL);
eid = 0;
while (events) {
if (events & EVENT_MASK(eid)) {
chDbgAssert(handlers[eid] != NULL, "null handler");
events &= ~EVENT_MASK(eid);
handlers[eid](eid);
}
eid++;
}
}
static msg_t Thread_mpu9150_int(void* arg) {
(void) arg;
static const evhandler_t evhndl_mpu9150[] = {
mpu9150_int_event_handler
};
struct EventListener evl_mpu9150;
chRegSetThreadName("mpu9150_int");
chEvtRegister(&mpu9150_int_event, &evl_mpu9150, 0);
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
return -1;
}
static msg_t PollAccelThread(void *semp){
chRegSetThreadName("PollAccel");
msg_t sem_status = RDY_OK;
struct EventListener self_el;
chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);
while (TRUE) {
sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
txbuf[0] = ACCEL_STATUS;
if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);
/* there is no need of correcting of placement. Just get milli g */
mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;
comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;
/* fill scaled debug struct */
mavlink_scaled_imu_struct.xacc = comp_data.xacc;
mavlink_scaled_imu_struct.yacc = comp_data.yacc;
mavlink_scaled_imu_struct.zacc = comp_data.zacc;
}
else{
raw_data.xacc = -32768;
raw_data.yacc = -32768;
raw_data.zacc = -32768;
mavlink_raw_imu_struct.xacc = -32768;
mavlink_raw_imu_struct.yacc = -32768;
mavlink_raw_imu_struct.zacc = -32768;
mavlink_scaled_imu_struct.xacc = -32768;
mavlink_scaled_imu_struct.yacc = -32768;
mavlink_scaled_imu_struct.zacc = -32768;
}
if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
chThdExit(RDY_OK);
}
return 0;
}
msg_t data_udp_send_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
static const evhandler_t evhndl_mpu9150[] = {
data_udp_send_mpu9150_data
};
struct EventListener evl_mpu9150;
err_t err;
ip_addr_t ip_addr_sensor;
ip_addr_t ip_addr_fc;
chRegSetThreadName("data_udp_send_thread");
chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0);
IMU_A_IP_ADDR(&ip_addr_sensor);
IP_PSAS_FC(&ip_addr_fc);
mpu9150_mac_info.conn = netconn_new( NETCONN_UDP );
/* Bind to the local address, or to ANY address */
// netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY)
err = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port
if (err == ERR_OK) {
/* Connect to specific address or a broadcast address */
/*
* \todo Understand why a UDP needs a connect...
* This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally.
*
*/
// netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT );
err = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A );
if(err == ERR_OK) {
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
}
return RDY_RESET;
}
return RDY_RESET;
}
msg_t WebThread(void *p) {
EvTimer evt1, evt2;
EventListener el0, el1, el2;
uip_ipaddr_t ipaddr;
(void)p;
/*
* Event sources setup.
*/
chEvtRegister(macGetReceiveEventSource(ÐD1), &el0, FRAME_RECEIVED_ID);
chEvtAddFlags(EVENT_MASK(FRAME_RECEIVED_ID)); /* In case some frames are already buffered */
evtInit(&evt1, MS2ST(500));
evtStart(&evt1);
chEvtRegister(&evt1.et_es, &el1, PERIODIC_TIMER_ID);
evtInit(&evt2, S2ST(10));
evtStart(&evt2);
chEvtRegister(&evt2.et_es, &el2, ARP_TIMER_ID);
/*
* EMAC driver start.
*/
macStart(ÐD1, &mac_config);
(void)macPollLinkStatus(ÐD1);
/*
* uIP initialization.
*/
uip_init();
uip_setethaddr(macaddr);
uip_ipaddr(ipaddr, IPADDR0, IPADDR1, IPADDR2, IPADDR3);
uip_sethostaddr(ipaddr);
httpd_init();
while (TRUE) {
chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
}
return 0;
}
void AP_IOMCU_FW::update()
{
eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(1));
if (do_reboot && (AP_HAL::millis() > reboot_time)) {
hal.scheduler->reboot(true);
while (true) {}
}
if ((mask & EVENT_MASK(IOEVENT_PWM)) ||
(last_safety_off != reg_status.flag_safety_off)) {
last_safety_off = reg_status.flag_safety_off;
pwm_out_update();
}
uint32_t now = AP_HAL::millis();
// output SBUS if enabled
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
reg_status.flag_safety_off &&
now - sbus_last_ms >= sbus_interval_ms) {
// output a new SBUS frame
sbus_last_ms = now;
sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS);
}
// run remaining functions at 1kHz
if (now != last_loop_ms) {
last_loop_ms = now;
heater_update();
rcin_update();
safety_update();
rcout_mode_update();
hal.rcout->timer_tick();
}
}
/*
* Application entry point.
*/
int main(void) {
thread_t *shelltp1 = NULL;
thread_t *shelltp2 = NULL;
event_listener_t shell_el;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Initializes two serial-over-USB CDC drivers.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg1);
sduObjectInit(&SDU2);
sduStart(&SDU2, &serusbcfg2);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg1.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg1.usbp, &usbcfg);
usbConnectBus(serusbcfg1.usbp);
/*
* Shell manager initialization.
* Event zero is shell exit.
*/
shellInit();
chEvtRegister(&shell_terminated, &shell_el, 0);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, managing two shells.
*/
while (true) {
if (SDU1.config->usbp->state == USB_ACTIVE) {
/* Starting shells.*/
if (shelltp1 == NULL) {
shelltp1 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell1", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg1);
}
if (shelltp2 == NULL) {
shelltp2 = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell2", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg2);
}
/* Waiting for an exit event then freeing terminated shells.*/
chEvtWaitAny(EVENT_MASK(0));
if (chThdTerminatedX(shelltp1)) {
chThdFreeToHeap(shelltp1);
shelltp1 = NULL;
}
if (chThdTerminatedX(shelltp2)) {
chThdFreeToHeap(shelltp2);
shelltp2 = NULL;
}
}
else {
chThdSleepMilliseconds(1000);
}
}
}
/**
* @brief Main estimation thread.
*
* @param[in/out] arg Unused.
*/
static THD_FUNCTION(ThreadEstimation, arg)
{
(void)arg;
chRegSetThreadName("Estimation");
tp = chThdGetSelfX();
/* Initialization data */
//uint32_t i;
//quaternion_t q_init = {1.0f, 0.0f, 0.0f, 0.0f};
//vector3f_t am, wb_init = {0.0f, 0.0f, 0.0f}; //, acc_init, mag_init;
/* Event registration for new sensor data */
event_listener_t el;
/* Register to new data from accelerometer and gyroscope */
chEvtRegisterMaskWithFlags(ptrGetNewDataEventSource(),
&el,
EVENT_MASK(0),
ACCGYRO_DATA_AVAILABLE_EVENTMASK |
MAG_DATA_AVAILABLE_EVENTMASK |
BARO_DATA_AVAILABLE_EVENTMASK);
/* Force an initialization of the estimation */
//chEvtAddEvents(ESTIMATION_RESET_EVENTMASK);
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
vInitializeMotionCaptureEstimator(&states);
while(1)
{
//if (isnan(states.q.q0) || isnan(states.q.q1) || isnan(states.q.q2) || isnan(states.q.q3) ||
// isnan(states.w.x) || isnan(states.w.y) || isnan(states.w.z) ||
// isnan(states.wb.x) || isnan(states.wb.y) || isnan(states.wb.z))
// AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
/* Check if there has been a request to reset the filter */
//if (chEvtWaitOneTimeout(ESTIMATION_RESET_EVENTMASK, TIME_IMMEDIATE))
// {
/* Initialize the estimation */
//AttitudeEstimationInit(&states, &data, &q_init, &wb_init);
//}
/* Wait for new measurement data */
chEvtWaitAny(ALL_EVENTS);
eventflags_t flags = chEvtGetAndClearFlags(&el);
if (flags & ACCGYRO_DATA_AVAILABLE_EVENTMASK)
{
/* Get sensor data */
GetIMUData(&imu_data);
/* Run estimation */
vInnovateMotionCaptureEstimator(&states,
&imu_data,
SENSOR_ACCGYRO_DT,
0.0007f);
/*InnovateAttitudeEKF(&states,
&data,
imu_data.gyroscope,
imu_data.accelerometer,
imu_data.magnetometer,
0.0f,
0.0f,
ESTIMATION_DT);*/
//states.w.x = -imu_data.gyroscope[0];
//states.w.y = -imu_data.gyroscope[1];
//states.w.z = imu_data.gyroscope[2];
//am.x = -imu_data.accelerometer[0];
//am.y = -imu_data.accelerometer[1];
//am.z = imu_data.accelerometer[2];
//states.q = MadgwickAHRSupdateIMU(states.w,
// am,
// states.q,
// 0.15f,
// dt);
/* Broadcast new estimation available */
chEvtBroadcastFlags(&estimation_events_es,
ESTIMATION_NEW_ESTIMATION_EVENTMASK);
}
}
}
#include "ui_widget.hpp"
#include "ui_painter.hpp"
#include "portapack.hpp"
#include "portapack_shared_memory.hpp"
#include "message.hpp"
#include "touch.hpp"
#include "ch.h"
#include <cstdint>
constexpr auto EVT_MASK_RTC_TICK = EVENT_MASK(0);
constexpr auto EVT_MASK_LCD_FRAME_SYNC = EVENT_MASK(1);
constexpr auto EVT_MASK_SWITCHES = EVENT_MASK(3);
constexpr auto EVT_MASK_ENCODER = EVENT_MASK(4);
constexpr auto EVT_MASK_TOUCH = EVENT_MASK(5);
constexpr auto EVT_MASK_APPLICATION = EVENT_MASK(6);
constexpr auto EVT_MASK_CAPTURE_THREAD = EVENT_MASK(7);
class EventDispatcher {
public:
EventDispatcher(
ui::Widget* const top_widget,
ui::Context& context
);
void run();
void GKI_shutdown(void)
{
UINT8 task_id;
volatile int *p_run_cond = &gki_cb.os.no_timer_suspend;
int oldCOnd = 0;
#if ( FALSE == GKI_PTHREAD_JOINABLE )
int i = 0;
#else
int result;
#endif
/* release threads and set as TASK_DEAD. going from low to high priority fixes
* GKI_exception problem due to btu->hci sleep request events */
for (task_id = GKI_MAX_TASKS; task_id > 0; task_id--)
{
if (gki_cb.com.OSRdyTbl[task_id - 1] != TASK_DEAD)
{
gki_cb.com.OSRdyTbl[task_id - 1] = TASK_DEAD;
/* paranoi settings, make sure that we do not execute any mailbox events */
gki_cb.com.OSWaitEvt[task_id-1] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);
GKI_send_event(task_id - 1, EVENT_MASK(GKI_SHUTDOWN_EVT));
#if ( FALSE == GKI_PTHREAD_JOINABLE )
i = 0;
while ((gki_cb.com.OSWaitEvt[task_id - 1] != 0) && (++i < 10))
usleep(100 * 1000);
#else
/* wait for proper Arnold Schwarzenegger task state */
result = pthread_join( gki_cb.os.thread_id[task_id-1], NULL );
if ( result < 0 )
{
GKI_TRACE_1( "pthread_join() FAILED: result: %d", result );
}
#endif
GKI_TRACE_1( "GKI_shutdown(): task %s dead", gki_cb.com.OSTName[task_id]);
GKI_exit_task(task_id - 1);
}
}
/* Destroy mutex and condition variable objects */
pthread_mutex_destroy(&gki_cb.os.GKI_mutex);
/* pthread_mutex_destroy(&GKI_sched_mutex); */
#if (GKI_DEBUG == TRUE)
pthread_mutex_destroy(&gki_cb.os.GKI_trace_mutex);
#endif
/* pthread_mutex_destroy(&thread_delay_mutex);
pthread_cond_destroy (&thread_delay_cond); */
#if ( FALSE == GKI_PTHREAD_JOINABLE )
i = 0;
#endif
#ifdef NO_GKI_RUN_RETURN
shutdown_timer = 1;
#endif
if (gki_cb.os.gki_timer_wake_lock_on)
{
GKI_TRACE_0("GKI_shutdown : release_wake_lock(brcm_btld)");
release_wake_lock(WAKE_LOCK_ID);
gki_cb.os.gki_timer_wake_lock_on = 0;
}
oldCOnd = *p_run_cond;
*p_run_cond = GKI_TIMER_TICK_EXIT_COND;
if (oldCOnd == GKI_TIMER_TICK_STOP_COND)
pthread_cond_signal( &gki_cb.os.gki_timer_cond );
}
void GKI_shutdown(void)
{
UINT8 task_id;
#if ( FALSE == GKI_PTHREAD_JOINABLE )
int i = 0;
#else
int result;
#endif
#ifdef GKI_USE_DEFERED_ALLOC_BUF_POOLS
gki_dealloc_free_queue();
#endif
/* release threads and set as TASK_DEAD. going from low to high priority fixes
* GKI_exception problem due to btu->hci sleep request events */
for (task_id = GKI_MAX_TASKS; task_id > 0; task_id--)
{
if (gki_cb.com.OSRdyTbl[task_id - 1] != TASK_DEAD)
{
gki_cb.com.OSRdyTbl[task_id - 1] = TASK_DEAD;
/* paranoi settings, make sure that we do not execute any mailbox events */
gki_cb.com.OSWaitEvt[task_id-1] &= ~(TASK_MBOX_0_EVT_MASK|TASK_MBOX_1_EVT_MASK|
TASK_MBOX_2_EVT_MASK|TASK_MBOX_3_EVT_MASK);
GKI_send_event(task_id - 1, EVENT_MASK(GKI_SHUTDOWN_EVT));
#if ( FALSE == GKI_PTHREAD_JOINABLE )
i = 0;
while ((gki_cb.com.OSWaitEvt[task_id - 1] != 0) && (++i < 10))
usleep(100 * 1000);
#else
result = pthread_join( gki_cb.os.thread_id[task_id-1], NULL );
if ( result < 0 )
{
ALOGE( "pthread_join() FAILED: result: %d", result );
}
#endif
// GKI_ERROR_LOG( "GKI_shutdown(): task %s dead\n", gki_cb.com.OSTName[task_id]);
GKI_exit_task(task_id - 1);
}
}
/* Destroy mutex and condition variable objects */
pthread_mutex_destroy(&gki_cb.os.GKI_mutex);
/* pthread_mutex_destroy(&GKI_sched_mutex); */
#if (GKI_DEBUG == TRUE)
pthread_mutex_destroy(&gki_cb.os.GKI_trace_mutex);
#endif
/* pthread_mutex_destroy(&thread_delay_mutex);
pthread_cond_destroy (&thread_delay_cond); */
#if ( FALSE == GKI_PTHREAD_JOINABLE )
i = 0;
#endif
#ifdef NO_GKI_RUN_RETURN
shutdown_timer = 1;
#endif
if (g_GkiTimerWakeLockOn)
{
GKI_TRACE("GKI_shutdown : release_wake_lock(brcm_btld)");
release_wake_lock(WAKE_LOCK_ID);
g_GkiTimerWakeLockOn = 0;
}
}
/*******************************************************************************
**
** Function GKI_wait
**
** Description This function is called by tasks to wait for a specific
** event or set of events. The task may specify the duration
** that it wants to wait for, or 0 if infinite.
**
** Parameters: flag - (input) the event or set of events to wait for
** timeout - (input) the duration that the task wants to wait
** for the specific events (in system ticks)
**
**
** Returns the event mask of received events or zero if timeout
**
*******************************************************************************/
UINT16 GKI_wait (UINT16 flag, UINT32 timeout)
{
UINT16 evt;
UINT8 rtask;
struct timespec abstime = { 0, 0 };
int sec;
int nano_sec;
rtask = GKI_get_taskid();
GKI_TRACE_3("GKI_wait %d %x %d", rtask, flag, timeout);
if (rtask >= GKI_MAX_TASKS) {
pthread_exit(NULL);
return 0;
}
gki_pthread_info_t* p_pthread_info = &gki_pthread_info[rtask];
if (p_pthread_info->pCond != NULL && p_pthread_info->pMutex != NULL) {
int ret;
GKI_TRACE_3("GKI_wait task=%i, pCond/pMutex = %x/%x", rtask, p_pthread_info->pCond, p_pthread_info->pMutex);
ret = pthread_mutex_lock(p_pthread_info->pMutex);
ret = pthread_cond_signal(p_pthread_info->pCond);
ret = pthread_mutex_unlock(p_pthread_info->pMutex);
p_pthread_info->pMutex = NULL;
p_pthread_info->pCond = NULL;
}
gki_cb.com.OSWaitForEvt[rtask] = flag;
/* protect OSWaitEvt[rtask] from modification from an other thread */
pthread_mutex_lock(&gki_cb.os.thread_evt_mutex[rtask]);
#if 0 /* for clean scheduling we probably should always call pthread_cond_wait() */
/* Check if anything in any of the mailboxes. There is a potential race condition where OSTaskQFirst[rtask]
has been modified. however this should only result in addtional call to pthread_cond_wait() but as
the cond is met, it will exit immediately (depending on schedulling) */
if (gki_cb.com.OSTaskQFirst[rtask][0])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][1])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][2])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][3])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;
#endif
if (!(gki_cb.com.OSWaitEvt[rtask] & flag))
{
if (timeout)
{
// timeout = GKI_MS_TO_TICKS(timeout); /* convert from milliseconds to ticks */
/* get current system time */
// clock_gettime(CLOCK_MONOTONIC, &currSysTime);
// abstime.tv_sec = currSysTime.time;
// abstime.tv_nsec = NANOSEC_PER_MILLISEC * currSysTime.millitm;
clock_gettime(CLOCK_MONOTONIC, &abstime);
/* add timeout */
sec = timeout / 1000;
nano_sec = (timeout % 1000) * NANOSEC_PER_MILLISEC;
abstime.tv_nsec += nano_sec;
if (abstime.tv_nsec > NSEC_PER_SEC)
{
abstime.tv_sec += (abstime.tv_nsec / NSEC_PER_SEC);
abstime.tv_nsec = abstime.tv_nsec % NSEC_PER_SEC;
}
abstime.tv_sec += sec;
pthread_cond_timedwait_monotonic(&gki_cb.os.thread_evt_cond[rtask],
&gki_cb.os.thread_evt_mutex[rtask], &abstime);
}
else
{
pthread_cond_wait(&gki_cb.os.thread_evt_cond[rtask], &gki_cb.os.thread_evt_mutex[rtask]);
}
/* TODO: check, this is probably neither not needed depending on phtread_cond_wait() implmentation,
e.g. it looks like it is implemented as a counter in which case multiple cond_signal
should NOT be lost! */
// we are waking up after waiting for some events, so refresh variables
// no need to call GKI_disable() here as we know that we will have some events as we've been waking up after condition pending or timeout
if (gki_cb.com.OSTaskQFirst[rtask][0])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][1])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][2])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][3])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;
if (gki_cb.com.OSRdyTbl[rtask] == TASK_DEAD)
{
gki_cb.com.OSWaitEvt[rtask] = 0;
/* unlock thread_evt_mutex as pthread_cond_wait() does auto lock when cond is met */
pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
BT_TRACE_1( TRACE_LAYER_HCI, TRACE_TYPE_DEBUG, "GKI TASK_DEAD received. exit thread %d...", rtask );
gki_cb.os.thread_id[rtask] = 0;
pthread_exit(NULL);
return (EVENT_MASK(GKI_SHUTDOWN_EVT));
}
}
/* Clear the wait for event mask */
gki_cb.com.OSWaitForEvt[rtask] = 0;
/* Return only those bits which user wants... */
evt = gki_cb.com.OSWaitEvt[rtask] & flag;
/* Clear only those bits which user wants... */
gki_cb.com.OSWaitEvt[rtask] &= ~flag;
/* unlock thread_evt_mutex as pthread_cond_wait() does auto lock mutex when cond is met */
pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
GKI_TRACE_4("GKI_wait %d %x %d %x resumed", rtask, flag, timeout, evt);
return (evt);
}
/*******************************************************************************
**
** Function GKI_wait
**
** Description This function is called by tasks to wait for a specific
** event or set of events. The task may specify the duration
** that it wants to wait for, or 0 if infinite.
**
** Parameters: flag - (input) the event or set of events to wait for
** timeout - (input) the duration that the task wants to wait
** for the specific events (in system ticks)
**
**
** Returns the event mask of received events or zero if timeout
**
*******************************************************************************/
UINT16 GKI_wait (UINT16 flag, UINT32 timeout)
{
UINT16 evt;
UINT8 rtask;
struct timespec abstime = { 0, 0 };
int sec;
int nano_sec;
rtask = GKI_get_taskid();
GKI_TRACE("GKI_wait %d %x %d", (int)rtask, (int)flag, (int)timeout);
gki_cb.com.OSWaitForEvt[rtask] = flag;
/* protect OSWaitEvt[rtask] from modification from an other thread */
pthread_mutex_lock(&gki_cb.os.thread_evt_mutex[rtask]);
if (!(gki_cb.com.OSWaitEvt[rtask] & flag))
{
if (timeout)
{
clock_gettime(CLOCK_MONOTONIC, &abstime);
/* add timeout */
sec = timeout / 1000;
nano_sec = (timeout % 1000) * NANOSEC_PER_MILLISEC;
abstime.tv_nsec += nano_sec;
if (abstime.tv_nsec > NSEC_PER_SEC)
{
abstime.tv_sec += (abstime.tv_nsec / NSEC_PER_SEC);
abstime.tv_nsec = abstime.tv_nsec % NSEC_PER_SEC;
}
abstime.tv_sec += sec;
pthread_cond_timedwait_monotonic(&gki_cb.os.thread_evt_cond[rtask],
&gki_cb.os.thread_evt_mutex[rtask], &abstime);
}
else
{
pthread_cond_wait(&gki_cb.os.thread_evt_cond[rtask], &gki_cb.os.thread_evt_mutex[rtask]);
}
/* TODO: check, this is probably neither not needed depending on phtread_cond_wait() implmentation,
e.g. it looks like it is implemented as a counter in which case multiple cond_signal
should NOT be lost! */
/* we are waking up after waiting for some events, so refresh variables
no need to call GKI_disable() here as we know that we will have some events as we've been waking
up after condition pending or timeout */
if (gki_cb.com.OSTaskQFirst[rtask][0])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_0_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][1])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_1_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][2])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_2_EVT_MASK;
if (gki_cb.com.OSTaskQFirst[rtask][3])
gki_cb.com.OSWaitEvt[rtask] |= TASK_MBOX_3_EVT_MASK;
if (gki_cb.com.OSRdyTbl[rtask] == TASK_DEAD)
{
gki_cb.com.OSWaitEvt[rtask] = 0;
/* unlock thread_evt_mutex as pthread_cond_wait() does auto lock when cond is met */
pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
return (EVENT_MASK(GKI_SHUTDOWN_EVT));
}
}
/* Clear the wait for event mask */
gki_cb.com.OSWaitForEvt[rtask] = 0;
/* Return only those bits which user wants... */
evt = gki_cb.com.OSWaitEvt[rtask] & flag;
/* Clear only those bits which user wants... */
gki_cb.com.OSWaitEvt[rtask] &= ~flag;
/* unlock thread_evt_mutex as pthread_cond_wait() does auto lock mutex when cond is met */
pthread_mutex_unlock(&gki_cb.os.thread_evt_mutex[rtask]);
GKI_TRACE("GKI_wait %d %x %d %x done", (int)rtask, (int)flag, (int)timeout, (int)evt);
return (evt);
}
bool AP_IOMCU_FW::handle_code_write()
{
switch (rx_io_packet.page) {
case PAGE_SETUP:
switch (rx_io_packet.offset) {
case PAGE_REG_SETUP_ARMING:
reg_setup.arming = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_off();
reg_status.flag_safety_off = true;
} else {
return false;
}
break;
case PAGE_REG_SETUP_FORCE_SAFETY_ON:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_on();
reg_status.flag_safety_off = false;
} else {
return false;
}
break;
case PAGE_REG_SETUP_ALTRATE:
reg_setup.pwm_altrate = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_PWM_RATE_MASK:
reg_setup.pwm_rates = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_DEFAULTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
update_default_rate = true;
break;
case PAGE_REG_SETUP_SBUS_RATE:
reg_setup.sbus_rate = rx_io_packet.regs[0];
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
break;
case PAGE_REG_SETUP_FEATURES:
reg_setup.features = rx_io_packet.regs[0];
/* disable the conflicting options with SBUS 1 */
if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
P_SETUP_FEATURES_ADC_RSSI |
P_SETUP_FEATURES_SBUS2_OUT);
// enable SBUS output at specified rate
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
} else {
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = AP_HAL::millis();
break;
case PAGE_REG_SETUP_REBOOT_BL:
if (reg_status.flag_safety_off) {
// don't allow reboot while armed
return false;
}
// check the magic value
if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
return false;
}
schedule_reboot(100);
break;
default:
break;
}
break;
case PAGE_DIRECT_PWM: {
/* copy channel data */
uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
}
offset++;
num_values--;
i++;
}
fmu_data_received_time = AP_HAL::millis();
reg_status.flag_fmu_ok = true;
reg_status.flag_raw_pwm = true;
chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
break;
}
default:
break;
}
memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
tx_io_packet.count = 0;
tx_io_packet.code = CODE_SUCCESS;
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}