static void check_and_update_heartbeat_watchdog() {
ss_id_e i;
bool all_alive;
subsystem_t *ss;
all_alive = true;
if (watchdog_has_started_a_new_period()) {
// log_report(LOG_SS_PLATFORM, "PLATFORM: All subsystem's heartbeats cleared\n");
for (i=0; i < ARRAY_COUNT(subsystems); i++) {
if (IS_SS_VALID(i)) {
subsystems[i]->state->current_heartbeats = 0;
}
}
} else {
for (i=0; i < ARRAY_COUNT(subsystems); i++) {
if (!IS_SS_VALID(i)) continue;
ss = subsystems[i];
if ((ss->state->current_heartbeats & ss->state->expected_heartbeats) != ss->state->expected_heartbeats) {
all_alive = false;
PLATFORM_save_boot_reason(ss->config->name);
// log_report_fmt(LOG_SS_PLATFORM, "PLARFORM: Subsystem %s missed a heartbeat (%02x != %02x)\n",
// ss->config->name,
// ss->state->current_heartbeats,
// ss->state->expected_heartbeats);
}
}
if (all_alive) {
// log_report(LOG_SS_PLATFORM, "PLATFORM: All subsystems are alive!\n");
watchdog_restart_counter();
POWER_watchdog_restart_counter();
led_toggle(1); /* triggers the Alive LED */
PLATFORM_save_boot_reason_unexpected();
}
}
}
int
main(void)
{
HAL_Init();
SystemClock_Config();
RCC_Config();
GPIO_Config();
UART_Config();
if (SysTick_Config(SystemCoreClock / 1000)) {
error_handler();
}
log_init(LOGLEVEL_INFO);
log_info("PROJECT STERTED");
while (1) {
led_toggle(LED_BLUE);
log_info("Hello! :)");
HAL_Delay(DELAY);
}
}
void __fatal_error(const char *msg) {
for (volatile uint delay = 0; delay < 10000000; delay++) {
}
led_state(1, 1);
led_state(2, 1);
led_state(3, 1);
led_state(4, 1);
stdout_tx_strn("\nFATAL ERROR:\n", 14);
stdout_tx_strn(msg, strlen(msg));
#if 0 && MICROPY_HW_HAS_LCD
lcd_print_strn("\nFATAL ERROR:\n", 14);
lcd_print_strn(msg, strlen(msg));
#endif
for (uint i = 0;;) {
led_toggle(((i++) & 3) + 1);
for (volatile uint delay = 0; delay < 10000000; delay++) {
}
if (i >= 16) {
// to conserve power
__WFI();
}
}
}
// EXTI
// for USRSW on A13
// for cc3000 on A14
void EXTI15_10_IRQHandler(void) {
// work out if it's A13 that had the interrupt
if (EXTI_GetITStatus(EXTI_Line13) != RESET) {
// this is used just to wake the device
EXTI_ClearITPendingBit(EXTI_Line13);
}
// work out if it's A14 that had the interrupt
if (EXTI_GetITStatus(EXTI_Line14) != RESET) {
led_toggle(PYB_LED_G2);
extern void SpiIntGPIOHandler(void);
extern uint32_t exti14_enabled;
extern uint32_t exti14_missed;
//printf("-> EXTI14 en=%lu miss=%lu\n", exti14_enabled, exti14_missed);
if (exti14_enabled) {
exti14_missed = 0;
SpiIntGPIOHandler(); // CC3000 interrupt
} else {
exti14_missed = 1;
}
EXTI_ClearITPendingBit(EXTI_Line14);
//printf("<- EXTI14 done\n");
}
}
int main(void) {
setup();
printf("%s\r\n", "STARTING LOOP");
float encoder_master_count = 0;
float previous_degs = 0;
float current_degs = 0;
uint16_t current_ticks = 0;
uint16_t previous_ticks = spi_read_ticks();
while (1) {
if (timer_flag(&timer2)) {
// Blink green light to show normal operation.
timer_lower(&timer2);
led_toggle(&led2);
printf("ddegs %3f, dd %d\r\n", delta_degs, drive_direction);
}
if (timer_flag(&timer3)) {
timer_lower(&timer3);
delta_degs = current_degs - previous_degs;
current_duty_cycle = damper(delta_degs);
previous_degs = current_degs;
}
if (!sw_read(&sw2)) {
// If switch 2 is pressed, the UART output terminal is cleared.
printf("%s", clear);
}
ServiceUSB();
current_ticks = spi_read_ticks();
encoder_master_count = encoder_counter(current_ticks, previous_ticks, encoder_master_count);
current_degs = count_to_deg(encoder_master_count);
previous_ticks = current_ticks;
}
}
void led_net_callback(config_section_t * conf_sect, const msg_lvl2_t * net_msg) {
int rc;
if(net_msg != NULL) {
pmd_net_led_data_t pmd_led_data;
rc = pmd_net_led_read_data(&(net_msg->data), &pmd_led_data);
if(rc == 0) {
switch(pmd_led_data.operation) {
case PMD_NET_LED_ON:
led_on(conf_sect);
break;
case PMD_NET_LED_OFF:
led_off(conf_sect);
break;
case PMD_NET_LED_TOGGLE:
led_toggle(conf_sect);
break;
}
}
}
}
void led_twinkle( uint8 id ,uint16 interval )
{
switch(id)
{
case LED_GREEN:
led_toggle(LED_GREEN);
hal_delay( interval );
led_toggle(LED_GREEN);
break;
case LED_RED:
led_toggle(LED_RED);
hal_delay( interval );
led_toggle(LED_RED);
break;
case LED_YELLOW:
led_toggle(LED_YELLOW);
hal_delay( interval );
led_toggle(LED_YELLOW);
break;
}
}
/*********************************
main entry point
*********************************/
int main ( void )
{
// Init the basic hardware
InitCnsts();
InitHardware();
// Start the main 1Khz timer and PWM timer
InitTimer1();
InitTimer2();
InitPWM();
// Initialize A2D,
InitA2D();
UART1_Init(XBEE_SPEED); // for communication and control signals
UART2_Init(LOGGING_RC_SPEED); // for spektrum RC satellite receiver
// Wait for a bit before doing rate gyro bias calibration
// TODO: test this length of wait
uint16_t i=0;for(i=0;i<60000;i++){Nop();}
// turn on the leds until the bias calibration is complete
led_on(LED_RED);
led_on(LED_GREEN);
// Initialize the AHRS
AHRS_init();
// Initialize the Controller variables
Controller_Init();
// MAIN CONTROL LOOP: Loop forever
while (1)
{
// Gyro propagation
if(loop.GyroProp){
loop.GyroProp = 0;
// Call gyro propagation
AHRS_GyroProp();
}
// Attitude control
if(loop.AttCtl){
loop.AttCtl = 0;
// Call attitude control
Controller_Update();
}
// Accelerometer correction
if( loop.ReadAccMag ){
loop.ReadAccMag = 0;
AHRS_AccMagCorrect( );
}
// Send data over modem - runs at ~20Hz
if(loop.SendSerial){
loop.SendSerial = 0;
// Send debug packet
UART1_SendAHRSpacket();
}
// Process Spektrum RC data
if(loop.ProcessSpektrum){
loop.ProcessSpektrum = 0;
UART2_ProcessSpektrumData();
}
// Read data from UART RX buffers - 500 Hz
if(loop.ReadSerial){
loop.ReadSerial = 0;
// Read serial data
//UART2_FlushRX_Spektrum();
}
// Toggle Red LED at 1Hz
if(loop.ToggleLED){
loop.ToggleLED = 0;
// Toggle LED
led_toggle(LED_RED);
}
} // End while(1)
}
// Timer callback
static void timer_handler (void* p_context) {
led_toggle(LED);
}
static THD_FUNCTION(rx_thread, arg) {
(void)arg;
chRegSetThreadName("CC2520 RX");
for(;;) {
if (basicRfPacketIsReady()) {
static uint8_t buf[130];
unsigned int len = 0;
unsigned int ind = 0;
len = basicRfReceive(buf, 130, NULL);
MOTE_PACKET packet = buf[0];
led_toggle(LED_RED);
switch (packet) {
case MOTE_PACKET_FILL_RX_BUFFER:
memcpy(rx_buffer + buf[1], buf + 2, len - 2);
break;
case MOTE_PACKET_FILL_RX_BUFFER_LONG: {
int rxbuf_ind = (unsigned int)buf[1] << 8;
rxbuf_ind |= buf[2];
if (rxbuf_ind < RX_BUFFER_SIZE) {
memcpy(rx_buffer + rxbuf_ind, buf + 3, len - 3);
}
}
break;
case MOTE_PACKET_PROCESS_RX_BUFFER: {
ind = 1;
int rxbuf_len = (unsigned int)buf[ind++] << 8;
rxbuf_len |= (unsigned int)buf[ind++];
if (rxbuf_len > RX_BUFFER_SIZE) {
break;
}
uint8_t crc_high = buf[ind++];
uint8_t crc_low = buf[ind++];
memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind);
if (crc16(rx_buffer, rxbuf_len)
== ((unsigned short) crc_high << 8
| (unsigned short) crc_low)) {
comm_usb_send_packet(rx_buffer, rxbuf_len);
}
}
break;
case MOTE_PACKET_PROCESS_SHORT_BUFFER:
comm_usb_send_packet(buf + 1, len - 1);
break;
default:
break;
}
}
chThdSleepMicroseconds(100);
}
}
int preflight_check_main(int argc, char *argv[])
{
bool fail_on_error = false;
if (argc > 1 && !strcmp(argv[1], "--help")) {
warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
exit(1);
}
if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
fail_on_error = true;
}
bool system_ok = true;
int fd;
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
/* give the system some time to sample the sensors in the background */
usleep(150000);
/* ---- MAG ---- */
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- ACCEL ---- */
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */
close(fd);
fd = open(GYRO_DEVICE_PATH, 0);
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- BARO ---- */
close(fd);
fd = open(BARO_DEVICE_PATH, 0);
/* ---- RC CALIBRATION ---- */
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
float param_min, param_max, param_trim, param_rev, param_dz;
bool rc_ok = true;
char nbuf[20];
for (int i = 0; i < 12; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, ¶m_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, ¶m_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, ¶m_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, ¶m_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, ¶m_dz);
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
}
/* XXX needs inspection of all the _MAP params */
// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
// /* give system time to flush error message in case there are more */
// usleep(100000);
// count++;
// }
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
rc_ok = false;
}
}
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
system_eval:
if (system_ok) {
/* all good, exit silently */
exit(0);
} else {
fflush(stdout);
int buzzer = open("/dev/tone_alarm", O_WRONLY);
int leds = open(LED_DEVICE_PATH, 0);
/* flip blue led into alternating amber */
led_off(leds, LED_BLUE);
led_off(leds, LED_AMBER);
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 150; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
if (i % 10 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 4);
} else if (i % 5 == 0) {
ioctl(buzzer, TONE_SET_ALARM, 2);
}
usleep(100000);
}
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, 0);
/* switch on leds */
led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER);
if (fail_on_error) {
/* exit with error message */
exit(1);
} else {
/* do not emit an error code to make sure system still boots */
exit(0);
}
}
}
void updateProgram(void)
{
int clear = 0;
int cmdByte, i, temp;
FLASH_EraseResult result;
uint32_t addr, maxAddr = 0;
uint16_t endSector = 0xFFFF;
_ee_getReserved(_AI_EE_RES_ADDR_MAX_SECTOR, &endSector);
if (endSector > APPLICATION_END_SECTOR || !IS_FLASH_SECTOR(endSector))
endSector = APPLICATION_END_SECTOR;
lcd_clear();
lcd_printf("Aithon Board\nProgramming...");
// Unlock the Flash Program Erase controller
FLASH_If_Init();
while (TRUE)
{
led_toggle(0);
cmdByte = getByte();
debugPrintCmd(cmdByte);
switch (cmdByte)
{
case SYNC:
// sync
flushInterface();
sendResponse(SYNC, ACK);
break;
case ERASE_FLASH_START:
if (FLASH_If_Erase_Start() == FLASH_ERASE_IN_PROGRESS)
sendResponse(ERASE_FLASH_START, ACK);
else
sendResponse(ERASE_FLASH_START, NACK);
break;
case ERASE_FLASH_STATUS:
result = FLASH_If_Erase_Status(endSector);
if (result == FLASH_ERASE_COMPLETE)
sendResponse(ERASE_FLASH_STATUS, ACK);
else if (result == FLASH_ERASE_IN_PROGRESS)
sendResponse(ERASE_FLASH_STATUS, BUSY);
else
sendResponse(ERASE_FLASH_STATUS, NACK);
break;
case SET_ADDR:
// Read in the address, MSB first.
addr = 0;
for (i = 0; i < 4; i++)
{
if ((temp = getByte()) == Q_TIMEOUT)
break;
addr |= (((uint8_t) temp) & 0xFF) << (i * 8);
}
// Check for errors.
if (temp == Q_TIMEOUT)
sendResponse(SET_ADDR, NACK);
else
{
sendResponse(SET_ADDR, ACK);
// We'll get relative addresses, so add the start address.
addr += APPLICATION_START_ADDRESS;
}
break;
case CHECK_ADDR:
// Get the checksum
temp = getByte();
if (temp == Q_TIMEOUT)
sendResponse(CHECK_ADDR, NACK);
else
{
// Subtract the start address before calculating the checksum
addr -= APPLICATION_START_ADDRESS;
if (temp == calcChecksum((uint8_t *)&addr, 4))
sendResponse(CHECK_ADDR, ACK);
else
sendResponse(CHECK_ADDR, NACK);
addr += APPLICATION_START_ADDRESS;
}
break;
case FILL_BUFFER:
for (i = 0; i < PACKET_LEN; i++)
{
if ((temp = getByte()) == Q_TIMEOUT)
break;
_buffer[i] = (uint8_t) (temp & 0xFF);
}
if (temp == Q_TIMEOUT)
sendResponse(FILL_BUFFER, NACK);
else
sendResponse(FILL_BUFFER, ACK);
break;
case CHECK_BUFFER:
// Get the checksum
temp = getByte();
if (temp != Q_TIMEOUT && temp == calcChecksum(_buffer, PACKET_LEN))
sendResponse(CHECK_BUFFER, ACK);
else
sendResponse(CHECK_BUFFER, NACK);
break;
case COMMIT_BUFFER:
maxAddr = addr + PACKET_LEN - 1;
if (FLASH_If_Write((__IO uint32_t *)&addr, (uint32_t *)_buffer, PACKET_LEN/4))
sendResponse(COMMIT_BUFFER, NACK);
else
sendResponse(COMMIT_BUFFER, ACK);
break;
case START_PROGRAM:
sendResponse(START_PROGRAM, ACK);
flushInterface();
_ee_putReserved(_AI_EE_RES_ADDR_MAX_SECTOR, FLASH_Addr_To_Sector(maxAddr));
delayS(1);
startProgram();
// ...should never get here
return;
case Q_TIMEOUT:
default:
if (clear == 0) {
lcd_clear();
clear = 1;
}
lcd_printf ("0%x ", cmdByte);
break;
}
}
}
int preflight_check_main(int argc, char *argv[])
{
bool fail_on_error = false;
if (argc > 1 && !strcmp(argv[1], "--help")) {
warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
exit(1);
}
if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
fail_on_error = true;
}
bool system_ok = true;
int fd;
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
/* give the system some time to sample the sensors in the background */
usleep(150000);
/* ---- MAG ---- */
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- ACCEL ---- */
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */
close(fd);
fd = open(GYRO_DEVICE_PATH, 0);
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
system_ok = false;
goto system_eval;
}
/* ---- BARO ---- */
close(fd);
fd = open(BARO_DEVICE_PATH, 0);
close(fd);
/* ---- RC CALIBRATION ---- */
bool rc_ok = (OK == rc_calibration_check());
/* warn */
if (!rc_ok)
warnx("rc calibration test failed");
/* require RC ok to keep system_ok */
system_ok &= rc_ok;
system_eval:
if (system_ok) {
/* all good, exit silently */
exit(0);
} else {
fflush(stdout);
warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM");
fflush(stderr);
int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
int leds = open(LED_DEVICE_PATH, 0);
if (leds < 0) {
close(buzzer);
errx(1, "failed to open leds, aborting");
}
/* flip blue led into alternating amber */
led_off(leds, LED_BLUE);
led_off(leds, LED_AMBER);
led_toggle(leds, LED_BLUE);
/* display and sound error */
for (int i = 0; i < 50; i++)
{
led_toggle(leds, LED_BLUE);
led_toggle(leds, LED_AMBER);
if (i % 10 == 0) {
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
} else if (i % 5 == 0) {
ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
}
usleep(100000);
}
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
/* switch on leds */
led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER);
if (fail_on_error) {
/* exit with error message */
exit(1);
} else {
/* do not emit an error code to make sure system still boots */
exit(0);
}
}
}
static void CLI_blink_switch(servo_usb_control_context_t *ctx, int on){
(void) ctx;
led_toggle();
blink_switch(on);
}
/**
* @brief Receive communication packets and handle them
*
* This function decodes packets on the protocol level and also handles
* their value by calling the appropriate functions.
*/
void communication_receive(void)
{
mavlink_message_t msg;
mavlink_status_t status =
{ 0 };
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH ONBOARD COMPUTER
while (uart0_char_available())
{
uint8_t c = uart0_get_char();
if (global_data.state.uart0mode == UART_MODE_MAVLINK)
{
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_0, &msg);
}
}
else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD)
{
uart1_transmit(c);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
// COMMUNICATION WITH EXTERNAL COMPUTER
while (uart1_char_available())
{
uint8_t c = uart1_get_char();
// Check if this link is used for MAVLink or GPS
if (global_data.state.uart1mode == UART_MODE_MAVLINK)
{
//uart0_transmit((unsigned char)c);
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status))
{
// Handle message
handle_mavlink_message(MAVLINK_COMM_1, &msg);
}
}
else if (global_data.state.uart1mode == UART_MODE_GPS)
{
if (global_data.state.gps_mode == 10)
{
static uint8_t gps_i = 0;
static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN
- 1)
{
gps_i = 0;
char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
strncpy(gps_chars_buf, gps_chars,
MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
debug_message_buffer(gps_chars_buf);
}
gps_chars[gps_i++] = c;
}
if (gps_parse(c))
{
// New GPS data received
//debug_message_buffer("RECEIVED NEW GPS DATA");
parse_gps_msg();
if (gps_lat == 0)
{
global_data.state.gps_ok = 0;
//debug_message_buffer("GPS Signal Lost");
}
else
{
global_data.state.gps_ok = 1;
mavlink_msg_gps_raw_send(
global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_unix_time(), gps_mode, gps_lat
/ 1e7f, gps_lon / 1e7f, gps_alt / 100.0f,
0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f);
}
// // Output satellite info
// for (int i = 0; i < gps_nb_channels; i++)
// {
// mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno);
// }
}
}
else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD)
{
uart0_transmit(c);
led_toggle(LED_YELLOW);
}
// And get the next one
}
// Update global packet drops counter
global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count;
global_data.comm.uart0_rx_success_count += status.packet_rx_success_count;
status.packet_rx_drop_count = 0;
}
void wait_for_clock() {
led_toggle();
while (gpio_pin_read(GPIO_PIN23) == 0) {}
while (gpio_pin_read(GPIO_PIN23) == 1) {}
}
/**@breif Function for checking event flags and handling them.
*/
void check_and_handle_asc_flags(void)
{
uint32_t asc_flags = ascs_events_get();
do
{
if (asc_flags & EVENT_ASC_STATE_CHANGED)
{
ascs_event_clear(EVENT_ASC_STATE_CHANGED);
//Determine which state changed and react
ascs_states_t state = ascs_state_get();
switch(state)
{
case ASSIGNED:
{
led_on(CONNECTED_LED);
break;
}
case SEARCHING: //Intentional fallthrough
case REQUESTING: //Intentional fallthrough
case WAITING: //Intentional fallthrough
case CONFIRMING: //Intentional fallthrough
case OFF: //Intentional fallthrough
default:
{
led_off(CONNECTED_LED);
break;
}
}
}
if (asc_flags & EVENT_ASC_DEVICE_IN_WRONG_STATE)
{
ascs_event_clear(EVENT_ASC_DEVICE_IN_WRONG_STATE);
//Do nothing
for(;;)
{
led_toggle(LED_ERROR_0);
led_toggle(LED_ERROR_1);
led_toggle(LED_ERROR_2);
led_toggle(LED_ERROR_3);
nrf_delay_ms(200);
}
}
if (asc_flags & EVENT_ASC_LIGHT_STATE_CHANGED)
{
ascs_event_clear(EVENT_ASC_LIGHT_STATE_CHANGED);
asc_slave_states_t light_state = ascs_light_state_get();
switch(light_state)
{
case SLAVE_STATE_ON:
{
led_on(LIGHT);
break;
}
case SLAVE_STATE_OFF:
{
led_off(LIGHT);
break;
}
default:
{
break;
}
}
}
if (asc_flags & EVENT_ASC_REQUEST_RECEIVED)
{
ascs_event_clear(EVENT_ASC_REQUEST_RECEIVED);
uint8_t response_buffer[ANT_STANDARD_DATA_PAYLOAD_SIZE] = {0};
asc_request_data_t request = ascs_get_last_request();
//encode the response buffer with whatever page was requested
switch(request.page_id_requested)
{
default:
{
break;
}
}
//tell the slave to send the response
ascs_send_response(response_buffer);
}
if (asc_flags & EVENT_ASC_DATA_TIMEOUT)
{
ascs_event_clear(EVENT_ASC_DATA_TIMEOUT);
//Do nothing
}
asc_flags = ascs_events_get();
}
while(asc_flags != 0);
}
void tinymac_recvnode(void)
{
TiCc2420Adapter * cc;
TiFrameRxTxInterface * rxtx;;
TiTinyMAC * mac;
TiTimerAdapter *timer;
TiFrame * rxbuf;
char * msg = "welcome to tinymac recv test...";
int len=0;
target_init();
// flash the led to indicate the software is successfully running now.
//
led_open();
led_on( LED_ALL );
hal_delay( 500 );
led_off( LED_ALL );
led_on( LED_RED );
// initialize the runtime library for debugging input/output and assertion
// hal_assert_report is defined in module "hal_assert"
//
//dbo_open( 38400 );
rtl_init( (void *)dbio_open(38400), (TiFunDebugIoPutChar)dbio_putchar, (TiFunDebugIoGetChar)dbio_getchar, hal_assert_report );
dbc_putchar( 0xF0 );
dbc_mem( msg, strlen(msg) );
cc = cc2420_construct( (void *)(&m_cc), sizeof(TiCc2420Adapter) );
mac = tinymac_construct((char *)(&m_tiny), sizeof(TiTinyMAC));
timer= timer_construct((char *)(&m_timer),sizeof(TiTimerAdapter));
#ifdef CONFIG_TSET_LISTENER
// cc = cc2420_open( cc, 0, _aloha_listener, NULL, 0x00 );
cc = cc2420_open( cc, 0, tinymac_evolve, mac, 0x00 );
rxtx = cc2420_interface( cc, &m_rxtx );
mac = tinymac_open( mac, rxtx, CONFIG_CHANNEL, CONFIG_PANID, CONFIG_LOCAL_ADDRESS,
timer, _tinymac_listener, NULL,0x00 );
#endif
#ifndef CONFIG_TSET_LISTENER
cc = cc2420_open( cc, 0, NULL, NULL, 0x00 );
rxtx = cc2420_interface( cc, &m_rxtx );
mac = tinymac_open( mac, rxtx, CONFIG_CHANNEL, CONFIG_PANID, CONFIG_LOCAL_ADDRESS, NULL, NULL,0x00 );
#endif
cc2420_setchannel( cc, CONFIG_CHANNEL );
cc2420_setrxmode( cc ); // enable RX mode
cc2420_setpanid( cc, CONFIG_PANID ); // network identifier, seems no use in sniffer mode
cc2420_setshortaddress( cc, CONFIG_LOCAL_ADDRESS ); // in network address, seems no use in sniffer mode
cc2420_enable_autoack( cc );
#ifdef CONFIG_TEST_ADDRESSRECOGNITION
cc2420_enable_addrdecode( cc );
#else
cc2420_disable_addrdecode( cc );
#endif
#ifdef CONFIG_TEST_ACK
cc2420_enable_autoack( cc );
#endif
rxbuf = frame_open( (char*)(&m_rxbufmem), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 );
#ifdef CONFIG_TEST_ACK
//fcf = OPF_DEF_FRAMECONTROL_DATA_ACK;
#else
//fcf = OPF_DEF_FRAMECONTROL_DATA_NOACK;
#endif
hal_enable_interrupts();
/* Wait for listener action. The listener function will be called by the TiCc2420Adapter
* object when a frame arrives */
#ifdef CONFIG_TEST_LISTENER
while (1) {}
#endif
/* Query the TiCc2420Adapter object if there's no listener */
#ifndef CONFIG_TEST_LISTENER
while(1)
{
frame_reset( rxbuf, 3, 20, 0 );
len = tinymac_recv( mac, rxbuf, 0x00 );
if (len > 0)
{
dbc_putchar( 0xF3 );
dbc_mem( frame_startptr(rxbuf), frame_length(rxbuf) );
//frame_moveouter( rxbuf );
//_output_frame( rxbuf, NULL );
//frame_moveinner( rxbuf );
led_off( LED_RED );
/* warning: You shouldn't wait too long in the while loop, or else
* you may encounter frame loss. However, the program should still
* work properly even the delay time is an arbitrary value. No error
* are allowed in this case.
*/
//hal_delay( 500 );
led_toggle( LED_RED );
//hal_delay( 500 );
}
tinymac_evolve(mac, NULL );
}
#endif
frame_close( rxbuf );
tinymac_close( mac );
cc2420_close( cc );
}
void led_test( void )
{
int i;
target_init();
led_open();
led_on( LED_ALL );
hal_delay( 1000 );
/* testing each single LED */
led_off( LED_ALL );
i=0;
while (i<3)
{
led_off(LED_RED) ;
hal_delay(300);
led_on(LED_RED) ;
hal_delay(300);
i++;
}
led_off( LED_RED );
i=0;
while (i<3)
{
led_off(LED_GREEN) ;
hal_delay(300);
led_on(LED_GREEN) ;
hal_delay(300);
i++;
}
led_off( LED_GREEN );
i=0;
while (i<3)
{
led_off(LED_YELLOW) ;
hal_delay(300);
led_on(LED_YELLOW) ;
hal_delay(300);
i++;
}
led_off( LED_YELLOW );
/* test the LEDs at the same time */
led_off( LED_ALL );
i = 0;
while (i<3)
{
led_toggle( LED_RED );
led_toggle( LED_YELLOW );
led_toggle( LED_GREEN );
hal_delay(500);
i++;
}
/* testing LED twinkle
* there's a infinite loop in led_twinkle, so it will never come out*/
led_off( LED_ALL );
// led_twinkle( LED_RED, 300, 5 );
// led_off( LED_RED );
led_twinkle( LED_YELLOW, 300, 5 );
led_off( LED_YELLOW );
// led_twinkle( LED_GREEN, 300, 5 );
// led_off( LED_GREEN );
}
AbiBindMsgIMU_ACCEL_INT32(ABI_BROADCAST, &accel_ev, accel_cb);
AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb);
}
static inline void led_toggle(void)
{
#ifdef BOARD_LISA_L
LED_TOGGLE(7);
#endif
}
static inline void main_periodic_task(void)
{
RunOnceEvery(100, {
led_toggle();
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
});
#if USE_I2C1
RunOnceEvery(111, {
uint16_t i2c1_wd_reset_cnt = i2c1.errors->wd_reset_cnt;
uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt;
uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt;
uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt;
uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt;
uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt;
uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt;
uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt;
uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt;
uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt;
int16_t main(void) {
init_pin();
init_clock();
init_uart();
init_ui();
init_timer();
init_oc();
//setup the signal input pin
pin_digitalIn(&D[4]);
val1 = 0;
val2 = 0;
pos = 0; //16 bit int with binary point in front of the MSB
led_on(&led2);
timer_setPeriod(&timer2, PULSE_FREQUENCY); //how often we send a pulse
timer_start(&timer2);
timer_setPeriod(&timer3, 0.5); //heartbeat
timer_start(&timer3);
oc_servo(&oc1,&D[0],&timer4, INTERVAL,MIN_WIDTH, MAX_WIDTH, pos);
oc_servo(&oc2,&D[2],&timer5, INTERVAL,MIN_WIDTH, MAX_WIDTH, pos);
oc_pwm(&oc3,&D[3],NULL,FREQ,ZERO_DUTY);
printf("Good morning\n");
InitUSB(); // initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
while (1) {
ServiceUSB();
pin_write(&D[0],val1);
pin_write(&D[2],val2);
//adapted from Patrick and Charlie's approach
if (!send_pulse && timer_read(&timer2) < PULSE_WIDTH){
send_pulse = 1;
pin_write(&D[3],HALF_DUTY);
get_distance = 1;
} else if (send_pulse && timer_read(&timer2) >= PULSE_WIDTH) {
send_pulse = 0;
pin_write(&D[3],ZERO_DUTY);
}
if (timer_read(&timer2) >= ECHO_TIME)
{
if (pin_read(&D[4]) && get_distance)
{
printf("%d\n", timer_read(&timer2));
get_distance = 0;
}
}
if (timer_flag(&timer3)) {
//show a heartbeat and a status message
timer_lower(&timer3);
led_toggle(&led1);
}
}
}
// Timer fired handler
static void timer_handler (void* p_context) {
//led_toggle(BLEES_LED_PIN);
led_toggle(LED_0);
}
void userbutton_callback(button_id_t button_id)
{
log_print_string("Button PB%u pressed.", button_id);
console_print("Button Pressed\r\n");
led_toggle(0);
}
int main(void)
{
uint16 value, count;
uint8 len;
char * request;
char * response;
char * payload;
char * msg = "welcome to node...";
TiTimerAdapter * timeradapter;
TiTimerManager * vtm;
TiTimer * mac_timer;
TiCc2420Adapter * cc;
TiFrameRxTxInterface * rxtx;
TiNioAcceptor * nac;
TiAloha * mac;
TiAdcAdapter * adc;
TiLumSensor * lum;
TiDataTreeNetwork * dtp;
TiFrame * rxbuf;
TiFrame * txbuf;
target_init();
led_open();
led_on( LED_ALL );
hal_delay( 500 );
led_off( LED_ALL );
rtl_init( (void *)dbio_open(38400), (TiFunDebugIoPutChar)dbio_putchar, (TiFunDebugIoGetChar)dbio_getchar, hal_assert_report );
dbc_write( msg, strlen(msg) );
timeradapter = timer_construct( (void *)(&m_timeradapter), sizeof(m_timeradapter) );
vtm = vtm_construct( (void*)&m_vtm, sizeof(m_vtm) );
cc = cc2420_construct( (char *)(&m_cc), sizeof(TiCc2420Adapter) );
nac = nac_construct( &m_nacmem[0], NAC_SIZE );
mac = aloha_construct( (char *)(&m_aloha), sizeof(TiAloha) );
dtp = dtp_construct( (char *)(&m_dtp), sizeof(TiDataTreeNetwork) );
adc = adc_construct( (void *)&m_adc, sizeof(TiAdcAdapter) );
lum = lum_construct( (void *)&m_lum, sizeof(TiLumSensor) );
txbuf = frame_open( (char*)(&m_txbuf), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 );
rxbuf = frame_open( (char*)(&m_rxbuf), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 );
// timeradapter is used by the vtm(virtual timer manager). vtm require to enable the
// period interrupt modal of vtm
//timeradapter = timer_open( timeradapter, 0, NULL, NULL, 0x01 );
vtm = vtm_open( vtm, timeradapter, CONFIG_VTM_RESOLUTION );
cc = cc2420_open(cc, 0, NULL, NULL, 0x00 );
rxtx = cc2420_interface( cc, &m_rxtx );
mac_timer = vtm_apply( vtm );
mac_timer = vti_open( mac_timer, NULL, mac_timer);
hal_assert( rxtx != NULL );
nac = nac_open( nac, rxtx, CONFIG_NIOACCEPTOR_RXQUE_CAPACITY, CONFIG_NIOACCEPTOR_TXQUE_CAPACITY);
hal_assert( nac != NULL );
mac = aloha_open( mac, rxtx,nac, CONFIG_NODE_CHANNEL, CONFIG_NODE_PANID, CONFIG_NODE_ADDRESS,mac_timer, NULL, NULL,0x01);
adc = adc_open( adc, 0, NULL, NULL, 0 );
lum = lum_open( lum, 0, adc );
dtp = dtp_open( dtp, mac, CONFIG_NODE_ADDRESS, NULL, NULL, 0x00 );
//todo
cc2420_setchannel( cc, CONFIG_NODE_CHANNEL );
cc2420_setrxmode( cc ); // enable RX mode
cc2420_setpanid( cc, CONFIG_NODE_PANID ); // network identifier, seems no use in sniffer mode
cc2420_setshortaddress( cc, CONFIG_NODE_ADDRESS ); // in network address, seems no use in sniffer mode
cc2420_enable_autoack( cc );
//todo
cc2420_settxpower( cc, CC2420_POWER_1);//cc2420_settxpower( cc, CC2420_POWER_2);
cc2420_enable_autoack( cc );
// ledtune = ledtune_construct( (void*)(&m_ledtune), sizeof(m_ledtune), vti );
// ledtune = ledtune_open( ledtune );
/* assert: all the above open() functions return non NULL values */
hal_assert((timeradapter != NULL) && (cc != NULL) && (mac != NULL) && (adc != NULL) && (lum != NULL)
&& (rxbuf != NULL) && (txbuf != NULL) && (dtp != NULL));
hal_enable_interrupts();
dtp->state = DTP_STATE_IDLE;//todo for testing 临时用这一句必须删掉
//todo for testing
//dtp->root = 0x01;//todo for testing
//dtp->parent = 0x03;//todo for testing
/*
while ( 1)//todo for testing
{
response = frame_startptr( txbuf );
value = lum_value( lum );
payload = DTP_PAYLOAD_PTR(response);
payload[0] = 0x13;
payload[1] = 0x14;
if (dtp_send_response(dtp, txbuf, 0x01) > 0)
{
led_toggle( LED_RED);//todo for testing
}
dtp_evolve( dtp, NULL );
hal_delay( 2000);//todo for testing
}
*/
while(1)
{
/* Only the following two kinds of frames will be put into "rxbuf" by dtp_recv()
* - broadcast frames. the destination address of these frames are 0xFFFF.
* - destination is the current node.
*/
//dbo_putchar(0x33);
len = dtp_recv( dtp, rxbuf, 0x00 );
if (len > 0)
{
//ieee802frame154_dump( rxbuf);
request = frame_startptr( rxbuf );
switch (DTP_CMDTYPE(request))
{
/* if the frame is DTP_DATA_REQUEST, then the node will measure the data and
* encapsulate the data into the txbuf, which is a TiOpenFrame and sent it back.
*/
case DTP_DATA_REQUEST:
//payload = DTP_PAYLOAD_PTR( frame_startptr(txbuf) );
//ledtune_write( ledtune, MAKE_WORD(payload[1], payload[0]) );
// response frame = PHY Length 1B
// + Frame Control 2B
// + Sequence No 1B
// + Destination Pan & Address 4B
// + Source Pan & Address 4B
// + DTP Section 15B
//opf_cast( txbuf, 50, OPF_DEF_FRAMECONTROL_DATA_ACK );
response = frame_startptr( txbuf );
value = lum_value( lum );
DTP_SET_MAX_HOPCOUNT( response,0x03);//todo for testing
payload = DTP_PAYLOAD_PTR(response);
//payload[0] = 0x17;//todo 第三个节点数据
//payload[1] = 0x18;//todo 第三个节点数据
//payload[1] = 0x13;
//payload[2] = 0x14;
payload[1] = 0x15;//todo 另一个节点
payload[2] = 0x16;//todo 另一个节点
/* call dtp_send_response() to send the data in txbuf out.
*
* modified by zhangwei on 20091230
* - Bug fix. In the past, there's no delay between two adjacent
* dtp_send_response() calls. This policy is too ambitious and this
* node will occupy the whole time so that the other nodes will lost
* chances to send, or encounter much higher frame collision probabilities.
* so I add a little time delay here.
* Attention the delay time here shouldn't be too large because
* we don't want the hal_delay() to occupy all the CPU time. If this
* occurs, it may lead to unnecessary frame lossing.
*/
// try some times until the RESPONSE is successfully sent
for (count=0; count<10; count++)
{
//hal_delay( 500);
if (dtp_send_response(dtp, txbuf, 0x03) > 0)
{
led_toggle( LED_RED);//todo for testing
break;
}
//hal_delay( 50 );
}
break;
default:
//hal_assert(false);
break;
}
}
nac_evolve( nac,NULL);//todo for tesitng
aloha_evolve( mac,NULL);//todo for testing
dtp_evolve( dtp, NULL );
hal_delay( 50 );
}
}
mp_obj_t led_obj_toggle(mp_obj_t self_in) {
pyb_led_obj_t *self = self_in;
led_toggle(self->led_id);
return mp_const_none;
}
/// \method toggle()
/// Toggle the LED between on and off.
mp_obj_t led_obj_toggle(mp_obj_t self_in) {
board_led_obj_t *self = self_in;
led_toggle(self);
return mp_const_none;
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
led_toggle();
usb_tim_cb(htim);
}
int main(void)
{
bool_t isUserRun = FALSE;
bool_t displayCountdown = TRUE;
uint16_t bootByte;
_ee_getReserved(_AI_EE_RES_ADDR_BOOT, &bootByte);
if (button_get(0) && button_get(1))
{
// Both buttons are pressed so the user
// wants to run the bootloader.
isUserRun = TRUE;
}
else if (bootByte != _AI_EE_RES_VAL_BOOT_RUN)
{
startProgram();
}
_ee_putReserved(_AI_EE_RES_ADDR_BOOT, _AI_EE_RES_VAL_DEFAULT);
led_on(0);
led_on(1);
int i, j;
for (i = 0; i < BOOT_TIMEOUT; i++)
{
if (isUserRun)
{
// flash LED1
if (i % 100 == 0)
led_toggle(1);
// update the countdown
if (i % 1000 == 0)
{
lcd_clear();
lcd_printf("Aithon Board\n%d", (BOOT_TIMEOUT-i)/1000);
displayCountdown = TRUE;
}
// show the bootloader build date if button 0 pressed
if (button_get(0) && displayCountdown) {
if (i > (.1 * BOOT_TIMEOUT)) {
lcd_clear();
lcd_printf(DATE);
displayCountdown = FALSE;
}
}
}
// check all the interfaces for a SYNC
for (j = 0; j < NUM_INTERFACES; j++)
{
if (sdGetTimeout(_interfaces[j], TIME_IMMEDIATE) == SYNC)
{
_interface = _interfaces[j];
updateProgram();
// We should never get here...
while(1);
}
}
chThdSleepMilliseconds(1);
}
startProgram();
return 0;
}
/*******************************************************************
* MAIN()
*******************************************************************/
int
main(void)
{
long lEEPROMRetStatus;
uint16_t i=0;
uint8_t halted_latch = 0;
// Set the clocking to run at 80 MHz from the PLL.
// (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't
// write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we
// could slow the clock down when writing to FLASH but then we need to find out how long
// it takes for the clock to stabilize. This is on at the bottom of my list of things to do
// for now)
SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// Initialize the device pinout.
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
// Enable processor interrupts.
IntMasterEnable();
// Setup the UART's
my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// command_handler_init overwrites the baud rate. We still need to configure the pins though
my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// Enable the command handler
command_handler_init(); // We set the baud in here
// Start the timers
my_timer0_init();
my_timer1_init();
i2c_init();
motor_init();
qei_init();
gyro_init();
accel_init();
led_init();
//rc_radio_init();
//setupBluetooth();
// Initialize the EEPROM emulation region.
lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE);
if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14);
#if 0
// If ever we wanted to write some parameters to FLASH without the HMI
// we could do it here.
SoftEEPROMWriteDouble(kP_ID, 10.00);
SoftEEPROMWriteDouble(kI_ID, 10.00);
SoftEEPROMWriteDouble(kD_ID, 10.00);
SoftEEPROMWriteDouble(ANG_ID, 0.0);
SoftEEPROMWriteDouble(COMPC_ID, 0.99);
#endif
kP = SoftEEPROMReadDouble(kP_ID);
kI = SoftEEPROMReadDouble(kI_ID);
kD = SoftEEPROMReadDouble(kD_ID);
commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID);
COMP_C = SoftEEPROMReadDouble(COMPC_ID);
pid_init(kP, kI, kD, &pid_ang);
motor_controller_init(20, 100, 10, &mot_left);
motor_controller_init(20, 100, 10, &mot_right);
//pid_init(0.0, 0.0, 0.0, &pid_pos_left);
//pid_init(0.0, 0.0, 0.0, &pid_pos_right);
//UART0Send("Hello World!\n", 13);
// Tell the HMI what the initial parameters are.
print_params(1);
while(1)
{
delta_t = myTimerValueGet();
myTimerZero();
sum_delta_t += delta_t;
// Read our sensors
accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true);
gyro_get_y_cal(&gyro_y, false);
// Calculate the pitch angle with the accelerometer only
R = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang;
//accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0);
gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t);
// Kalman filter
//filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t));
filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang);
// Skip the rest of the process until the angle stabilizes
if(i < 250) { i++; continue; }
// Tell the HMI what's going on every 100ms
if(sum_delta_t >= 1000)
{
print_update(1);
print_debug(0);
//print_control_surfaces(0);
led_toggle();
//print_angle();
sum_delta_t = 0;
}
// See if the HMI has anything to say
command_handler();
//continue;
// If we are leaning more than +/- FALL_ANG deg off center it's hopeless.
// Turn off the motors in hopes of some damage control
if( abs(filtered_ang) > FALL_ANG )
{
if(halted_latch) continue;
stop_motors();
halted_latch = 1;
continue;
}
halted_latch = 0;
motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang);
motor_left = motor_right = motor_val;
drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain);
}
}
void timer1_callback()
{
led_toggle(1);
timer_post_task_delay(&timer1_callback, 0x0000FFFF + (uint32_t)100);
log_print_string("Toggled led %d", 1);
}
