void updateOWSensors() { #ifdef OW_DEBUG printf_P(PSTR("Update onewire \r\n")); #endif uint8_t subzero, cel, cel_frac_bits; uint16_t maalt; if (!DS18B20Conv) { DS18B20Conv = true; for (uint8_t active_OW_channel=1; active_OW_channel<=3; active_OW_channel++) { OW_selectPort(active_OW_channel); DS18X20_start_meas( DS18X20_POWER_PARASITE, NULL ); } } else { DS18B20Conv = false; for (uint8_t active_OW_channel=1; active_OW_channel<=3; active_OW_channel++) { OW_selectPort(active_OW_channel); #ifdef OW_DEBUG printf_P(PSTR("Scanner for onewire sensore paa %d\r\n"),active_OW_channel); #endif int nSensors = search_sensors(MAXSENSORS); //Finder alle sensore (op til max) #ifdef OW_DEBUG printf("Found %d sensors \r\n", nSensors); #endif for ( int i=0; i<nSensors; i++ ) { if (sensorScan[i*OW_ROMCODE_SIZE+0] == 0x10 || sensorScan[i*OW_ROMCODE_SIZE+0] == 0x28) { uint8_t sensorID[OW_ROMCODE_SIZE]; for (uint8_t o=0; o<OW_ROMCODE_SIZE; o++) { sensorID[o] = sensorScan[i*OW_ROMCODE_SIZE+o]; } if ( DS18X20_read_meas( sensorID, &subzero, &cel, &cel_frac_bits, &maalt) == DS18X20_OK ) { #ifdef OW_DEBUG int frac = cel_frac_bits*DS18X20_FRACCONV; //Ganger de sidste par bits, med det step DS18B20 bruger #endif char sign = (subzero) ? '-' : '+'; uint16_t pos = findSensor( sensorScan[i*OW_ROMCODE_SIZE+FAMILY], sensorScan[i*OW_ROMCODE_SIZE+ID1], sensorScan[i*OW_ROMCODE_SIZE+ID2], sensorScan[i*OW_ROMCODE_SIZE+ID3], sensorScan[i*OW_ROMCODE_SIZE+ID4], sensorScan[i*OW_ROMCODE_SIZE+ID5], sensorScan[i*OW_ROMCODE_SIZE+ID6], sensorScan[i*OW_ROMCODE_SIZE+CRC] ); sensorValues[(pos*SENSORSIZE)+VALUE1] = cel; sensorValues[(pos*SENSORSIZE)+VALUE2] = cel_frac_bits; sensorValues[(pos*SENSORSIZE)+SIGN] = sign; #ifdef OW_DEBUG printf_P(PSTR("Sensor# %d (%02X%02X%02X%02X%02X%02X%02X%02X) = : %c%d.%04d\r\n"),i+1, sensorValues[(pos*SENSORSIZE)+FAMILY], sensorValues[(pos*SENSORSIZE)+ID1], sensorValues[(pos*SENSORSIZE)+ID2], sensorValues[(pos*SENSORSIZE)+ID3], sensorValues[(pos*SENSORSIZE)+ID4], sensorValues[(pos*SENSORSIZE)+ID5], sensorValues[(pos*SENSORSIZE)+ID6], sensorValues[(pos*SENSORSIZE)+CRC], sensorValues[(pos*SENSORSIZE)+SIGN], sensorValues[(pos*SENSORSIZE)+VALUE1], frac ); #endif } else printf_P(PSTR("CRC Error (lost connection?)")); } } } } }
int16_t parse_cmd_c6_set(char *cmd, char *output, uint16_t len) { char *buf; buf = strrchr (cmd, ' '); if (!buf) return ECMD_ERR_PARSE_ERROR; *(buf ++) = 0; struct c6_vario_type varvalue; if (!control6_get(cmd, &varvalue)) return ECMD_ERR_PARSE_ERROR; if (varvalue.type == C6_TYPE_int8_t || varvalue.type == C6_TYPE_int16_t) { /* signed */ int16_t nv = strtol (buf, NULL, 0); int32_t nlv = strtol (buf, NULL, 0); if (varvalue.type == C6_TYPE_int8_t) { if (nv < INT8_MIN || nv > INT8_MAX) range_error: return ECMD_FINAL(snprintf_P(output, len, PSTR("range error."))); else varvalue.data.d_int8_t = nv; } else if (varvalue.type == C6_TYPE_int32_t) { varvalue.data.d_int32_t = nlv; } else varvalue.data.d_int16_t = nv; } else { /* unsigned */ if (*buf == '-') goto range_error; uint16_t nv = strtoul (buf, NULL, 0); uint32_t nlv = strtoul (buf, NULL, 0); if (varvalue.type == C6_TYPE_uint8_t) { if (nv > UINT8_MAX) goto range_error; else varvalue.data.d_uint8_t = nv; } else if (varvalue.type == C6_TYPE_uint32_t) { varvalue.data.d_uint32_t = nlv; } else varvalue.data.d_uint16_t = nv; } control6_set (cmd, varvalue); return ECMD_FINAL_OK; }
/** Event handler for the USB_DeviceAttached event. This indicates that a device has been attached to the host, and * starts the library USB task to begin the enumeration and USB management process. */ void EVENT_USB_Host_DeviceAttached(void) { puts_P(PSTR(ESC_FG_GREEN "Device Attached.\r\n" ESC_FG_WHITE)); LEDs_SetAllLEDs(LEDMASK_USB_ENUMERATING); }
int main(void){ uint16_t plen; uint16_t dat_p; uint8_t i=0; uint8_t cmd_pos=0; int8_t cmd; uint8_t payloadlen=0; char str[30]; char cmdval; // set the clock speed to "no pre-scaler" (8MHz with internal osc or // full external speed) // set the clock prescaler. First write CLKPCE to enable setting of clock the // next four instructions. CLKPR=(1<<CLKPCE); // change enable CLKPR=0; // "no pre-scaler" delay_ms(1); /* enable PD2/INT0, as input */ DDRD&= ~(1<<DDD2); /*initialize enc28j60*/ enc28j60Init(mymac); enc28j60clkout(2); // change clkout from 6.25MHz to 12.5MHz delay_ms(10); // LED /* enable PB1, LED as output */ DDRB|= (1<<DDB1); /* set output to Vcc, LED off */ PORTB|= (1<<PB1); // the transistor on PD7 DDRD|= (1<<DDD7); PORTD &= ~(1<<PD7);// transistor off /* Magjack leds configuration, see enc28j60 datasheet, page 11 */ // LEDB=yellow LEDA=green // // 0x476 is PHLCON LEDA=links status, LEDB=receive/transmit // enc28j60PhyWrite(PHLCON,0b0000 0100 0111 01 10); enc28j60PhyWrite(PHLCON,0x476); delay_ms(20); /* set output to GND, red LED on */ PORTB &= ~(1<<PB1); i=1; //init the ethernet/ip layer: init_ip_arp_udp_tcp(mymac,myip,MYWWWPORT); while(1){ // get the next new packet: plen = enc28j60PacketReceive(BUFFER_SIZE, buf); /*plen will ne unequal to zero if there is a valid * packet (without crc error) */ if(plen==0){ continue; } // arp is broadcast if unknown but a host may also // verify the mac address by sending it to // a unicast address. if(eth_type_is_arp_and_my_ip(buf,plen)){ make_arp_answer_from_request(buf); continue; } // check if ip packets are for us: if(eth_type_is_ip_and_my_ip(buf,plen)==0){ continue; } // led---------- if (i){ /* set output to Vcc, LED off */ PORTB|= (1<<PB1); i=0; }else{ /* set output to GND, LED on */ PORTB &= ~(1<<PB1); i=1; } if(buf[IP_PROTO_P]==IP_PROTO_ICMP_V && buf[ICMP_TYPE_P]==ICMP_TYPE_ECHOREQUEST_V){ // a ping packet, let's send pong make_echo_reply_from_request(buf,plen); continue; } // tcp port www start, compare only the lower byte if (buf[IP_PROTO_P]==IP_PROTO_TCP_V&&buf[TCP_DST_PORT_H_P]==0&&buf[TCP_DST_PORT_L_P]==MYWWWPORT){ if (buf[TCP_FLAGS_P] & TCP_FLAGS_SYN_V){ make_tcp_synack_from_syn(buf); // make_tcp_synack_from_syn does already send the syn,ack continue; } if (buf[TCP_FLAGS_P] & TCP_FLAGS_ACK_V){ init_len_info(buf); // init some data structures // we can possibly have no data, just ack: dat_p=get_tcp_data_pointer(); if (dat_p==0){ if (buf[TCP_FLAGS_P] & TCP_FLAGS_FIN_V){ // finack, answer with ack make_tcp_ack_from_any(buf); } // just an ack with no data, wait for next packet continue; } if (strncmp("GET ",(char *)&(buf[dat_p]),4)!=0){ // head, post and other methods: // // for possible status codes see: // http://www.w3.org/Protocols/rfc2616/rfc2616-sec10.html plen=fill_tcp_data_p(buf,0,PSTR("HTTP/1.0 200 OK\r\nContent-Type: text/html\r\n\r\n<h1>200 OK</h1>")); goto SENDTCP; } if (strncmp("/ ",(char *)&(buf[dat_p+4]),2)==0){ plen=fill_tcp_data_p(buf,0,PSTR("HTTP/1.0 200 OK\r\nContent-Type: text/html\r\n\r\n")); plen=fill_tcp_data_p(buf,plen,PSTR("<p>Usage: http://host_or_ip/password</p>\n")); goto SENDTCP; } cmd=analyse_get_url((char *)&(buf[dat_p+5])); // for possible status codes see: // http://www.w3.org/Protocols/rfc2616/rfc2616-sec10.html if (cmd==-1){ plen=fill_tcp_data_p(buf,0,PSTR("HTTP/1.0 401 Unauthorized\r\nContent-Type: text/html\r\n\r\n<h1>401 Unauthorized</h1>")); goto SENDTCP; } if (cmd==1){ PORTD|= (1<<PD7);// transistor on } if (cmd==0){ PORTD &= ~(1<<PD7);// transistor off } if (cmd==-3){ // redirect to add a trailing slash plen=moved_perm(buf); goto SENDTCP; } // if (cmd==-2) or any other value // just display the status: plen=print_webpage(buf,(PORTD & (1<<PD7))); // SENDTCP: make_tcp_ack_from_any(buf); // send ack for http get make_tcp_ack_with_data(buf,plen); // send data continue; } } // tcp port www end // // udp start, we listen on udp port 1200=0x4B0 if (buf[IP_PROTO_P]==IP_PROTO_UDP_V&&buf[UDP_DST_PORT_H_P]==4&&buf[UDP_DST_PORT_L_P]==0xb0){ payloadlen=buf[UDP_LEN_L_P]-UDP_HEADER_LEN; // you must sent a string starting with v // e.g udpcom version 10.0.0.24 if (verify_password((char *)&(buf[UDP_DATA_P]))){ // find the first comma which indicates // the start of a command: cmd_pos=0; while(cmd_pos<payloadlen){ cmd_pos++; if (buf[UDP_DATA_P+cmd_pos]==','){ cmd_pos++; // put on start of cmd break; } } // a command is one char and a value. At // least 3 characters long. It has an '=' on // position 2: if (cmd_pos<2 || cmd_pos>payloadlen-3 || buf[UDP_DATA_P+cmd_pos+1]!='='){ strcpy(str,"e=no_cmd"); goto ANSWER; } // supported commands are // t=1 t=0 t=? if (buf[UDP_DATA_P+cmd_pos]=='t'){ cmdval=buf[UDP_DATA_P+cmd_pos+2]; if(cmdval=='1'){ PORTD|= (1<<PD7);// transistor on strcpy(str,"t=1"); goto ANSWER; }else if(cmdval=='0'){ PORTD &= ~(1<<PD7);// transistor off strcpy(str,"t=0"); goto ANSWER; }else if(cmdval=='?'){ if (PORTD & (1<<PD7)){ strcpy(str,"t=1"); goto ANSWER; } strcpy(str,"t=0"); goto ANSWER; } } strcpy(str,"e=no_such_cmd"); goto ANSWER; } strcpy(str,"e=invalid_pw"); ANSWER: make_udp_reply_from_request(buf,str,strlen(str),MYUDPPORT); } } return (0); }
/** * \b main_thread_func * * Entry point for main application thread. * * This is the first thread that will be executed when the OS is started. * * @param[in] data Unused (optional thread entry parameter) * * @return None */ static void main_thread_func (uint32_t data) { uint32_t test_status; int sleep_ticks; /* Enable all LEDs (STK500-specific) */ DDRB = 0xFF; PORTB = 0xFF; /* Initialise UART (9600bps) */ if (uart_init(9600) != 0) { /* Error initialising UART */ } /** * Redirect stdout via the UART. Note that the UART write routine * is protected via a semaphore, so the OS must be started before * use of the UART. */ stdout = &uart_stdout; /* Put a message out on the UART */ printf_P (PSTR("Go\n")); /* Start test. All tests use the same start API. */ test_status = 0; /* Check main thread stack usage (if enabled) */ #ifdef ATOM_STACK_CHECKING if (test_status == 0) { uint32_t used_bytes, free_bytes; /* Check idle thread stack usage */ if (atomThreadStackCheck (&main_tcb, &used_bytes, &free_bytes) == ATOM_OK) { /* Check the thread did not use up to the end of stack */ if (free_bytes == 0) { printf_P (PSTR("Main stack overflow\n")); test_status++; } /* Log the stack usage */ #ifdef TESTS_LOG_STACK_USAGE printf_P (PSTR("MainUse:%d\n"), used_bytes); #endif } } #endif /* Log final status */ if (test_status == 0) { printf_P (PSTR("Pass\n")); } else { printf_P (PSTR("Fail(%d)\n"), test_status); } /* Flash LED once per second if passed, very quickly if failed */ sleep_ticks = (test_status == 0) ? SYSTEM_TICKS_PER_SEC : (SYSTEM_TICKS_PER_SEC/8); /* Test finished, flash slowly for pass, fast for fail */ while (1) { /* Toggle a LED (STK500-specific) */ PORTB ^= (1 << 7); /* Sleep then toggle LED again */ atomTimerDelay (sleep_ticks); } }
void AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { Vector3f last_average, best_avg; float ins_gyro[3]; float best_diff = 0; // cold start delay_cb(100); Serial.printf_P(PSTR("Init Gyro")); for(int c = 0; c < 25; c++) { // Mostly we are just flashing the LED's here // to tell the user to keep the IMU still FLASH_LEDS(true); delay_cb(20); _ins->update(); _ins->get_gyros(ins_gyro); FLASH_LEDS(false); delay_cb(20); } // the strategy is to average 200 points over 1 second, then do it // again and see if the 2nd average is within a small margin of // the first last_average.zero(); // we try to get a good calibration estimate for up to 10 seconds // if the gyros are stable, we should get it in 2 seconds for (int j = 0; j <= 10; j++) { Vector3f gyro_sum, gyro_avg, gyro_diff; float diff_norm; uint8_t i; Serial.printf_P(PSTR("*")); gyro_sum.zero(); for (i=0; i<200; i++) { _ins->update(); _ins->get_gyros(ins_gyro); gyro_sum += Vector3f(ins_gyro[0], ins_gyro[1], ins_gyro[2]); if (i % 40 == 20) { FLASH_LEDS(true); } else if (i % 40 == 0) { FLASH_LEDS(false); } delay_cb(5); } gyro_avg = gyro_sum / i; gyro_diff = last_average - gyro_avg; diff_norm = gyro_diff.length(); if (j == 0) { best_diff = diff_norm; best_avg = gyro_avg; } else if (gyro_diff.length() < ToRad(0.04)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average = (gyro_avg * 0.5) + (last_average * 0.5); _sensor_cal[0] = last_average.x; _sensor_cal[1] = last_average.y; _sensor_cal[2] = last_average.z; // all done return; } else if (diff_norm < best_diff) { best_diff = diff_norm; best_avg = (gyro_avg * 0.5) + (last_average * 0.5); } last_average = gyro_avg; } // we've kept the user waiting long enough - use the best pair we // found so far Serial.printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff)); _sensor_cal[0] = best_avg.x; _sensor_cal[1] = best_avg.y; _sensor_cal[2] = best_avg.z; }
// // print_flight_mode - prints flight mode to serial port. // void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) { switch (mode) { case STABILIZE: port->print_P(PSTR("STABILIZE")); break; case ACRO: port->print_P(PSTR("ACRO")); break; case ALT_HOLD: port->print_P(PSTR("ALT_HOLD")); break; case AUTO: port->print_P(PSTR("AUTO")); break; case GUIDED: port->print_P(PSTR("GUIDED")); break; case LOITER: port->print_P(PSTR("LOITER")); break; case RTL: port->print_P(PSTR("RTL")); break; case CIRCLE: port->print_P(PSTR("CIRCLE")); break; case LAND: port->print_P(PSTR("LAND")); break; case OF_LOITER: port->print_P(PSTR("OF_LOITER")); break; case DRIFT: port->print_P(PSTR("DRIFT")); break; case SPORT: port->print_P(PSTR("SPORT")); break; case FLIP: port->print_P(PSTR("FLIP")); break; case AUTOTUNE: port->print_P(PSTR("AUTOTUNE")); break; case POSHOLD: port->print_P(PSTR("POSHOLD")); break; case BRAKE: port->print_P(PSTR("BRAKE")); break; default: port->printf_P(PSTR("Mode(%u)"), (unsigned)mode); break; } }
/*---------------------------------------------------------------------------*/ int main(void) { uint16_t rval; /* init hardware layer */ init_hci(); timer1_init(); init_serial(); sei(); led1_high(); led2_high(); /* init protothreads */ PT_INIT(&blink_pt); PT_INIT(&nrf24_pt); PT_INIT(&www_client_pt); PT_INIT(&www_server_pt); PT_INIT(&udp_server_pt); PT_INIT(&coap_server_pt); PT_INIT(&temperature_pt); PT_INIT(&udp_broadcast_pt); /* greeting message */ dbg(PSTR("> Hello World!\r\n")); /* init the ethernet chip */ enc28j60Init(mymac); enc28j60PhyWrite(PHLCON,0x476); /* get automatic IP */ rval=0; init_mac(mymac); while(rval==0) { plen=enc28j60PacketReceive(BUFFER_SIZE, buf); rval=packetloop_dhcp_initial_ip_assignment(buf,plen,mymac[5]); } dhcp_get_my_ip(myip,netmask,gwip); client_ifconfig(myip,netmask); /* learn the MAC address of the gateway */ get_mac_with_arp(gwip,0,&arpresolver_result_callback); while(get_mac_with_arp_wait()) { plen=enc28j60PacketReceive(BUFFER_SIZE, buf); packetloop_arp_icmp_tcp(buf,plen); } /* set WWW server port */ www_server_port(MYWWWPORT); dbg(PSTR("> System is ready.\r\n")); led1_low(); led2_low(); /* main loop */ while(1) { if(enc28j60linkup()) { /* poll hardware ethernet buffer */ plen = enc28j60PacketReceive(BUFFER_SIZE,buf); /* terminate the buffer */ buf[BUFFER_SIZE]='\0'; /* handle DHCP messages if neccessary */ plen = packetloop_dhcp_renewhandler(buf,plen); /* handle and analyse the packet slightly */ dat_p = packetloop_arp_icmp_tcp(buf,plen); if( dat_p == 0) { udp_client_check_for_dns_answer(buf,plen); } new_packet = 0xFF; PT_SCHEDULE(blink_thread(&blink_pt)); PT_SCHEDULE(nrf24_thread(&nrf24_pt)); PT_SCHEDULE(www_server_thread(&www_server_pt)); PT_SCHEDULE(udp_server_thread(&udp_server_pt)); PT_SCHEDULE(www_client_thread(&www_client_pt)); PT_SCHEDULE(coap_server_thread(&coap_server_pt)); PT_SCHEDULE(temperature_thread(&temperature_pt)); PT_SCHEDULE(udp_broadcast_thread(&udp_broadcast_pt)); } } return 0; }
/* Process command */ void processCmd(uint8_t cmd) { switch(cmd) { case '?': printf_P(PSTR("Commands are:\n\r")); printf_P(PSTR("? = Print available commands\n\r")); printf_P(PSTR("V = Print firmware version\n\r")); printf_P(PSTR("0 = Clear buffers\n\r")); printf_P(PSTR("1 = Dump raw ISO Stripe 1 bit buffer\n\r")); printf_P(PSTR("2 = Dump raw ISO Stripe 2 bit buffer\n\r")); printf_P(PSTR("3 = Dump raw ISO Stripe 3 bit buffer\n\r")); printf_P(PSTR("4 = Flip magstripe buffers\n\r")); printf_P(PSTR("5 = Decode ISO Stripe 1 data\n\r")); printf_P(PSTR("6 = Decode ISO Stripe 2 data\n\r")); printf_P(PSTR("7 = Decode ISO Stripe 3 data\n\r")); printf_P(PSTR("p = Switch USEROUT1 to low\n\r")); printf_P(PSTR("q = Switch USEROUT2 to low\n\r")); printf_P(PSTR("r = Switch USEROUT3 to low\n\r")); printf_P(PSTR("P = Switch USEROUT1 to high\n\r")); printf_P(PSTR("Q = Switch USEROUT2 to high\n\r")); printf_P(PSTR("R = Switch USEROUT3 to high\n\r")); printf_P(PSTR("u = Read USERIN1 status\n\r")); printf_P(PSTR("v = Read USERIN2 status\n\r")); printf_P(PSTR("w = Read USERIN3 status\n\r")); printf_P(PSTR("x = Disable USERIN1 pullup\n\r")); printf_P(PSTR("y = Disable USERIN2 pullup\n\r")); printf_P(PSTR("z = Disable USERIN3 pullup\n\r")); printf_P(PSTR("X = Enable USERIN1 pullup\n\r")); printf_P(PSTR("Y = Enable USERIN2 pullup\n\r")); printf_P(PSTR("Z = Enable USERIN3 pullup\n\r")); break; case 'V': printf_P(PSTR("1.1.0§\n\r"),magstripe3Data); break; /* Magstripe commands */ case '0': magstripeInit(); printf_P(PSTR("§\n\r")); break; case '1': magstripePrintRaw(1); printf_P(PSTR("§\n\r")); break; case '2': magstripePrintRaw(2); printf_P(PSTR("§\n\r")); break; case '3': magstripePrintRaw(3); printf_P(PSTR("§\n\r")); break; case '4': magstripeFlip(); printf_P(PSTR("§\n\r")); break; case '5': magstripeDecode(1); printf_P(PSTR("%s§\n\r"),magstripeData); break; case '6': magstripeDecode(2); printf_P(PSTR("%s§\n\r"),magstripeData); break; case '7': magstripeDecode(3); printf_P(PSTR("%s§\n\r"),magstripeData); break; /* GPIO Commands */ case 'p': userout(1,0); printf_P(PSTR("§\n\r")); break; case 'q': userout(2,0); printf_P(PSTR("§\n\r")); break; case 'r': userout(3,0); printf_P(PSTR("§\n\r")); break; case 'P': userout(1,1); printf_P(PSTR("§\n\r")); break; case 'Q': userout(2,1); printf_P(PSTR("§\n\r")); break; case 'R': userout(3,1); printf_P(PSTR("§\n\r")); break; case 'u': printf_P(PSTR("%i§\n\r"),userin(1)); break; case 'v': printf_P(PSTR("%i§\n\r"),userin(2)); break; case 'w': printf_P(PSTR("%i§\n\r"),userin(3)); break; case 'x': userinPullup(1,0); printf_P(PSTR("§\n\r")); break; case 'y': userinPullup(2,0); printf_P(PSTR("§\n\r")); break; case 'z': userinPullup(3,0); printf_P(PSTR("§\n\r")); break; case 'X': userinPullup(1,1); printf_P(PSTR("§\n\r")); break; case 'Y': userinPullup(2,1); printf_P(PSTR("§\n\r")); break; case 'Z': userinPullup(3,1); printf_P(PSTR("§\n\r")); break; default: printf_P(PSTR("\n\r"),cmd); break; } }
/*---------------------------------------------------------------------------*/ uint8_t return_counter() { dbg(PSTR("> %d\r\n"),www_client_counter); return 0; }
/*---------------------------------------------------------------------------*/ static PT_THREAD(www_client_thread(struct pt *pt)) { PT_BEGIN(pt); while(1) { /* TODO: Add debouncing ... */ PT_WAIT_UNTIL(pt,btn2_pressed()); PT_WAIT_UNTIL(pt,!btn2_pressed()); dbg(PSTR("> Web client starts.\r\n")); retry_count = 0; do { /* disable interrupts */ cli(); /* reset timeout counter */ www_client_counter = 0; /* re-enable interrupts */ sei(); /* make a dns request */ dnslkup_request(buf,URL_base_address,gwmac); /* wait an answer ... */ PT_WAIT_UNTIL(pt,dnslkup_haveanswer()||www_client_timeout()); if(dnslkup_haveanswer()) { /* exit the loop */ retry_count = 0xFF; } else { dbg(PSTR("> DNS lookup timeout!\r\n")); retry_count++; } } while(retry_count < 5); if(dnslkup_haveanswer()) { /* save the received IP to your userspace variable */ dnslkup_get_ip(otherside_www_ip); /* reset callback state */ www_callback_state = 0; /* reset retry count */ retry_count = 0; /* browsing action */ do { /* disable interrupts */ cli(); /* reset timeout counter */ www_client_counter = 0; /* re-enable interrupts */ sei(); /* increment the trial counter */ retry_count++; client_browse_url( PSTR("/ws.php?c="), /* constant part of the URL suffix */ "Samsun", /* variable part of the URL which comes after the constant suffix */ URL_base_address, /* base-name of the web page we want to browse */ &browserresult_callback, /* callback function for our URL browsing attempt */ otherside_www_ip, /* IP representation of the webpage host */ gwmac /* mac address of our gateway */ ); /* wait ... */ PT_WAIT_UNTIL(pt,www_client_failed() || www_client_timeout() || www_client_ok() || return_counter() ); if(www_client_ok()) { /* exit the loop */ retry_count = 0xFF; } else if(www_client_failed()) { dbg(PSTR("> Browsing problem!\r\n")); } else if(www_client_timeout()) { dbg(PSTR("> Browsing timeout!\r\n")); } } while(retry_count < 5); if(retry_count == 0xFF) { dbg(PSTR("> Browsing went ok!\r\n")); } else { dbg(PSTR("> Browsing failed!\r\n")); } dbg(PSTR("> -------------------------------------\r\n")); } else { dbg(PSTR("> Browsing failed!\r\n")); dbg(PSTR("> -------------------------------------\r\n")); } } PT_END(pt); }
/*---------------------------------------------------------------------------*/ void browserresult_callback(uint16_t webstatuscode,uint16_t datapos,uint16_t len) { #if 1 dbg(PSTR("> -------------------------------------\r\n")); dbg(PSTR("> Status code: %d\r\n"),webstatuscode); dbg(PSTR("> Datapos: %d\r\n"),datapos); dbg(PSTR("> Len: %d\r\n"),len); dbg(PSTR("> -------------------------------------\r\n")); #endif #if 1 uint16_t q; dbg(PSTR("> Complete HTTP message: \r\n")); dbg(PSTR("> -------------------------------------\r\n")); for(q=datapos;q<(datapos+len);q++) { dbg(PSTR("%c"),buf[q]); } dbg(PSTR("\r\r> -------------------------------------\r\n")); #endif #if 1 uint16_t qx; uint16_t qxx; dbg(PSTR("> -------------------------------------\r\n")); dbg(PSTR("> Returned actual message: \r\n")); dbg(PSTR("> -------------------------------------\r\n")); /* This loop finds the location of actual data that comes with the HTTP response */ for(qx=datapos;qx<(datapos+len-4);qx++) { if((buf[qx+0] == '\r') && (buf[qx+1] == '\n') && (buf[qx+2] == '\r') && (buf[qx+3] == '\n')) { for (qxx = qx+4; qxx < datapos+len; ++qxx) { /* print the actual contents */ dbg(PSTR("%c"),buf[qxx]); /* break the loop */ qx = 0xFFF0; } } } dbg(PSTR("> -------------------------------------\r\n")); #endif www_callback_state = 1; }
void look_for_lines_to_connect() { float sx, sy, ex, ey; for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { if (i < GRID_MAX_POINTS_X) { // We can't connect to anything to the right than GRID_MAX_POINTS_X. // This is already a half circle because we are at the edge of the bed. if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i + 1, j)) { // check if we can do a line to the left if (!is_bit_set(horizontal_mesh_line_flags, i, j)) { // // We found two circles that need a horizontal line to connect them // Print it! // sx = pgm_read_float(&ubl.mesh_index_to_xpos[ i ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); if (ubl.g26_debug_flag) { SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(", ey=", ey); SERIAL_CHAR(')'); SERIAL_EOL; //debug_current_and_destination(PSTR("Connecting horizontal line.")); } print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again } } if (j < GRID_MAX_POINTS_Y) { // We can't connect to anything further back than GRID_MAX_POINTS_Y. // This is already a half circle because we are at the edge of the bed. if (is_bit_set(circle_flags, i, j) && is_bit_set(circle_flags, i, j + 1)) { // check if we can do a line straight down if (!is_bit_set( vertical_mesh_line_flags, i, j)) { // // We found two circles that need a vertical line to connect them // Print it! // sy = pgm_read_float(&ubl.mesh_index_to_ypos[ j ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); if (ubl.g26_debug_flag) { SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(", ey=", ey); SERIAL_CHAR(')'); SERIAL_EOL; debug_current_and_destination(PSTR("Connecting vertical line.")); } print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again } } } } } } }
/** * G26: Mesh Validation Pattern generation. * * Used to interactively edit UBL's Mesh by placing the * nozzle in a problem area and doing a G29 P4 R command. */ void gcode_G26() { SERIAL_ECHOLNPGM("G26 command started. Waiting for heater(s)."); float tmp, start_angle, end_angle; int i, xi, yi; mesh_index_pair location; // Don't allow Mesh Validation without homing first, // or if the parameter parsing did not go OK, abort if (axis_unhomed_error(true, true, true) || parse_G26_parameters()) return; if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) { do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); stepper.synchronize(); set_current_to_destination(); } if (turn_on_heaters()) goto LEAVE; current_position[E_AXIS] = 0.0; sync_plan_position_e(); if (prime_flag && prime_nozzle()) goto LEAVE; /** * Bed is preheated * * Nozzle is at temperature * * Filament is primed! * * It's "Show Time" !!! */ ZERO(circle_flags); ZERO(horizontal_mesh_line_flags); ZERO(vertical_mesh_line_flags); // Move nozzle to the specified height for the first layer set_destination_to_current(); destination[Z_AXIS] = layer_height; move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0); move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], ooze_amount); ubl.has_control_of_lcd_panel = true; //debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern.")); /** * Declare and generate a sin() & cos() table to be used during the circle drawing. This will lighten * the CPU load and make the arc drawing faster and more smooth */ float sin_table[360 / 30 + 1], cos_table[360 / 30 + 1]; for (i = 0; i <= 360 / 30; i++) { cos_table[i] = SIZE_OF_INTERSECTION_CIRCLES * cos(RADIANS(valid_trig_angle(i * 30.0))); sin_table[i] = SIZE_OF_INTERSECTION_CIRCLES * sin(RADIANS(valid_trig_angle(i * 30.0))); } do { if (ubl_lcd_clicked()) { // Check if the user wants to stop the Mesh Validation #if ENABLED(ULTRA_LCD) lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), 99); lcd_quick_feedback(); #endif while (!ubl_lcd_clicked()) { // Wait until the user is done pressing the idle(); // Encoder Wheel if that is why we are leaving lcd_reset_alert_level(); lcd_setstatuspgm(PSTR("")); } while (ubl_lcd_clicked()) { // Wait until the user is done pressing the idle(); // Encoder Wheel if that is why we are leaving lcd_setstatuspgm(PSTR("Unpress Wheel"), 99); } goto LEAVE; } location = continue_with_closest ? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS]) : find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. if (location.x_index >= 0 && location.y_index >= 0) { const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem #ifdef DELTA if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) { SERIAL_ERROR_START; SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS."); goto LEAVE; } #endif // TODO: Change this to use `position_is_reachable` if (!WITHIN(circle_x, X_MIN_POS, X_MAX_POS) || !WITHIN(circle_y, Y_MIN_POS, Y_MAX_POS)) { SERIAL_ERROR_START; SERIAL_ERRORLNPGM("Attempt to print off the bed."); goto LEAVE; } xi = location.x_index; // Just to shrink the next few lines and make them easier to understand yi = location.y_index; if (ubl.g26_debug_flag) { SERIAL_ECHOPAIR(" Doing circle at: (xi=", xi); SERIAL_ECHOPAIR(", yi=", yi); SERIAL_CHAR(')'); SERIAL_EOL; } start_angle = 0.0; // assume it is going to be a full circle end_angle = 360.0; if (xi == 0) { // Check for bottom edge start_angle = -90.0; end_angle = 90.0; if (yi == 0) // it is an edge, check for the two left corners start_angle = 0.0; else if (yi == GRID_MAX_POINTS_Y - 1) end_angle = 0.0; } else if (xi == GRID_MAX_POINTS_X - 1) { // Check for top edge start_angle = 90.0; end_angle = 270.0; if (yi == 0) // it is an edge, check for the two right corners end_angle = 180.0; else if (yi == GRID_MAX_POINTS_Y - 1) start_angle = 180.0; } else if (yi == 0) { start_angle = 0.0; // only do the top side of the cirlce end_angle = 180.0; } else if (yi == GRID_MAX_POINTS_Y - 1) { start_angle = 180.0; // only do the bottom side of the cirlce end_angle = 360.0; } for (tmp = start_angle; tmp < end_angle - 0.1; tmp += 30.0) { int tmp_div_30 = tmp / 30.0; if (tmp_div_30 < 0) tmp_div_30 += 360 / 30; if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30; float x = circle_x + cos_table[tmp_div_30], // for speed, these are now a lookup table entry y = circle_y + sin_table[tmp_div_30], xe = circle_x + cos_table[tmp_div_30 + 1], ye = circle_y + sin_table[tmp_div_30 + 1]; #ifdef DELTA if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of continue; // the 'circle' is on the bed. If #else // not, we need to skip x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1); xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1); ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1); #endif //if (ubl.g26_debug_flag) { // char ccc, *cptr, seg_msg[50], seg_num[10]; // strcpy(seg_msg, " segment: "); // strcpy(seg_num, " \n"); // cptr = (char*) "01234567890ABCDEF????????"; // ccc = cptr[tmp_div_30]; // seg_num[1] = ccc; // strcat(seg_msg, seg_num); // debug_current_and_destination(seg_msg); //} print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), layer_height); } //debug_current_and_destination(PSTR("Looking for lines to connect.")); look_for_lines_to_connect(); //debug_current_and_destination(PSTR("Done with line connect.")); } //debug_current_and_destination(PSTR("Done with current circle.")); } while (location.x_index >= 0 && location.y_index >= 0); LEAVE: lcd_reset_alert_level(); lcd_setstatuspgm(PSTR("Leaving G26")); retract_filament(destination); destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; //debug_current_and_destination(PSTR("ready to do Z-Raise.")); move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle //debug_current_and_destination(PSTR("done doing Z-Raise.")); destination[X_AXIS] = x_pos; // Move back to the starting position destination[Y_AXIS] = y_pos; //destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position //debug_current_and_destination(PSTR("done doing X/Y move.")); ubl.has_control_of_lcd_panel = false; // Give back control of the LCD Panel! if (!keep_heaters_on) { #if HAS_TEMP_BED thermalManager.setTargetBed(0); #endif thermalManager.setTargetHotend(0, 0); } }
int TembooChoreo::run(IPAddress addr, uint16_t port) { m_nextChar = NULL; if (m_accountName == NULL || *m_accountName == '\0') { return TEMBOO_ERROR_ACCOUNT_MISSING; } if (m_path == NULL || *m_path == '\0') { return TEMBOO_ERROR_CHOREO_MISSING; } if (m_appKeyName == NULL || *m_appKeyName == '\0') { return TEMBOO_ERROR_APPKEY_NAME_MISSING; } if (m_appKeyValue == NULL || *m_appKeyValue == '\0') { return TEMBOO_ERROR_APPKEY_MISSING; } TembooSession session(m_client, addr, port); uint16_t httpCode = 0; for (int i = 0; i < 2; i++) { if (0 != session.executeChoreo(m_accountName, m_appKeyName, m_appKeyValue, m_path, m_inputs, m_outputs, m_preset)) { httpCode = 0; break; } while(!m_client.available()) { if (!m_client.connected()) { TEMBOO_TRACELN("Disconnected"); return TEMBOO_ERROR_HTTP_ERROR; } delay(10); } if (!m_client.find("HTTP/1.1 ")) { TEMBOO_TRACELN("No HTTP"); return TEMBOO_ERROR_HTTP_ERROR; } httpCode = (uint16_t)m_client.parseInt(); // We expect HTTP response codes to be <= 599, but // we need to be prepared for anything. if (httpCode >= 600) { TEMBOO_TRACELN("Invalid HTTP"); httpCode = 0; } // if we get an auth error AND there was an x-temboo-time header, // update the session timeOffset if ((httpCode == 401) && (i == 0)) { if (m_client.find("x-temboo-time:")) { TembooSession::setTime((unsigned long)m_client.parseInt()); while(m_client.available()) { m_client.read(); } m_client.stop(); } } else { break; } } uint16toa(httpCode, m_httpCodeStr); strcat_P(m_httpCodeStr, PSTR("\x0A\x1E")); m_nextState = START; m_nextChar = HTTP_CODE; if (httpCode < 200 || httpCode >= 300) { return TEMBOO_ERROR_HTTP_ERROR; } if (!m_client.find("\x0D\x0A\x0D\x0A")) { return TEMBOO_ERROR_HTTP_ERROR; } return TEMBOO_ERROR_OK; }
/** Mount the SD card. */ void sd_mount(void) { result = pf_mount(&sdfile); if (result != FR_OK) sersendf_P(PSTR("E: SD init failed. (%su)\n"), result); }
void AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { int flashcount = 0; float adc_in; float prev[6] = {0,0,0}; float total_change; float max_offset; float ins_accel[3]; // cold start delay_cb(500); Serial.printf_P(PSTR("Init Accel")); for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time do { _ins->update(); _ins->get_accels(ins_accel); for (int j = 3; j <= 5; j++){ prev[j] = _sensor_cal[j]; adc_in = ins_accel[j-3]; _sensor_cal[j] = adc_in; } for(int i = 0; i < 50; i++){ // We take some readings... delay_cb(20); _ins->update(); _ins->get_accels(ins_accel); for (int j = 3; j < 6; j++){ adc_in = ins_accel[j-3]; _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; } if(flashcount == 5) { Serial.printf_P(PSTR("*")); FLASH_LEDS(true); } if(flashcount >= 10) { flashcount = 0; FLASH_LEDS(false); } flashcount++; } // null gravity from the Z accel _sensor_cal[5] += 9.805; total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]); max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4]; max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5]; delay_cb(500); } while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset); Serial.printf_P(PSTR(" ")); }
/** Open a file for reading. \param filename Name of the file to open and to read G-code from. Before too long this will cause the printer to read G-code from this file until done or until stopped by G-code coming in over the serial line. */ void sd_open(const char* filename) { result = pf_open(filename); if (result != FR_OK) { sersendf_P(PSTR("E: failed to open file. (%su)\n"), result); } }
/** * \brief Der Temp_Json Thread, wird zyklisch aufgerufen. * \return void */ void temp_json_thread( void ) { int retval = LOGGER_ERROR; int Temp=0x8000; static char Sensor=0; char FILENAME[32]; struct TEMP_JSON temp_json; temp_json.logger_entry.DataSize = sizeof( temp_json.temp ); // Zeit holen struct TIME nowtime; CLOCK_GetTime ( &nowtime ); // Update ? if ( ( nowtime.mm % 10 == 0 ) && ( lastmm != nowtime.mm ) ) { Sensor = 0; lastmm = nowtime.mm ; } if( Sensor < TEMP_MAX_SENSORS ) { Temp = TEMP_readtemp( Sensor ); if ( Temp != TEMP_ERROR ) { // erzeuge mal einen Dummydatensatz temp_logger_clean( &temp_json ); // Dateinamen erzeigen zum Sensor sprintf_P( FILENAME, Templogfilename_P , Sensor ); // Versuche letzten Datensatz zu holen retval = LOGGER_getlastDBEntry ( &temp_json.logger_entry , FILENAME ); // Wenn letzter Eintrag nicht lesbar, neue NanoDB erzeugen if ( retval == LOGGER_ERROR ) { #if defined( DEBUG ) printf_P( PSTR("Last DBEntry failed. Make new nano_DB %s.\r\n"), FILENAME ); #endif // Neue NanoDB erzeugen retval = LOGGER_addDBentry( &temp_json.logger_entry , FILENAME ); } // Datenbankeintrag erzeugt oder gelesen ? if ( retval == LOGGER_OK ) { // letzer Eintrag von heute? if ( ! ( temp_json.logger_entry.time.YY == nowtime.YY && temp_json.logger_entry.time.MM == nowtime.MM && temp_json.logger_entry.time.DD == nowtime.DD ) ) { #if defined( DEBUG ) printf_P(PSTR("Add new DBEntry in %s.\r\n"), FILENAME ); #endif // Neue NanoDB erzeugen temp_logger_clean( &temp_json ); retval = LOGGER_addDBentry( &temp_json.logger_entry , FILENAME ); } } #if defined( DEBUG ) else printf_P(PSTR("Make new nano_DB %s failed.\r\n"), FILENAME ); #endif // alles Ok bis hier hin if ( retval == LOGGER_OK ) { #if defined( DEBUG ) printf_P(PSTR("Write actual DBEntry in %s.\r\n"), FILENAME ); #endif // Temp speichern temp_json.temp[ ( nowtime.hh * 6 ) + ( nowtime.mm / 10 ) ] = Temp; // und sichern in Datenbank LOGGER_writeDBEntry( &temp_json.logger_entry, FILENAME ); } } Sensor++; } }
void demo() { lcdCls(); for (uint8_t i = 0x20; i <= 128; ++i) { lcdChar(i, LCD_XOR); } lcdUpdate(); _delay_ms(2000); lcdStrPos (3,11); lcdStr_P(PSTR("Free string"), LCD_WHITE); lcdStrPos (22,37); lcdStr_P(PSTR("positioning"), LCD_WHITE); lcdUpdate(); _delay_ms(3000); lcdCls(); lcdStr_P(PSTR("Lines, rectangles, circles."), LCD_BLACK); lcdUpdate(); _delay_ms(1000); lcdNewLine(); lcdStr_P(PSTR("Filled or unfilled."), LCD_BLACK); lcdUpdate(); _delay_ms(1000); lcdNewLine(); lcdStr_P(PSTR("Solid or XORed filling, text."), LCD_BLACK); lcdUpdate(); _delay_ms(5000); lcdRect(0, 0, LCD_Y_RES, LCD_X_RES, LCD_BLACK, LCD_FILL_BLACK); lcdStrPos(36,0); lcdStr_P(PSTR("MENU"), LCD_XOR); lcdRect(5,7,LCD_Y_RES-8, LCD_X_RES-10,LCD_WHITE, LCD_FILL_WHITE ); for (int i = 1; i < 8; ++i) { lcdStrPos(10, i*8); lcdInt(i, LCD_BLACK); lcdStr_P(PSTR(". Sample"), LCD_BLACK); lcdLine(5,8*i-1,LCD_X_RES-10, LCD_HORIZ, LCD_BLACK); } lcdUpdate(); _delay_ms(2000); lcdCircle(LCD_X_RES/2,LCD_Y_RES/2,30, LCD_XOR, LCD_FILL_XOR); lcdUpdate(); _delay_ms(2000); lcdBresenhamLine(0, 0, LCD_X_RES-1, LCD_Y_RES-1, LCD_XOR); lcdUpdate(); _delay_ms(2000); lcdBresenhamLine(0, LCD_Y_RES-1, LCD_X_RES-1, 0, LCD_XOR); lcdUpdate(); _delay_ms(5000); lcdCls(); lcdStr_P(PSTR("Per-pixel horizontal fill speed test in:"), LCD_BLACK);lcdUpdate();lcdNewLine(); _delay_ms(1000); lcdStr_P(PSTR("3..."), LCD_BLACK);lcdUpdate(); _delay_ms(1000); lcdStr_P(PSTR("2..."), LCD_BLACK);lcdUpdate(); _delay_ms(1000); lcdStr_P(PSTR("1..."), LCD_BLACK);lcdUpdate(); _delay_ms(1000); for (uint8_t x = 0; x < LCD_X_RES; ++x) { for (uint8_t y = 0; y < LCD_Y_RES; ++y) { lcdPixel(x,y,LCD_PIXEL_XOR); lcdUpdate(); } } _delay_ms(1000); lcdNewLine(); lcdStr_P(PSTR("Vertical in:"), LCD_XOR);lcdUpdate();lcdNewLine(); _delay_ms(1000); lcdStr_P(PSTR("3..."), LCD_XOR);lcdUpdate(); _delay_ms(1000); lcdStr_P(PSTR("2..."), LCD_XOR);lcdUpdate(); _delay_ms(1000); lcdStr_P(PSTR("1..."), LCD_XOR);lcdUpdate(); _delay_ms(1000); for (uint8_t y = 0; y < LCD_Y_RES; ++y) { for (uint8_t x = 0; x < LCD_X_RES; ++x) { lcdPixel(x,y,LCD_PIXEL_XOR); lcdUpdate(); } } _delay_ms(2000); lcdCls(); lcdStr_P(PSTR("Every pixel was filled with it's XORed value and updated."), LCD_XOR);lcdUpdate(); _delay_ms(5000); lcdCls(); lcdStr_P(PSTR("That's it!"), LCD_XOR); lcdNewLine(); lcdStr_P(PSTR("Thanks!"), LCD_XOR);lcdUpdate(); _delay_ms(5000); }
void gui_set_autostart_item(uint8_t index, char * text, uint8_t * flags, char * sub_text) { switch (index) { case (0): sprintf_P(text, PSTR("State")); *flags |= GUI_LIST_SUB_TEXT; switch (fc.flight_state) { case(FLIGHT_WAIT): sprintf_P(sub_text, PSTR("Waiting")); break; case(FLIGHT_FLIGHT): sprintf_P(sub_text, PSTR("Flying")); break; case(FLIGHT_LAND): sprintf_P(sub_text, PSTR("Landed")); break; } break; case (1): sprintf_P(text, PSTR("Start threshold")); if (config.autostart.start_sensititvity > 0) sprintf_P(sub_text, PSTR("+/-%dm"), config.autostart.start_sensititvity); else sprintf_P(sub_text, PSTR("disabled")); *flags |= GUI_LIST_SUB_TEXT; break; case (2): sprintf_P(text, PSTR("Land threshold")); if (config.autostart.land_sensititvity > 0) sprintf_P(sub_text, PSTR("+/-%dm"), config.autostart.land_sensititvity); else sprintf_P(sub_text, PSTR("disabled")); *flags |= GUI_LIST_SUB_TEXT; break; case (3): sprintf_P(text, PSTR("Timeout")); sprintf_P(sub_text, PSTR("%d sec"), config.autostart.timeout); *flags |= GUI_LIST_SUB_TEXT; break; case (4): sprintf_P(text, PSTR("Suppress audio")); if (config.autostart.flags & AUTOSTART_SUPRESS_AUDIO) *flags |= GUI_LIST_CHECK_ON; else *flags |= GUI_LIST_CHECK_OFF; break; case (5): sprintf_P(text, PSTR("Record always")); if (config.autostart.flags & AUTOSTART_ALWAYS_ENABLED) *flags |= GUI_LIST_CHECK_ON; else *flags |= GUI_LIST_CHECK_OFF; break; } }
/** * * @param parent (not used) * @param port (not used) * @param lowspeed true if device is low speed * @return 0 for success */ uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) { uint8_t rcode; uint8_t num_of_conf = epInfo[1].epAddr; // number of configurations epInfo[1].epAddr = 0; USBTRACE("MS Init\r\n"); AddressPool &addrPool = pUsb->GetAddressPool(); UsbDevice *p = addrPool.GetUsbDevicePtr(bAddress); if (!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; // Assign new address to the device delay(2000); rcode = pUsb->setAddr(0, 0, bAddress); if (rcode) { p->lowspeed = false; addrPool.FreeAddress(bAddress); bAddress = 0; USBTRACE2("setAddr:", rcode); return rcode; } USBTRACE2("Addr:", bAddress); p->lowspeed = false; p = addrPool.GetUsbDevicePtr(bAddress); if (!p) return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; p->lowspeed = lowspeed; // Assign epInfo to epinfo pointer rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); if (rcode) goto FailSetDevTblEntry; USBTRACE2("NC:", num_of_conf); for (uint8_t i = 0; i < num_of_conf; i++) { ConfigDescParser< USB_CLASS_MASS_STORAGE, MASS_SUBCLASS_SCSI, MASS_PROTO_BBB, CP_MASK_COMPARE_CLASS | CP_MASK_COMPARE_SUBCLASS | CP_MASK_COMPARE_PROTOCOL > BulkOnlyParser(this); rcode = pUsb->getConfDescr(bAddress, 0, i, &BulkOnlyParser); if (rcode) goto FailGetConfDescr; if (bNumEP > 1) break; } // for if (bNumEP < 3) return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; // Assign epInfo to epinfo pointer pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); USBTRACE2("Conf:", bConfNum); // Set Configuration Value rcode = pUsb->setConf(bAddress, 0, bConfNum); if (rcode) goto FailSetConfDescr; //Linux does a 1sec delay after this. delay(1000); rcode = GetMaxLUN(&bMaxLUN); if (rcode) goto FailGetMaxLUN; if (bMaxLUN >= MASS_MAX_SUPPORTED_LUN) bMaxLUN = MASS_MAX_SUPPORTED_LUN - 1; ErrorMessage<uint8_t > (PSTR("MaxLUN"), bMaxLUN); delay(1000); // Delay a bit for slow firmware. //bTheLUN = bMaxLUN; for (uint8_t lun = 0; lun <= bMaxLUN; lun++) { InquiryResponse response; rcode = Inquiry(lun, sizeof (InquiryResponse), (uint8_t*) & response); if (rcode) { ErrorMessage<uint8_t > (PSTR("Inquiry"), rcode); } else { uint8_t tries = 0xf0; while (rcode = TestUnitReady(lun)) { if (rcode == 0x08) break; // break on no media, this is OK to do. // try to lock media and spin up if (tries < 14) { LockMedia(lun, 1); MediaCTL(lun, 1); // I actually have a USB stick that needs this! } else delay(2 * (tries + 1)); tries++; if (!tries) break; } if (!rcode) { delay(1000); LUNOk[lun] = CheckLUN(lun); if (!LUNOk[lun]) LUNOk[lun] = CheckLUN(lun); } } } #if 0 { bool good; for (uint8_t i = 1; i == 0; i++) { good = false; CheckMedia(); for (uint8_t lun = 0; lun <= bMaxLUN; lun++) good |= LUNOk[lun]; if (good) break; delay(118); // 255 loops =~ 30 seconds to allow for spin up, as per SCSI spec. } } #else CheckMedia(); #endif rcode = OnInit(); if (rcode) goto FailOnInit; USBTRACE("MS configured\r\n\r\n"); bPollEnable = true; //USBTRACE("Poll enabled\r\n"); return 0; FailSetConfDescr: #ifdef DEBUG_USB_HOST NotifyFailSetConfDescr(); goto Fail; #endif FailOnInit: #ifdef DEBUG_USB_HOST USBTRACE("OnInit:"); goto Fail; #endif FailGetMaxLUN: #ifdef DEBUG_USB_HOST USBTRACE("GetMaxLUN:"); goto Fail; #endif FailInvalidSectorSize: #ifdef DEBUG_USB_HOST USBTRACE("Sector Size is NOT VALID: "); goto Fail; #endif FailSetDevTblEntry: #ifdef DEBUG_USB_HOST NotifyFailSetDevTblEntry(); goto Fail; #endif FailGetConfDescr: #ifdef DEBUG_USB_HOST NotifyFailGetConfDescr(); #endif Fail: #ifdef DEBUG_USB_HOST NotifyFail(rcode); #endif Release(); return rcode; }
//Connect TCP continue function. void GSM3ShieldV1ClientProvider::connectTCPClientContinue() { bool resp; // 0: Dot or DNS notation activation // 1: Disable SW flow control // 2: Waiting for IFC OK // 3: Start-up TCP connection "AT+QIOPEN" // 4: Wait for connection OK // 5: Wait for CONNECT switch (theGSM3ShieldV1ModemCore.getCommandCounter()) { case 1: theGSM3ShieldV1ModemCore.genericCommand_rq(PSTR("AT+QIDNSIP="), false); if ((theGSM3ShieldV1ModemCore.getPhoneNumber()!=0)&& ((*(theGSM3ShieldV1ModemCore.getPhoneNumber())<'0')||((*(theGSM3ShieldV1ModemCore.getPhoneNumber())>'9')))) { theGSM3ShieldV1ModemCore.print('1'); theGSM3ShieldV1ModemCore.print('\r'); } else { theGSM3ShieldV1ModemCore.print('0'); theGSM3ShieldV1ModemCore.print('\r'); } theGSM3ShieldV1ModemCore.setCommandCounter(2); break; case 2: if(theGSM3ShieldV1ModemCore.genericParse_rsp(resp)) { //Response received if(resp) { // AT+QIOPEN theGSM3ShieldV1ModemCore.genericCommand_rq(PSTR("AT+QIOPEN="),false); theGSM3ShieldV1ModemCore.print("\"TCP\",\""); if(theGSM3ShieldV1ModemCore.getPhoneNumber()!=0) { theGSM3ShieldV1ModemCore.print(theGSM3ShieldV1ModemCore.getPhoneNumber()); } else { remoteIP.printTo(theGSM3ShieldV1ModemCore); } theGSM3ShieldV1ModemCore.print('"'); theGSM3ShieldV1ModemCore.print(','); theGSM3ShieldV1ModemCore.print(theGSM3ShieldV1ModemCore.getPort()); theGSM3ShieldV1ModemCore.print('\r'); theGSM3ShieldV1ModemCore.setCommandCounter(3); } else theGSM3ShieldV1ModemCore.closeCommand(3); } break; case 3: if(theGSM3ShieldV1ModemCore.genericParse_rsp(resp)) { // Response received if(resp) { // OK Received // Great. Go for the next step theGSM3ShieldV1ModemCore.setCommandCounter(4); } else theGSM3ShieldV1ModemCore.closeCommand(3); } break; case 4: char auxLocate [12]; prepareAuxLocate(PSTR("CONNECT\r\n"), auxLocate); if(theGSM3ShieldV1ModemCore.genericParse_rsp(resp,auxLocate)) { // Response received if(resp) { // Received CONNECT OK // Great. We're done theGSM3ShieldV1ModemCore.setStatus(TRANSPARENT_CONNECTED); theGSM3ShieldV1ModemCore.theBuffer().chopUntil(auxLocate, true); theGSM3ShieldV1ModemCore.closeCommand(1); } else theGSM3ShieldV1ModemCore.closeCommand(3); } break; } }
void gui_factory_test_loop() { gui_dialog_P(PSTR("Factory test")); bool blik = GUI_BLINK_TGL(1000); bool res; bool err = false; if (f_test_lcd) { char tmp[16]; disp.LoadFont(F_TEXT_L); switch (f_test_lcd) { case(FTEST_LCD_MIN_AUTO): strcpy_P(tmp, PSTR("Min auto")); break; case(FTEST_LCD_MIN): strcpy_P(tmp, PSTR("Set minimum")); break; case(FTEST_LCD_MAX_AUTO): strcpy_P(tmp, PSTR("Max auto")); break; case(FTEST_LCD_MAX): strcpy_P(tmp, PSTR("Set maximum")); break; case(FTEST_LCD_MID): strcpy_P(tmp, PSTR("Set optimal")); break; } gui_caligh_text(tmp, GUI_DISP_WIDTH / 2, GUI_DIALOG_TOP); disp.DrawLine(4, 24, 14, 24, 1); disp.DrawLine(4, 26, 14, 26, 1); disp.DrawLine(4, 28, 14, 28, 1); disp.DrawLine(4, 30, 14, 30, 1); disp.DrawLine(4, 32, 14, 32, 1); disp.DrawLine(16, 24, 16, 32, 1); disp.DrawLine(18, 24, 18, 32, 1); disp.DrawLine(20, 24, 20, 32, 1); disp.DrawLine(22, 24, 22, 32, 1); disp.DrawLine(24, 24, 24, 32, 1); disp.DrawRectangle(26, 24, 35, 33, 1, 1); disp.DrawRectangle(37, 24, 46, 33, 1, 0); disp.DrawRectangle(48, 24, 57, 33, 1, 0); disp.DrawRectangle(50, 26, 55, 31, 1, 0); disp.DrawRectangle(52, 28, 53, 29, 1, 0); disp.DrawCircle(71, 35, 1, 1); disp.DrawCircle(71, 35, 3, 1); disp.DrawCircle(71, 35, 5, 1); disp.DrawCircle(71, 35, 7, 1); disp.DrawCircle(71, 35, 9, 1); disp.LoadFont(F_TEXT_M); disp.GotoXY(4, 36); fprintf_P(lcd_out, PSTR("%03d"), f_test_lcd_cont_min); disp.GotoXY(24, 36); fprintf_P(lcd_out, PSTR("%03d"), f_test_lcd_cont); disp.GotoXY(44, 36); fprintf_P(lcd_out, PSTR("%03d"), f_test_lcd_cont_max); if (f_test_lcd == FTEST_LCD_MIN_AUTO || f_test_lcd == FTEST_LCD_MAX_AUTO) { f_test_lcd_cont = (f_test_lcd_cont + 1) % 128; } led_set(0, f_test_lcd_cont / 4, 0); if (f_test_lcd == FTEST_LCD_MID) disp.SetContrast(lcd_contrast_min + ((lcd_contrast_max - lcd_contrast_min) * f_test_lcd_cont) / GUI_CONTRAST_STEPS); else disp.SetContrast(f_test_lcd_cont); return; } //store contrast values eeprom_busy_wait(); eeprom_update_byte(&config_ro.lcd_contrast_max, f_test_lcd_cont_max); eeprom_update_byte(&config_ro.lcd_contrast_min, f_test_lcd_cont_min); eeprom_update_byte(&config_ee.gui.contrast, f_test_lcd_cont); eeprom_busy_wait(); //reload contrast value gui_load_eeprom(); disp.LoadFont(F_TEXT_S); uint8_t f_h = disp.GetTextHeight(); if (!mems_i2c_selftest()) { if (blik) { disp.GotoXY(4, f_h * 3.5 + 3); fprintf_P(lcd_out, PSTR("I2C ERROR")); assert(0); } err = true; } else { res = ms5611.SelfTest(); if (!res) err = true; if (res || blik) { disp.GotoXY(4, f_h * 2 + 3); fprintf_P(lcd_out, PSTR("MS5611:%s"), (res) ? "OK" : "ERR"); assert(res); } res = lsm303d.SelfTest(); if (!res) err = true; if (res || blik) { disp.GotoXY(4, f_h * 3 + 3); fprintf_P(lcd_out, PSTR("LSM303:%s"), (res) ? "OK" : "ERR"); assert(res); } res = l3gd20.SelfTest(); if (!res) err = true; if (res || blik) { disp.GotoXY(4, f_h * 4 + 3); fprintf_P(lcd_out, PSTR("L3GD20:%s"), (res) ? "OK" : "ERR"); assert(res); } res = sht21.SelfTest(); if (!res) err = true; if (res || blik) { disp.GotoXY(4, f_h * 5 + 3); fprintf_P(lcd_out, PSTR("SHT21:%s"), (res) ? "OK" : "ERR"); assert(res); } } res = bt_ready(); if (!res) err = true; if (res || blik) { assert(res); disp.GotoXY(4, f_h * 6 + 3); if (!res) { fprintf_P(lcd_out, PSTR("BT:ERR")); } else { if (bt_get_module_type() == BT_PAN1322) fprintf_P(lcd_out, PSTR("BT:1322")); else fprintf_P(lcd_out, PSTR("BT:1026")); } } res = storage_ready(); if (!res) err = true; if (res || blik) { disp.GotoXY(GUI_DISP_WIDTH / 2, f_h * 2 + 3); fprintf_P(lcd_out, PSTR("SD:%s"), (res) ? "OK" : "ERR"); assert(res); } res = gps_selftest(); if (!res) err = true; if (res || blik) { disp.GotoXY(GUI_DISP_WIDTH / 2, f_h * 3 + 3); fprintf_P(lcd_out, PSTR("GPS:%s"), (res) ? "OK" : "ERR"); assert(res); } res = f_test_button_test == 0b00000111; if (!res) err = true; if (res || blik) { disp.GotoXY(GUI_DISP_WIDTH / 2, f_h * 4 + 3); fprintf_P(lcd_out, PSTR("BUT:%d %d %d"), f_test_button_test & (1 << 0), (f_test_button_test & (1 << 1)) >> 1, (f_test_button_test & (1 << 2)) >> 2); assert(res); }
/** Task to set the configuration of the attached device after it has been enumerated, and to print device information * through the serial port. */ void StillImage_Task(void) { uint8_t ErrorCode; switch (USB_HostState) { case HOST_STATE_Addressed: puts_P(PSTR("Getting Config Data.\r\n")); /* Get and process the configuration descriptor data */ if ((ErrorCode = ProcessConfigurationDescriptor()) != SuccessfulConfigRead) { if (ErrorCode == ControlError) puts_P(PSTR(ESC_FG_RED "Control Error (Get Configuration).\r\n")); else puts_P(PSTR(ESC_FG_RED "Invalid Device.\r\n")); printf_P(PSTR(" -- Error Code: %d\r\n" ESC_FG_WHITE), ErrorCode); /* Indicate error via status LEDs */ LEDs_SetAllLEDs(LEDMASK_USB_ERROR); /* Wait until USB device disconnected */ USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } /* Set the device configuration to the first configuration (rarely do devices use multiple configurations) */ if ((ErrorCode = USB_Host_SetDeviceConfiguration(1)) != HOST_SENDCONTROL_Successful) { printf_P(PSTR(ESC_FG_RED "Control Error (Set Configuration).\r\n" " -- Error Code: %d\r\n" ESC_FG_WHITE), ErrorCode); /* Indicate error via status LEDs */ LEDs_SetAllLEDs(LEDMASK_USB_ERROR); /* Wait until USB device disconnected */ USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } puts_P(PSTR("Still Image Device Enumerated.\r\n")); USB_HostState = HOST_STATE_Configured; break; case HOST_STATE_Configured: /* Indicate device busy via the status LEDs */ LEDs_SetAllLEDs(LEDMASK_USB_BUSY); puts_P(PSTR("Retrieving Device Info...\r\n")); PIMA_SendBlock = (PIMA_Container_t) { .DataLength = PIMA_COMMAND_SIZE(0), .Type = CType_CommandBlock, .Code = PIMA_OPERATION_GETDEVICEINFO, .TransactionID = 0x00000000, .Params = {}, }; /* Send the GETDEVICEINFO block */ SImage_SendBlockHeader(); /* Receive the response data block */ if ((ErrorCode = SImage_ReceiveBlockHeader()) != PIPE_RWSTREAM_NoError) { ShowCommandError(ErrorCode, false); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } /* Calculate the size of the returned device info data structure */ uint16_t DeviceInfoSize = (PIMA_ReceivedBlock.DataLength - PIMA_COMMAND_SIZE(0)); /* Create a buffer large enough to hold the entire device info */ uint8_t DeviceInfo[DeviceInfoSize]; /* Read in the data block data (containing device info) */ SImage_ReadData(DeviceInfo, DeviceInfoSize); /* Once all the data has been read, the pipe must be cleared before the response can be sent */ Pipe_ClearIN(); /* Create a pointer for walking through the info dataset */ uint8_t* DeviceInfoPos = DeviceInfo; /* Skip over the data before the unicode device information strings */ DeviceInfoPos += 8; // Skip to VendorExtensionDesc String DeviceInfoPos += (1 + UNICODE_STRING_LENGTH(*DeviceInfoPos)); // Skip over VendorExtensionDesc String DeviceInfoPos += 2; // Skip over FunctionalMode DeviceInfoPos += (4 + (*(uint32_t*)DeviceInfoPos << 1)); // Skip over Supported Operations Array DeviceInfoPos += (4 + (*(uint32_t*)DeviceInfoPos << 1)); // Skip over Supported Events Array DeviceInfoPos += (4 + (*(uint32_t*)DeviceInfoPos << 1)); // Skip over Supported Device Properties Array DeviceInfoPos += (4 + (*(uint32_t*)DeviceInfoPos << 1)); // Skip over Capture Formats Array DeviceInfoPos += (4 + (*(uint32_t*)DeviceInfoPos << 1)); // Skip over Image Formats Array /* Extract and convert the Manufacturer Unicode string to ASCII and print it through the USART */ char Manufacturer[*DeviceInfoPos]; UnicodeToASCII(DeviceInfoPos, Manufacturer); printf_P(PSTR(" Manufacturer: %s\r\n"), Manufacturer); DeviceInfoPos += 1 + UNICODE_STRING_LENGTH(*DeviceInfoPos); // Skip over Manufacturer String /* Extract and convert the Model Unicode string to ASCII and print it through the USART */ char Model[*DeviceInfoPos]; UnicodeToASCII(DeviceInfoPos, Model); printf_P(PSTR(" Model: %s\r\n"), Model); DeviceInfoPos += 1 + UNICODE_STRING_LENGTH(*DeviceInfoPos); // Skip over Model String /* Extract and convert the Device Version Unicode string to ASCII and print it through the USART */ char DeviceVersion[*DeviceInfoPos]; UnicodeToASCII(DeviceInfoPos, DeviceVersion); printf_P(PSTR(" Device Version: %s\r\n"), DeviceVersion); /* Receive the final response block from the device */ if ((ErrorCode = SImage_ReceiveBlockHeader()) != PIPE_RWSTREAM_NoError) { ShowCommandError(ErrorCode, false); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } /* Verify that the command completed successfully */ if ((PIMA_ReceivedBlock.Type != CType_ResponseBlock) || (PIMA_ReceivedBlock.Code != PIMA_RESPONSE_OK)) { ShowCommandError(PIMA_ReceivedBlock.Code, true); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } puts_P(PSTR("Opening Session...\r\n")); PIMA_SendBlock = (PIMA_Container_t) { .DataLength = PIMA_COMMAND_SIZE(1), .Type = CType_CommandBlock, .Code = PIMA_OPERATION_OPENSESSION, .TransactionID = 0x00000000, .Params = {0x00000001}, }; /* Send the OPENSESSION block, open a session with an ID of 0x0001 */ SImage_SendBlockHeader(); /* Receive the response block from the device */ if ((ErrorCode = SImage_ReceiveBlockHeader()) != PIPE_RWSTREAM_NoError) { ShowCommandError(ErrorCode, false); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } /* Verify that the command completed successfully */ if ((PIMA_ReceivedBlock.Type != CType_ResponseBlock) || (PIMA_ReceivedBlock.Code != PIMA_RESPONSE_OK)) { ShowCommandError(PIMA_ReceivedBlock.Code, true); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } puts_P(PSTR("Closing Session...\r\n")); PIMA_SendBlock = (PIMA_Container_t) { .DataLength = PIMA_COMMAND_SIZE(1), .Type = CType_CommandBlock, .Code = PIMA_OPERATION_CLOSESESSION, .TransactionID = 0x00000001, .Params = {0x00000001}, }; /* Send the CLOSESESSION block, close the session with an ID of 0x0001 */ SImage_SendBlockHeader(); /* Receive the response block from the device */ if ((ErrorCode = SImage_ReceiveBlockHeader()) != PIPE_RWSTREAM_NoError) { ShowCommandError(ErrorCode, false); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } /* Verify that the command completed successfully */ if ((PIMA_ReceivedBlock.Type != CType_ResponseBlock) || (PIMA_ReceivedBlock.Code != PIMA_RESPONSE_OK)) { ShowCommandError(PIMA_ReceivedBlock.Code, true); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } puts_P(PSTR("Done.\r\n")); /* Indicate device no longer busy */ LEDs_SetAllLEDs(LEDMASK_USB_READY); USB_HostState = HOST_STATE_WaitForDeviceRemoval; break; } } /** Function to convert a given Unicode encoded string to ASCII. This function will only work correctly on Unicode * strings which contain ASCII printable characters only. * * \param[in] UnicodeString Pointer to a Unicode encoded input string * \param[out] Buffer Pointer to a buffer where the converted ASCII string should be stored */ void UnicodeToASCII(uint8_t* UnicodeString, char* Buffer) { /* Get the number of characters in the string, skip to the start of the string data */ uint8_t CharactersRemaining = *(UnicodeString++); /* Loop through the entire unicode string */ while (CharactersRemaining--) { /* Load in the next unicode character (only the lower byte, as only Unicode coded ASCII is supported) */ *(Buffer++) = *UnicodeString; /* Jump to the next unicode character */ UnicodeString += 2; } /* Null terminate the string */ *Buffer = 0; } /** Displays a PIMA command error via the device's serial port. * * \param[in] ErrorCode Error code of the function which failed to complete successfully * \param[in] ResponseCodeError Indicates if the error is due to a command failed indication from the device, or a communication failure */ void ShowCommandError(uint8_t ErrorCode, bool ResponseCodeError) { char* FailureType = ((ResponseCodeError) ? PSTR("Response Code != OK") : PSTR("Transaction Fail")); printf_P(PSTR(ESC_FG_RED "Command Error (%S).\r\n" " -- Error Code %d\r\n" ESC_FG_WHITE), FailureType, ErrorCode); /* Indicate error via status LEDs */ LEDs_SetAllLEDs(LEDMASK_USB_ERROR); }
void AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on)) { Vector3f last_average, best_avg; Vector3f ins_gyro; float best_diff = 0; // cold start hal.scheduler->delay(100); hal.console->printf_P(PSTR("Init Gyro")); // remove existing gyro offsets _gyro_offset = Vector3f(0,0,0); for(int8_t c = 0; c < 25; c++) { // Mostly we are just flashing the LED's here // to tell the user to keep the IMU still FLASH_LEDS(true); hal.scheduler->delay(20); update(); ins_gyro = get_gyro(); FLASH_LEDS(false); hal.scheduler->delay(20); } // the strategy is to average 200 points over 1 second, then do it // again and see if the 2nd average is within a small margin of // the first last_average.zero(); // we try to get a good calibration estimate for up to 10 seconds // if the gyros are stable, we should get it in 2 seconds for (int16_t j = 0; j <= 10; j++) { Vector3f gyro_sum, gyro_avg, gyro_diff; float diff_norm; uint8_t i; hal.console->printf_P(PSTR("*")); gyro_sum.zero(); for (i=0; i<200; i++) { update(); ins_gyro = get_gyro(); gyro_sum += ins_gyro; if (i % 40 == 20) { FLASH_LEDS(true); } else if (i % 40 == 0) { FLASH_LEDS(false); } hal.scheduler->delay(5); } gyro_avg = gyro_sum / i; gyro_diff = last_average - gyro_avg; diff_norm = gyro_diff.length(); if (j == 0) { best_diff = diff_norm; best_avg = gyro_avg; } else if (gyro_diff.length() < ToRad(0.04)) { // we want the average to be within 0.1 bit, which is 0.04 degrees/s last_average = (gyro_avg * 0.5) + (last_average * 0.5); _gyro_offset = last_average; // all done return; } else if (diff_norm < best_diff) { best_diff = diff_norm; best_avg = (gyro_avg * 0.5) + (last_average * 0.5); } last_average = gyro_avg; } // we've kept the user waiting long enough - use the best pair we // found so far hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff)); _gyro_offset = best_avg; }
enum wsFrameType wsParseHandshake(const uint8_t *inputFrame, size_t inputLength, struct handshake *hs) { const char *inputPtr = (const char *)inputFrame; const char *endPtr = (const char *)inputFrame + inputLength; if (!strstr((const char *)inputFrame, "\r\n\r\n")) return WS_INCOMPLETE_FRAME; if (memcmp_P(inputFrame, PSTR("GET "), 4) != 0) return WS_ERROR_FRAME; // measure resource size char *first = strchr((const char *)inputFrame, ' '); if (!first) return WS_ERROR_FRAME; first++; char *second = strchr(first, ' '); if (!second) return WS_ERROR_FRAME; if (hs->resource) { free(hs->resource); hs->resource = NULL; } hs->resource = (char *)malloc(second - first + 1); // +1 is for \x00 symbol assert(hs->resource); if (sscanf_P(inputPtr, PSTR("GET %s HTTP/1.1\r\n"), hs->resource) != 1) return WS_ERROR_FRAME; inputPtr = strstr_P(inputPtr, rn) + 2; /* parse next lines */ #define prepare(x) do {if (x) { free(x); x = NULL; }} while(0) #define strtolower(x) do { int i; for (i = 0; x[i]; i++) x[i] = tolower(x[i]); } while(0) uint8_t connectionFlag = FALSE; uint8_t upgradeFlag = FALSE; uint8_t subprotocolFlag = FALSE; char versionString[2]; memset(versionString, 0, sizeof(versionString)); while (inputPtr < endPtr && inputPtr[0] != '\r' && inputPtr[1] != '\n') { if (memcmp_P(inputPtr, hostField, strlen_P(hostField)) == 0) { inputPtr += strlen_P(hostField); prepare(hs->host); hs->host = getUptoLinefeed(inputPtr); } else if (memcmp_P(inputPtr, originField, strlen_P(originField)) == 0) { inputPtr += strlen_P(originField); prepare(hs->origin); hs->origin = getUptoLinefeed(inputPtr); } else if (memcmp_P(inputPtr, protocolField, strlen_P(protocolField)) == 0) { inputPtr += strlen_P(protocolField); //getUptoLinefeed(inputPtr); subprotocolFlag = TRUE; } else if (memcmp_P(inputPtr, keyField, strlen_P(keyField)) == 0) { inputPtr += strlen_P(keyField); prepare(hs->key); hs->key = getUptoLinefeed(inputPtr); } else if (memcmp_P(inputPtr, versionField, strlen_P(versionField)) == 0) { inputPtr += strlen_P(versionField); copyUptoLinefeed(inputPtr, versionString); } else if (memcmp_P(inputPtr, connectionField, strlen_P(connectionField)) == 0) { inputPtr += strlen_P(connectionField); char *connectionValue = NULL; connectionValue = getUptoLinefeed(inputPtr); assert(connectionValue); strtolower(connectionValue); if (strstr_P(connectionValue, upgrade) != NULL) connectionFlag = TRUE; free(connectionValue); } else if (memcmp_P(inputPtr, upgradeField, strlen_P(upgradeField)) == 0) { inputPtr += strlen_P(upgradeField); char *compare = NULL; compare = getUptoLinefeed(inputPtr); strtolower(compare); assert(compare); if (memcmp_P(compare, websocket, strlen_P(websocket)) == 0) upgradeFlag = TRUE; free(compare); }; inputPtr = strstr_P(inputPtr, rn) + 2; } // we have read all data, so check them if (!hs->host || !hs->key || !connectionFlag || !upgradeFlag || subprotocolFlag || memcmp_P(versionString, version, strlen_P(version)) != 0) { hs->frameType = WS_ERROR_FRAME; } hs->frameType = WS_OPENING_FRAME; return hs->frameType; }
void AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) { int8_t flashcount = 0; Vector3f ins_accel; Vector3f prev; Vector3f accel_offset; float total_change; float max_offset; // cold start hal.scheduler->delay(100); hal.console->printf_P(PSTR("Init Accel")); // clear accelerometer offsets and scaling _accel_offset = Vector3f(0,0,0); _accel_scale = Vector3f(1,1,1); // initialise accel offsets to a large value the first time // this will force us to calibrate accels at least twice accel_offset = Vector3f(500, 500, 500); // loop until we calculate acceptable offsets do { // get latest accelerometer values update(); ins_accel = get_accel(); // store old offsets prev = accel_offset; // get new offsets accel_offset = ins_accel; // We take some readings... for(int8_t i = 0; i < 50; i++) { hal.scheduler->delay(20); update(); ins_accel = get_accel(); // low pass filter the offsets accel_offset = accel_offset * 0.9 + ins_accel * 0.1; // display some output to the user if(flashcount == 5) { hal.console->printf_P(PSTR("*")); FLASH_LEDS(true); } if(flashcount >= 10) { flashcount = 0; FLASH_LEDS(false); } flashcount++; } // null gravity from the Z accel accel_offset.z += GRAVITY_MSS; total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z); max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y; max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z; hal.scheduler->delay(500); } while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET); // set the global accel offsets _accel_offset = accel_offset; hal.console->printf_P(PSTR(" ")); }
/** Event handler for the USB_DeviceUnattached event. This indicates that a device has been removed from the host, and * stops the library USB task management process. */ void EVENT_USB_Host_DeviceUnattached(void) { puts_P(PSTR(ESC_FG_GREEN "\r\nDevice Unattached.\r\n" ESC_FG_WHITE)); LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY); }
static void reprapworld_keypad_move_home() { enquecommand_P((PSTR("G28"))); // move all axis home }