void Setup()
{
debug_level = 2;
//set clock to max for init 32MHz
ClockSetSource(x32MHz);
//disable 2MHZ osc
OSC.CTRL = 0b00000010;
//save power
turnoff_subsystems();
EnableInterrupts();
//init basic peripherals
led_init();
uart_init_buffers();
uart_init();
time_init();
buzzer_init();
battery_init();
buttons_init();
//basic power control
mems_power_init();
io_init();
SD_EN_INIT;
//load configuration
cfg_load();
_delay_ms(100);
}
bool state_init(state_t *state, state_t* state_config, const analog_monitor_t* analog_monitor)
{
bool init_success = true;
// Init dependencies
state->analog_monitor = analog_monitor;
// Init parameters
state->autopilot_type = state_config->autopilot_type;
state->autopilot_name = state_config->autopilot_name;
state->mav_state = state_config->mav_state;
state->mav_mode = state_config->mav_mode;
state->source_mode = state_config->source_mode;
state->mav_mode_custom = CUSTOM_BASE_MODE;
state->simulation_mode = state_config->simulation_mode;
init_success &= battery_init(&state->battery,state_config->battery.type,state_config->battery.low_level_limit);
if (state->simulation_mode == HIL_ON)
{
// state->mav_mode |= MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_ON;
}
else
{
// state->mav_mode |= !MAV_MODE_FLAG_HIL_ENABLED;
state->mav_mode.HIL = HIL_OFF;
}
state->fence_1_xy = state_config->fence_1_xy;
state->fence_1_z = state_config->fence_1_z;
state->fence_2_xy = state_config->fence_2_xy;
state->fence_2_z = state_config->fence_2_z;
state->out_of_fence_1 = false;
state->out_of_fence_2 = false;
state->nav_plan_active = false;
state->in_the_air = false;
state->reset_position = false;
state->last_heartbeat_msg = time_keeper_get_time();
state->max_lost_connection = state_config->max_lost_connection;
state->connection_lost = false;
state->first_connection_set = false;
state->msg_count = 0;
state->remote_active = state_config->remote_active;
print_util_dbg_print("[STATE] Initialized.\r\n");
return init_success;
}
/**@brief Function for application main entry.
*/
int main(void)
{
#ifdef OSSW_DEBUG
init_uart();
#endif
spi_init();
ext_ram_init();
init_lcd_with_splash_screen();
accel_init();
// Initialize.
timers_init();
rtc_timer_init();
buttons_init();
battery_init();
// Initialize the SoftDevice handler module.
SOFTDEVICE_HANDLER_INIT(NRF_CLOCK_LFCLKSRC_XTAL_20_PPM, NULL);
// splash screen
nrf_delay_ms(500);
fs_init();
config_init();
scr_mngr_init();
vibration_init();
notifications_init();
stopwatch_init();
timer_feature_init();
mlcd_timers_init();
// Enter main loop.
for (;;)
{
if (rtc_should_store_current_time()) {
rtc_store_current_time();
}
app_sched_execute();
stopwatch_process();
command_process();
watchset_process_async_operation();
scr_mngr_draw_screen();
power_manage();
}
}
static void init()
{
// OSCCAL = 71;
clock_prescale_set(CPU_DIV);
// power_twi_disable();
// power_usart0_disable();
// power_timer0_disable();
// power_timer1_disable();
// power_timer2_disable();
// power_adc_disable();
#if !UART_ENABLE
power_usart0_disable();
#endif
// Pull-up on unused pins
pinPullup(D0, PULLUP_ENABLE);
pinPullup(D1, PULLUP_ENABLE);
pinPullup(D3, PULLUP_ENABLE);
pinPullup(D4, PULLUP_ENABLE);
pinPullup(B7, PULLUP_ENABLE);
#if PIN_DEBUG != PIN_DEBUG_NONE
pinMode(PIN_DEBUG_PIN, OUTPUT);
#endif
// Pin change interrupt on USB power sense pin
PCICR |= _BV(PCIE0);
PCMSK0 |= _BV(PCINT6);
// Everything else
uart_init();
spi_init();
i2c_init();
watchconfig_init();
led_init();
buzzer_init();
battery_init();
ds3231_init();
buttons_init();
millis_init();
pwrmgr_init();
time_init();
alarm_init();
oled_init();
}
STATIC_INLINE void main_init( void ) {
#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
/* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
__asm("nop");
}
#endif
hw_init();
sys_time_init();
actuators_init();
radio_control_init();
booz2_analog_init();
baro_init();
#if defined USE_CAM || USE_DROP
booz2_pwm_init_hw();
#endif
battery_init();
imu_init();
// booz_fms_init(); // FIXME
autopilot_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
ins_init();
#ifdef USE_GPS
booz_gps_init();
#endif
modules_init();
int_enable();
}
void init(void)
{
/* Disable the watchdog timer */
WDTCTL = WDTHOLD | WDTPW;
/* GPIO: All inputs */
P1DIR = P2DIR = P3DIR = P4DIR = 0;
/* Use a 16 MHz clock (DCO) */
DCOCTL = CALDCO_16MHZ;
BCSCTL1 &= ~0x0f;
BCSCTL1 |=
/*XT2O=0: XT2 is on*/
/*XTS=0: LFXT1 mode select. 0 -Low frequency mode*/
DIVA_0 /* ACLK Divider 0: /1 */
|CALBC1_16MHZ; /* BCSCTL1 Calibration Data for 16MHz */
BCSCTL2 = SELM_DCOCLK /* MCLK from DCO */
/* DIVMx = 0 : No MCLK divider */
/* SELS = 0: SMCLK from DCO */
/* DIVSx = 0 : No SMCLK divider */
/* DCOR = 0 : DCO internal resistor */;
BCSCTL3 = LFXT1S1; /*VLOCLK */
flash_init();
opamp1_init();
bias_init();
adc10_init(); /* The order here matters. This configures the ADC */
random_init(); /* Grab some random data */
net_rx_init();
net_tx_init();
ir_receive_init();
ir_transmit_init();
motor_init();
leds_init();
battery_init();
food_init();
eint();
/* virus_init(); */
}
// MAIN TASK INITIALIZATIOON
result_t Task_SmartPHTApp_Init(void)
{
result_t result;
adc_init();
rtc_initialize();
rtc_set(&statusBarData.time);
RESULT_CHECK( ioexp_initialize(), result);
RESULT_CHECK( touch_initialize(), result);
battery_init();
battery_enable_usb_charger(bTrue);
battery_enable_ac_charger(bTrue);
install_event_handlers();
gui_set_current_view(&view_dashboard);
// Initialize USB
initCDC(); // setup the CDC state machine
usb_init(cdc_device_descriptor, cdc_config_descriptor, cdc_str_descs, USB_NUM_STRINGS); // initialize USB. TODO: Remove magic with macro
usb_start(); //start the USB peripheral
EnableUsbPerifInterrupts(USB_TRN + USB_SOF + USB_UERR + USB_URST);
EnableUsbGlobalInterrupt(); // Only enables global USB interrupt. Chip interrupts must be enabled by the user (PIC18)
sensor_init();
reset_minmax();
reset_alarms();
// Reset waveforms
wfrm_clear(&wfrmPressure);
wfrm_clear(&wfrmHumidity);
wfrm_clear(&wfrmTemperature);
// Set plot scale
lineplot_set_sample_per(&lineplot, &supportedSamplingPeriods[sliderSampPeriodData.value]);
// Default sensor
checkable_set_checked(&btnSelInterface0, bTrue);
return RV_OK;
}
void Setup()
{
//set clock to max for init 32MHz
ClockSetSource(x32MHz);
//disable 2MHZ osc
OSC.CTRL = 0b00000010;
//get RAM info
free_ram_at_start = freeRam();
//get reset reason
system_rst = RST.STATUS;
RST.STATUS = 0b00111111;
//save power - peripherals are turned on on demand by drivers
turnoff_subsystems();
EnableInterrupts();
//load device id
GetID();
//init basic peripherals
led_init();
uart_init_buffers();
time_init();
buzzer_init();
battery_init();
buttons_init();
//basic power control
mems_power_init();
io_init();
SD_EN_INIT;
//load configuration from EE
cfg_load();
uart_init();
_delay_ms(100);
}
void rn42_task_init(void)
{
battery_init();
sleep_led_init();
}
void rn42_task_init(void)
{
battery_init();
}
//* ************************************************************************************************
/// @fn init_application(void)
/// @brief Init the watch's program
/// @return none
//* ************************************************************************************************
void init_application(void)
{
volatile unsigned char *ptr;
// ---------------------------------------------------------------------
// Enable watchdog
// Watchdog triggers after 16 seconds when not cleared
#ifdef USE_WATCHDOG
WDTCTL = WDTPW + WDTIS__512K + WDTSSEL__ACLK;
#else
WDTCTL = WDTPW + WDTHOLD;
#endif
// ---------------------------------------------------------------------
// Configure PMM
SetVCore(3);
// Set global high power request enable
PMMCTL0_H = 0xA5;
PMMCTL0_L |= PMMHPMRE;
PMMCTL0_H = 0x00;
// ---------------------------------------------------------------------
// Enable 32kHz ACLK
P5SEL |= 0x03; // Select XIN, XOUT on P5.0 and P5.1
UCSCTL6 &= ~XT1OFF; // XT1 On, Highest drive strength
UCSCTL6 |= XCAP_3; // Internal load cap
UCSCTL3 = SELA__XT1CLK; // Select XT1 as FLL reference
UCSCTL4 = SELA__XT1CLK | SELS__DCOCLKDIV | SELM__DCOCLKDIV;
// ---------------------------------------------------------------------
// Configure CPU clock for 12MHz
_BIS_SR(SCG0); // Disable the FLL control loop
UCSCTL0 = 0x0000; // Set lowest possible DCOx, MODx
UCSCTL1 = DCORSEL_5; // Select suitable range
UCSCTL2 = FLLD_1 + 0x16E; // Set DCO Multiplier
_BIC_SR(SCG0); // Enable the FLL control loop
// Worst-case settling time for the DCO when the DCO range bits have been
// changed is n x 32 x 32 x f_MCLK / f_FLL_reference. See UCS chapter in 5xx
// UG for optimization.
// 32 x 32 x 8 MHz / 32,768 Hz = 250000 = MCLK cycles for DCO to settle
#if __GNUC_MINOR__ > 5 || __GNUC_PATCHLEVEL__ > 8
__delay_cycles(250000);
#else
__delay_cycles(62500);
__delay_cycles(62500);
__delay_cycles(62500);
__delay_cycles(62500);
#endif
// Loop until XT1 & DCO stabilizes, use do-while to insure that
// body is executed at least once
do
{
UCSCTL7 &= ~(XT2OFFG + XT1LFOFFG + XT1HFOFFG + DCOFFG);
SFRIFG1 &= ~OFIFG; // Clear fault flags
}
while ((SFRIFG1 & OFIFG));
// ---------------------------------------------------------------------
// Configure port mapping
// Disable all interrupts
__disable_interrupt();
// Get write-access to port mapping registers:
PMAPPWD = 0x02D52;
// Allow reconfiguration during runtime:
PMAPCTL = PMAPRECFG;
// P2.7 = TA0CCR1A or TA1CCR0A output (buzzer output)
ptr = &P2MAP0;
*(ptr + 7) = PM_TA1CCR0A;
P2OUT &= ~BIT7;
P2DIR |= BIT7;
// P1.5 = SPI MISO input
ptr = &P1MAP0;
*(ptr + 5) = PM_UCA0SOMI;
// P1.6 = SPI MOSI output
*(ptr + 6) = PM_UCA0SIMO;
// P1.7 = SPI CLK output
*(ptr + 7) = PM_UCA0CLK;
// Disable write-access to port mapping registers:
PMAPPWD = 0;
// Re-enable all interrupts
__enable_interrupt();
// Init the hardwre real time clock (RTC_A)
rtca_init();
// ---------------------------------------------------------------------
// Configure ports
// ---------------------------------------------------------------------
// Reset radio core
radio_reset();
radio_powerdown();
// ---------------------------------------------------------------------
// Init acceleration sensor
#ifdef CONFIG_MOD_ACCELEROMETER
as_init();
#else
as_disconnect();
#endif
// ---------------------------------------------------------------------
// Init LCD
lcd_init();
// ---------------------------------------------------------------------
// Init buttons
init_buttons();
// ---------------------------------------------------------------------
// Configure Timer0 for use by the clock and delay functions
timer0_init();
// Init buzzer
buzzer_init();
// ---------------------------------------------------------------------
// Init pressure sensor
#ifdef CONFIG_PRESSURE_SENSOR
ps_init();
#endif
// ---------------------------------------------------------------------
// Init other sensors
// From: "driver/battery"
battery_init();
// From: "drivers/temperature"
temperature_init();
/// @todo What is this ?
#ifdef CONFIG_INFOMEM
if (infomem_ready() == -2)
infomem_init(INFOMEM_C, INFOMEM_C + 2 * INFOMEM_SEGMENT_SIZE);
#endif
}
/*--------------------------------------------------------------------------*/
int
main(int argc, char **argv)
{
/*
* Initalize hardware.
*/
msp430_cpu_init();
clock_init();
uart_init(9600); /* Must come before first printf */
/* xmem_init(); */
PRINTF("iWatch 0.10 build at " __TIME__ " " __DATE__ "\n");
UCSCTL8 &= ~BIT2;
/*
* Hardware initialization done!
*/
/*
* Initialize Contiki and our processes.
*/
process_init();
process_start(&etimer_process, NULL);
rtimer_init();
ctimer_init();
energest_init();
ENERGEST_ON(ENERGEST_TYPE_CPU);
backlight_init();
battery_init();
SPI_FLASH_Init();
if (system_testing())
{
clock_time_t t;
backlight_on(200, 0);
t = clock_seconds();
// sleep 1
while(clock_seconds() - t <= 3);
printf("$$OK BACKLIGHT\n");
t = clock_seconds();
while(clock_seconds() - t <= 3);
backlight_on(0, 0);
motor_on(200, 0);
// sleep 1s
t = clock_seconds();
while(clock_seconds() - t <= 3);
printf("$$OK MOTOR\n");
t = clock_seconds();
while(clock_seconds() - t <= 3);
motor_on(0, 0);
#if PRODUCT_W001
I2C_Init();
codec_init();
codec_bypass(1);
// sleep 1s
t = clock_seconds();
while(clock_seconds() - t <= 3);
printf("$$OK MIC\n");
// sleep 1s
t = clock_seconds();
while(clock_seconds() - t <= 3);
codec_bypass(0);
codec_shutdown();
#endif
}
int reason = CheckUpgrade();
window_init(reason);
button_init();
rtc_init();
CFSFontWrapperLoad();
system_init(); // check system status and do factor reset if needed
I2C_Init();
//codec_init();
//ant_init();
bluetooth_init();
#ifdef PRODUCT_W004
//bmx_init();
#else
mpu6050_init();
#endif
// check the button status
if (button_snapshot() & (1 << BUTTON_UP))
{
clock_time_t t;
// delay 1 second
// button up is pressed, we will set emerging flag
motor_on(200, CLOCK_SECOND * 2);
t = clock_seconds();
while(clock_seconds() - t <= 1);
if (button_snapshot() & (1 << BUTTON_UP))
system_setemerging();
motor_on(0, 0);
}
if (!system_retail())
{
bluetooth_discoverable(1);
}
#if PRODUCT_W001
if (system_testing())
ant_init(MODE_HRM);
#endif
system_restore();
// protocol_init();
// protocol_start(1);
process_start(&system_process, NULL);
/*
* This is the scheduler loop.
*/
msp430_dco_required = 0;
/*
check firmware update
*/
if (reason == 0xff)
{
printf("Start Upgrade\n");
Upgrade();
// never return if sucessfully upgrade
}
watchdog_start();
while(1) {
int r;
do {
/* Reset watchdog. */
watchdog_periodic();
r = process_run();
} while(r > 0);
/*
* Idle processing.
*/
int s = splhigh(); /* Disable interrupts. */
/* uart1_active is for avoiding LPM3 when still sending or receiving */
if(process_nevents() != 0) {
splx(s); /* Re-enable interrupts. */
} else {
static unsigned long irq_energest = 0;
/* Re-enable interrupts and go to sleep atomically. */
ENERGEST_OFF(ENERGEST_TYPE_CPU);
ENERGEST_ON(ENERGEST_TYPE_LPM);
/* We only want to measure the processing done in IRQs when we
are asleep, so we discard the processing time done when we
were awake. */
energest_type_set(ENERGEST_TYPE_IRQ, irq_energest);
watchdog_stop();
if (shutdown_mode)
{
system_shutdown(1); // never return
LPM4;
}
if (msp430_dco_required)
{
__low_power_mode_0();
}
else
{
__low_power_mode_3();
}
/* We get the current processing time for interrupts that was
done during the LPM and store it for next time around. */
__disable_interrupt();
irq_energest = energest_type_time(ENERGEST_TYPE_IRQ);
__enable_interrupt();
watchdog_start();
ENERGEST_OFF(ENERGEST_TYPE_LPM);
ENERGEST_ON(ENERGEST_TYPE_CPU);
}
}
}
void init_application(void)
{
volatile unsigned char *ptr;
// ---------------------------------------------------------------------
// Enable watchdog
// Watchdog triggers after 16 seconds when not cleared
#ifdef USE_WATCHDOG
WDTCTL = WDTPW + WDTIS__512K + WDTSSEL__ACLK;
#else
WDTCTL = WDTPW + WDTHOLD;
#endif
// ---------------------------------------------------------------------
// Configure port mapping
// Disable all interrupts
__disable_interrupt();
// Get write-access to port mapping registers:
PMAPPWD = 0x02D52;
// Allow reconfiguration during runtime:
PMAPCTL = PMAPRECFG;
// P2.7 = TA0CCR1A or TA1CCR0A output (buzzer output)
ptr = &P2MAP0;
*(ptr + 7) = PM_TA1CCR0A;
P2OUT &= ~BIT7;
P2DIR |= BIT7;
// P1.5 = SPI MISO input
ptr = &P1MAP0;
*(ptr + 5) = PM_UCA0SOMI;
// P1.6 = SPI MOSI output
*(ptr + 6) = PM_UCA0SIMO;
// P1.7 = SPI CLK output
*(ptr + 7) = PM_UCA0CLK;
// Disable write-access to port mapping registers:
PMAPPWD = 0;
// Re-enable all interrupts
__enable_interrupt();
// Init the hardwre real time clock (RTC_A)
rtca_init();
// ---------------------------------------------------------------------
// Configure ports
// ---------------------------------------------------------------------
// Reset radio core
radio_reset();
radio_powerdown();
#ifdef CONFIG_ACCELEROMETER
// ---------------------------------------------------------------------
// Init acceleration sensor
as_init();
#else
as_disconnect();
#endif
// ---------------------------------------------------------------------
// Init buttons
init_buttons();
// ---------------------------------------------------------------------
// Configure Timer0 for use by the clock and delay functions
timer0_init();
/* Init buzzer */
buzzer_init();
// ---------------------------------------------------------------------
// Init pressure sensor
ps_init();
/* drivers/battery */
battery_init();
/* drivers/temperature */
temperature_init();
#ifdef CONFIG_INFOMEM
if (infomem_ready() == -2) {
infomem_init(INFOMEM_C, INFOMEM_C + 2 * INFOMEM_SEGMENT_SIZE);
}
#endif
}
PROCESS_THREAD(sdfclient, ev, data) {
PROCESS_BEGIN();
// registers local ipv6 address
udphelper_registerlocaladdress(0);
// delay for initializing rpl routing tree
static struct etimer timer_rpldelay;
etimer_set(&timer_rpldelay, CLOCK_SECOND * RPLINITTIME);
PROCESS_WAIT_UNTIL(etimer_expired(&timer_rpldelay));
/*
*
* start SDF !!!
*
*/
time_init = time();
printf("Started SDF-Client at %dx (", SPEEDMULTIPLIER);
uip_ipaddr_t ip;
udphelper_print_address(udphelper_address_local(&ip));
printf(")\n");
// init (after rpl dag creation!)
battery_init();
consumptionrate_init();
// bind udp socket to port
udp = udphelper_bind(SDF_PORT);
// save sink ip
udphelper_address_sink(&ip_sink);
// timer for energy neutral consumption rate
static struct etimer timer_consumptionrate;
etimer_set(&timer_consumptionrate, CLOCK_SECOND * 86400 / SPEEDMULTIPLIER);
// timer for updating the sampling rate
static struct etimer timer_updatesamplingrate;
etimer_set(&timer_updatesamplingrate, CLOCK_SECOND * SDF_SAMPLINGRATE_UPDATEINTERVAL / SPEEDMULTIPLIER);
// timer for transmitting sampling rate to children
static struct etimer timer_samplingrate_transmit;
etimer_set(&timer_samplingrate_transmit, CLOCK_SECOND * 2 / SPEEDMULTIPLIER);
etimer_stop(&timer_samplingrate_transmit); // started by update_sampling_rate()
// timer for taking samples and transmitting them
static struct etimer timer_samples;
etimer_stop(&timer_samples); // started by update_sampling_rate()
// init node with first calculation of sampling rate
update_sampling_rate(&timer_samples, &timer_samplingrate_transmit, 1);
while(1) {
PROCESS_WAIT_EVENT();
// take an energy neutral consumptionrate sample
// (this has to be the first conditin! consumptionrate should be taken everytime
// before samplingrate is calculated, so samplingrate can use the new consumptionrate
// sample to calculate a more accurate sampling rate)
if(etimer_expired(&timer_consumptionrate)) {
consumptionrate_sample();
etimer_restart(&timer_consumptionrate);
}
// update SDF sampling rate
if(etimer_expired(&timer_updatesamplingrate)) {
// no real calculation when not in init phase and parent is not sink
// (mote will keep last samplingrate as long as a new samplingrate is received)
if((time() - time_init) / SDF_INITIALIZATIONPHASE == 0 || udphelper_address_equals(udphelper_address_parent(&ip_parent), &ip_sink)) {
update_sampling_rate(&timer_samples, &timer_samplingrate_transmit, 1);
} else {
update_sampling_rate(&timer_samples, &timer_samplingrate_transmit, 0);
}
etimer_restart(&timer_updatesamplingrate);
}
// new SDF sampling rate control message
if(ev == tcpip_event && uip_newdata()) {
// for some reason the real tmote skys in TUDμNet have routing problems not existent in cooja simulator
// solution: test if sampling rate update was sent by known parent
// (prevents multiple recalculations on incorrect routing tables)
static uip_ipaddr_t ip_sender;
if(udphelper_address_equals(udphelper_address_parent(&ip_parent), udphelper_packet_senderaddress(&ip_sender))) {
last_parent_samlingrate = str2int(udphelper_packet_data());
update_sampling_rate(&timer_samples, &timer_samplingrate_transmit, 1);
etimer_restart(&timer_updatesamplingrate); // new interval for samplingrate is set by rpl parent
}
}
// transmit SDF sampling rate to childs
// (etimer_stop() seems to not work, so a condition has been added to filter out
// invalid etimer events)
if(etimer_expired(&timer_samplingrate_transmit) && samplingrate_children_transmitted < samplingrate_children_count) {
// For some reason directly sending the messages within this process block has some
// random message losses. The contiki example ipv6/rpl-udp/udp-client.c uses the
// backoff timer (ctimer) and magically it works without any message lost.
// (backoff timer should not be speed up by SPEEDMULTIPLIER!)
ctimer_set(&backofftimer_send_samplingrate, CLOCK_SECOND / 8, send_samplingrate, &timer_samplingrate_transmit);
}
// take a sample and transmit it
// (etimer_stop() seems to not work, so a condition has been added to filter out
// invalid etimer events)
if(etimer_expired(&timer_samples) && ++sampled <= samplingrate) {
// For some reason directly sending the messages within this process block has some
// random message losses. The contiki example ipv6/rpl-udp/udp-client.c uses the
// backoff timer (ctimer) and magically it works without any message lost.
// (backoff timer should not be speed up by SPEEDMULTIPLIER!)
ctimer_set(&backofftimer_send_sample, CLOCK_SECOND / 8, send_packet, &timer_samples);
}
}
PROCESS_END();
}