int main (void)
{
char ch,aux1 ,aux2;
int UsbDetected = FALSE;
#ifdef CMSIS // If we are conforming to CMSIS, we need to call start here
start();
#endif
vfnMcuConfig();
printf("\n\rRunning the FRDMKL46_Demo project.\n\r");
vfnLCD_Init();
gpio_init();
Pit_init();
TSI_Init();
usb_init();
next_task(vfn_led_test);
accel_init();
mag_init();
eCompassInit();
adc_init();
tpm_init(); //Green LED 50%SIM_SCGC5_PORTC_MASK
// character test
vfnLCD_Write_Msg("8888");
_LCD_DP1_ON();
_LCD_DP2_ON();
_LCD_DP3_ON();
_LCD_COL_ON();
while(1)
{
#ifdef FRDM_REVA
if (uart_getchar_present(UART1_BASE_PTR))
#else
if (uart0_getchar_present(UART0_BASE_PTR))
#endif
{
ch = in_char();
printf("\n\r Received char = %c \n\r",ch);
if (ch==' ')
{
printf("\n\r light_sensor = %i",adc_light_sensor);
// printf("\n\r Yaw =%4d Pitch =%4d Roll =%4d \r", APhi6DOF, AThe6DOF, APsi6DOF);
printf("\n\r Yaw =%4d Pitch =%4d Roll =%4d \r", APsi6DOF, APhi6DOF, AThe6DOF);
printf("\n\r tsi %%= %03i ", AbsolutePercentegePosition);
}
}
if (input_rise(SW1_ON, &aux1))
{
printf("\n\r SW1 \n\r");
}
if (input_rise(SW2_ON, &aux2))
{
printf("\n\r SW2 \n\r ");
}
ptr_next_task(); // do the actual function
TSI_SliderRead();
usb_service();
if (gu8USB_State == uENUMERATED && !UsbDetected)
{
// next_task(vfn_rgb_test);
UsbDetected = TRUE;
}
adc_light_sensor = adc_read(3);
}
}
bool Ekf::initialiseFilter(void)
{
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial roll and pitch estimate from accel vector, assuming vehicle is static
Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
float pitch = 0.0f;
float roll = 0.0f;
if (accel_init.norm() > 0.001f) {
accel_init.normalize();
pitch = asinf(accel_init(0));
roll = -asinf(accel_init(1) / cosf(pitch));
}
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
// Get the latest magnetic field measurement.
// If we don't have a measurement then we cannot initialise the filter
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) {
return false;
}
// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
// TODO use declination if available
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
float declination = 0.0f;
euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;
resetVelocity();
resetPosition();
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (baro_init.time_us == 0) {
return false;
}
_state.pos(2) = -baro_init.hgt;
_output_new.pos(2) = -baro_init.hgt;
initialiseCovariance();
return true;
}
void
Sensors::task_main()
{
/* start individual sensors */
accel_init();
gyro_init();
mag_init();
baro_init();
adc_init();
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
/*
* do advertisements
*/
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.adc_voltage_v[0] = 0.0f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
diff_pres_poll(raw);
parameter_update_poll(true /* forced */);
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* wakeup source(s) */
struct pollfd fds[1];
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
fds[0].fd = _gyro_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 50ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_control_mode_poll();
/* check parameters for updates */
parameter_update_poll();
/* the timestamp of the raw struct is updated by the gyro_poll() method */
/* copy most recent sensor data */
gyro_poll(raw);
accel_poll(raw);
mag_poll(raw);
baro_poll(raw);
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing) {
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
}
/* Look for new r/c input data */
rc_poll();
perf_end(_loop_perf);
}
warnx("[sensors] exiting.");
_sensors_task = -1;
_exit(0);
}
// Main program
int main(void)
{
char i;
char ch;
char *heap_end;
char inBuffer[BUF_SIZE];
int readLen,bufPos;
int n;
int nargs[10];
int hasAccelData = 0;
int hasTouchData = 0;
// Initialize all modules
RGB_LED(100,0,0);
//uart_init(115200);
uart_init(9600);
accel_init();
touch_init((1 << 9) | (1 << 10)); // Channels 9 and 10
// usb_init();
setvbuf(stdin, NULL, _IONBF, 0); // No buffering
// Run tests
tests();
delay(500);
// Welcome banner
iprintf("\r\n\r\n====== Freescale Freedom FRDM-LK25Z\r\n");
iprintf("Built: %s %s\r\n\r\n", __DATE__, __TIME__);
heap_end = _sbrk(0);
iprintf("Heap: %p to %p (%d bytes used)\r\n", __heap_start, heap_end,
heap_end - (char *)__heap_start);
iprintf("Stack: %p to %p (%d bytes used)\r\n", &i, __StackTop,
(char *)__StackTop - &i);
iprintf("%d bytes free\r\n", &i - heap_end);
inBuffer[0] = 0; // reset buffer
bufPos = 0;
RGB_LED(0,100,0); // Green
for(;;) {
readLen = uart_read_nonblock(inBuffer+bufPos,BUF_SIZE-(bufPos+2));
if (readLen>0) {
bufPos+=readLen;
// quick trim
while(bufPos>0 && isspace(inBuffer[bufPos-1]))
bufPos--;
inBuffer[bufPos] = 0;
if (inBuffer[bufPos-1] == ';') {
iprintf("(in buf = '%s', len=%d)\r\n",inBuffer,bufPos);
switch(inBuffer[0]) {
case 'C':
n = sscanf(inBuffer+1,"%i,%i,%i",&nargs[0],&nargs[1],&nargs[2]);
if(n==3) RGB_LED(nargs[0],nargs[1],nargs[2]);
break;
case 'R':
RGB_LED(100,0,0);
break;
case 'G':
RGB_LED(0,100,0);
break;
case 'B':
RGB_LED(0,0,100);
break;
case 'A':
hasAccelData = 1;
break;
case 'a':
hasAccelData = 0;
break;
case 'T':
hasTouchData = 1;
break;
case 't':
hasTouchData = 0;
break;
case 'i':
iprintf("[efb,0.1] empiriKit Freescale Board, v.0.1\r\n");
break;
}
bufPos = 0; // "clear" the buffer
}
}
if (hasAccelData)
iprintf("a%d,%d,%d;\r\n", accel_x(), accel_y(), accel_z());
if (hasTouchData)
iprintf("t%d,%d;\r\n", touch_data(9), touch_data(10));
}
}
void prf_init_func(void)
{
#if (BLE_ACCEL)
accel_init();
#endif // (BLE_ACCEL)
#if (BLE_HT_THERMOM)
htpt_init();
#endif // (BLE_HT_THERMOM)
#if (BLE_HT_COLLECTOR)
htpc_init();
#endif // (BLE_HT_COLLECTOR)
#if (BLE_DIS_SERVER)
diss_init();
#endif // (BLE_DIS_SERVER)
#if (BLE_DIS_CLIENT)
disc_init();
#endif // (BLE_DIS_CLIENT)
#if (BLE_BP_SENSOR)
blps_init();
#endif // (BLE_BP_SENSOR)
#if (BLE_BP_COLLECTOR)
blpc_init();
#endif // (BLE_BP_COLLECTOR)
#if (BLE_TIP_SERVER)
tips_init();
#endif // (BLE_TIP_SERVER)
#if (BLE_TIP_CLIENT)
tipc_init();
#endif // (BLE_TIP_CLIENT)
#if (BLE_HR_SENSOR)
hrps_init();
#endif // (BLE_HR_SENSOR)
#if (BLE_HR_COLLECTOR)
hrpc_init();
#endif // (BLE_HR_COLLECTOR)
#if (BLE_FINDME_LOCATOR)
findl_init();
#endif // (BLE_FINDME_LOCATOR)
#if (BLE_FINDME_TARGET)
findt_init();
#endif // (BLE_FINDME_TARGET)
#if (BLE_PROX_MONITOR)
proxm_init();
#endif // (BLE_PROX_MONITOR)
#if (BLE_PROX_REPORTER)
proxr_init();
#endif // (BLE_PROX_REPORTER)
#if (BLE_STREAMDATA_HOST)
streamdatah_init();
#endif // (BLE_STREAMDATA_HOST)
#if (BLE_STREAMDATA_DEVICE)
streamdatad_init();
#endif // (BLE_STREAMDATA_DEVICE)
#if (BLE_SPOTA_RECEIVER)
spotar_init();
#endif // (BLE_SPOTA_RECEIVER)
#if (BLE_SP_SERVER)
scpps_init();
#endif // (BLE_SP_SERVER)
#if (BLE_SP_CLIENT)
scppc_init();
#endif // (BLE_SP_CLIENT)
#if (BLE_BATT_SERVER)
bass_init();
#endif // (BLE_BATT_SERVER)
#if (BLE_BATT_CLIENT)
basc_init();
#endif // (BLE_BATT_CLIENT)
#if (BLE_HID_DEVICE)
hogpd_init();
#endif // (BLE_HID_DEVICE)
#if (BLE_HID_BOOT_HOST)
hogpbh_init();
#endif // (BLE_HID_BOOT_HOST)
#if (BLE_HID_REPORT_HOST)
hogprh_init();
#endif // (BLE_HID_REPORT_HOST)
#if (BLE_GL_COLLECTOR)
glpc_init();
#endif // (BLE_GL_COLLECTOR)
#if (BLE_GL_SENSOR)
glps_init();
#endif // (BLE_GL_SENSOR)
#if (BLE_NEB_CLIENT)
nbpc_init();
#endif // (BLE_NEB_CLIENT)
#if (BLE_NEB_SERVER)
nbps_init();
#endif // (BLE_NEB_SERVER)
#if (BLE_RSC_COLLECTOR)
rscpc_init();
#endif // (BLE_RSC_COLLECTOR)
#if (BLE_RSC_SENSOR)
rscps_init();
#endif // (BLE_RSC_SENSOR)
#if (BLE_CSC_COLLECTOR)
cscpc_init();
#endif // (BLE_CSC_COLLECTOR)
#if (BLE_CSC_SENSOR)
cscps_init();
#endif // (BLE_CSC_SENSOR)
#if (BLE_AN_CLIENT)
anpc_init();
#endif // (BLE_AN_CLIENT)
#if (BLE_AN_SERVER)
anps_init();
#endif // (BLE_AN_SERVER)
#if (BLE_PAS_CLIENT)
paspc_init();
#endif // (BLE_PAS_CLIENT)
#if (BLE_PAS_SERVER)
pasps_init();
#endif // (BLE_PAS_SERVER)
#if (BLE_SAMPLE128)
sample128_init();
#endif // (BLE_SAMPLE128)
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
float pitch_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char *posture_message;
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, &pitch_angle, &yaw_angle);
printf("is moving: %d\n", isMoving(gyro_data) );
//printf("yaw angle: %f ", yaw_angle);
//printf("pitch angle: %f ", pitch_angle);
//printf("z vector: %f\n", accel_data.z);
if (count == 3) {
//send posture to cloud
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, yaw_angle);
else
curr_posture = prev_posture; //else just use the old posture
if (curr_posture==UNDEFINED)
curr_posture = prev_posture;
prev_posture = curr_posture; //set new value for prev posture
posture_message = construct_message(POS, accel_data, curr_posture);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
//store accel data in string
/*
x_accel_message = construct_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
*/
count = 0;
}
char *posture_string;
switch(curr_posture)
{
case 0:
posture_string = "upright";
break;
case 1:
posture_string = "face up";
break;
case 2:
posture_string = "face down";
break;
case 3:
posture_string = "left";
break;
case 4:
posture_string = "right";
break;
default:
posture_string = "undefined";
}
printf("current orientation: %s \n", posture_string);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
int main(int argc, char *argv[]) {
data_t accel_data, gyro_data;
data_t gyro_offset;
float a_res, g_res;
mraa_i2c_context accel, gyro;
accel_scale_t a_scale = A_SCALE_8G;
gyro_scale_t g_scale = G_SCALE_2000DPS;
int client_socket_fd, portno, n;
struct sockaddr_in serv_addr;
struct hostent *server;
int flags;
float buffer[BUFF_SIZE];
int send_idx = 0, curr_idx = 0;
int next_it, send_it, curr_it; // helper variables for converting idx (byte) to it (float)
int num_pts = 0;
int i;
int init_sam = (SAMP_RATE*INIT_SEC)+1; // number of samples during init period
int quit = 0; // quit flag
char stop; // dummy buffer for stop ping
// Read command line arguments, need to get the host IP address and port
if (argc < 3) {
fprintf(stderr, "USAGE: %s <hostname> <port>\n", argv[0]);
exit(1);
}
// Convert the arguments to the appropriate data types
portno = atoi(argv[2]);
// setup the socket
// technically PF_INET. Refer to:
// http://stackoverflow.com/questions/6729366/what-is-the-difference-between-af-inet-and-pf-inet-in-socket-programming
//client_socket_fd = socket(AF_INET, SOCK_STREAM, 0);
client_socket_fd = socket(PF_INET, SOCK_STREAM, 0);
// check if the socket was created successfully. If it wasnt, display an error and exit
if(client_socket_fd < 0) {
error("ERROR opening socket");
}
// check if the IP entered by the user is valid
server = gethostbyname(argv[1]);
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(1);
}
// clear our the serv_addr buffer
memset((char *) &serv_addr, 0, sizeof(serv_addr));
// set up the socket
serv_addr.sin_family = AF_INET;
memcpy((char *)&serv_addr.sin_addr.s_addr, (char *)server->h_addr, server->h_length);
serv_addr.sin_port = htons(portno);
// try to connect to the server
if (connect(client_socket_fd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0){
error("ERROR connecting");
}
// make non-blocking. Refer to:
// http://developerweb.net/viewtopic.php?id=3000
/* Set socket to non-blocking */
if ((flags = fcntl(client_socket_fd, F_GETFL, 0)) < 0) {
error("ERROR getting socket flags");
}
if (fcntl(client_socket_fd, F_SETFL, flags | O_NONBLOCK) < 0) {
error("ERROR setting socket to non-blocking");
}
printf("Socket connected to server!\n");
printf("Now beginning sensor initialization\n");
// initialize sensors, set scale, and calculate resolution.
accel = accel_init();
set_accel_scale(accel, a_scale);
a_res = calc_accel_res(a_scale);
gyro = gyro_init();
set_gyro_scale(gyro, g_scale);
g_res = calc_gyro_res(g_scale);
gyro_offset = calc_gyro_offset(gyro, g_res);
printf("GYRO OFFSETS x: %f y: %f z: %f\n",
gyro_offset.x, gyro_offset.y, gyro_offset.z);
//Read the sensor data and print them.
printf("STARTING TO COLLECT DATA\n");
printf("\n \t\tAccelerometer\t\t\t||");
printf("\t\t\tGyroscope\t\t\t\n");
while (1) {
for (i = 0; i < PACK_PTS; i++) {
n = read(client_socket_fd, &stop, 1);
if (n < 0) {
if (!(errno == EWOULDBLOCK || errno == EAGAIN))
error("ERROR reading from client socket");
// else no stop ping yet
} else {
#ifdef DEBUG
printf("RECEIVED STOP PING\n");
#endif
quit = 1;
break;
}
if (num_pts == init_sam) printf("INITIALIZATION PERIOD DONE\n");
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
if (num_pts%PRINT_PERIOD == 0) {
printf("%i points\n", num_pts);
printf(" send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
printf(" X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
}
#ifdef DEBUG
printf("X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z);
printf("\tX: %f\t Y: %f\t Z: %f p: %d\t\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts);
#endif
curr_it = curr_idx/sizeof(float);
buffer[curr_it] = gyro_data.x - gyro_offset.x;
buffer[curr_it+1] = gyro_data.y - gyro_offset.y;
buffer[curr_it+2] = gyro_data.z - gyro_offset.z;
buffer[curr_it+3] = accel_data.x;
buffer[curr_it+4] = accel_data.y;
buffer[curr_it+5] = accel_data.z;
num_pts++;
next_it = (curr_it + SAMP_SIZE)%BUFF_SIZE;
send_it = send_idx/sizeof(float); // truncate integer
if ((next_it >= send_it) &&
(next_it <= (send_it+SAMP_SIZE-1))) {
// note that curr_idx always advances SAMP_SIZE at a time
// went around ring buffer. panic!
fprintf(stderr, "Ring buffer is full. Abort!\n");
exit(1);
}
curr_idx = next_it*sizeof(float);
} // batch done
// stop writing packets if received stop ping
if (quit) break;
// write batch to server
n = write(client_socket_fd, ((char*)buffer)+send_idx,
((curr_idx>send_idx)?
(curr_idx-send_idx):
(BUFF_BYTES-send_idx)));
// n contains how many bytes were written to the socket
// if n is less than 0, then there was an error
// Refer to:
// http://stackoverflow.com/questions/3153939/properly-writing-to-a-nonblocking-socket-in-c
// http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html
if (n < 0) {
if (errno == EWOULDBLOCK || errno == EAGAIN) {
fprintf(stderr, "FAILED WRITE for byte %d. Currently at byte %d\n",
send_idx, curr_idx);
} else {
error("ERROR writing to socket");
}
} else { // successful write
send_idx = (send_idx+n)%BUFF_BYTES;
}
#ifdef DEBUG
printf("send_idx: %d curr_idx: %d\n", send_idx, curr_idx);
#endif
usleep(1000000/SAMP_RATE);
}
close(client_socket_fd);
return 0;
}
void prf_init_func(void)
{
#if (BLE_ACCEL)
accel_init();
#endif // (BLE_ACCEL)
#if (BLE_HT_THERMOM)
htpt_init();
#endif // (BLE_HT_THERMOM)
#if (BLE_HT_COLLECTOR)
htpc_init();
#endif // (BLE_HT_COLLECTOR)
#if (BLE_DIS_SERVER)
diss_init();
#endif // (BLE_DIS_SERVER)
#if (BLE_DIS_CLIENT)
disc_init();
#endif // (BLE_DIS_CLIENT)
#if (BLE_BP_SENSOR)
blps_init();
#endif // (BLE_BP_SENSOR)
#if (BLE_BP_COLLECTOR)
blpc_init();
#endif // (BLE_BP_COLLECTOR)
#if (BLE_TIP_SERVER)
tips_init();
#endif // (BLE_TIP_SERVER)
#if (BLE_TIP_CLIENT)
tipc_init();
#endif // (BLE_TIP_CLIENT)
#if (BLE_HR_SENSOR)
hrps_init();
#endif // (BLE_HR_SENSOR)
#if (BLE_HR_COLLECTOR)
hrpc_init();
#endif // (BLE_HR_COLLECTOR)
#if (BLE_FINDME_LOCATOR)
findl_init();
#endif // (BLE_FINDME_LOCATOR)
#if (BLE_FINDME_TARGET)
findt_init();
#endif // (BLE_FINDME_TARGET)
#if (BLE_PROX_MONITOR)
proxm_init();
#endif // (BLE_PROX_MONITOR)
#if (BLE_PROX_REPORTER)
proxr_init();
#endif // (BLE_PROX_REPORTER)
#if (BLE_STREAMDATA_HOST)
streamdatah_init();
#endif // (BLE_STREAMDATA_HOST)
#if (BLE_STREAMDATA_DEVICE)
streamdatad_init();
#endif // (BLE_STREAMDATA_DEVICE)
#if (BLE_SPOTA_RECEIVER)
spotar_init();
#endif // (BLE_SPOTA_RECEIVER)
#if (BLE_SPS_CLIENT)
sps_client_init();
#endif // (BLE_SPS_CLIENT)
#if (BLE_SPS_SERVER)
sps_server_init();
#endif // (BLE_SPS_SERVER)
#if (BLE_SP_SERVER)
scpps_init();
#endif // (BLE_SP_SERVER)
#if (BLE_SP_CLIENT)
scppc_init();
#endif // (BLE_SP_CLIENT)
#if (BLE_BATT_SERVER)
bass_init();
#endif // (BLE_BATT_SERVER)
#if (BLE_BATT_CLIENT)
basc_init();
#endif // (BLE_BATT_CLIENT)
#if (BLE_HID_DEVICE)
hogpd_init();
#endif // (BLE_HID_DEVICE)
#if (BLE_HID_BOOT_HOST)
hogpbh_init();
#endif // (BLE_HID_BOOT_HOST)
#if (BLE_HID_REPORT_HOST)
hogprh_init();
#endif // (BLE_HID_REPORT_HOST)
#if (BLE_GL_COLLECTOR)
glpc_init();
#endif // (BLE_GL_COLLECTOR)
#if (BLE_GL_SENSOR)
glps_init();
#endif // (BLE_GL_SENSOR)
#if (BLE_NEB_CLIENT)
nbpc_init();
#endif // (BLE_NEB_CLIENT)
#if (BLE_NEB_SERVER)
nbps_init();
#endif // (BLE_NEB_SERVER)
#if (BLE_RSC_COLLECTOR)
rscpc_init();
#endif // (BLE_RSC_COLLECTOR)
#if (BLE_RSC_SENSOR)
rscps_init();
#endif // (BLE_RSC_SENSOR)
#if (BLE_CSC_COLLECTOR)
cscpc_init();
#endif // (BLE_CSC_COLLECTOR)
#if (BLE_CSC_SENSOR)
cscps_init();
#endif // (BLE_CSC_SENSOR)
#if (BLE_CP_COLLECTOR)
cppc_init();
#endif // (BLE_CP_COLLECTOR)
#if (BLE_CP_SENSOR)
cpps_init();
#endif // (BLE_CP_SENSOR)
#if (BLE_LN_COLLECTOR)
lanc_init();
#endif // (BLE_LN_COLLECTOR)
#if (BLE_LN_SENSOR)
lans_init();
#endif // (BLE_LN_SENSOR)
#if (BLE_AN_CLIENT)
anpc_init();
#endif // (BLE_AN_CLIENT)
#if (BLE_AN_SERVER)
anps_init();
#endif // (BLE_AN_SERVER)
#if (BLE_ANC_CLIENT)
ancc_init();
#endif // (BLE_ANC_CLIENT)
#if (BLE_PAS_CLIENT)
paspc_init();
#endif // (BLE_PAS_CLIENT)
#if (BLE_PAS_SERVER)
pasps_init();
#endif // (BLE_PAS_SERVER)
#if (BLE_SAMPLE128)
sample128_init();
#endif // (BLE_SAMPLE128)
#if (BLE_WPT_CLIENT)
wptc_init();
#endif // WPT_CLIENT
#if (BLE_WPTS)
wpts_init();
#endif // BLE_WPTS
#if (BLE_IEU)
ieu_init();
#endif // (BLE_IEU)
#if (BLE_MPU)
mpu_init();
#endif // (BLE_MPU)
#if (BLE_UDS_SERVER)
udss_init();
#endif // (BLE_UDS_SERVER)
#if (BLE_WSS_SERVER)
wsss_init();
#endif // (BLE_WSS_SERVER)
// aiwesky 20151004 wechat
#if (BLE_WECHAT)
wechat_init();
#endif // (BLE_WECHAT)
}
int main(void) {
// TODO disable JTAG
/* STM32F4xx HAL library initialization:
- Configure the Flash prefetch, instruction and Data caches
- Configure the Systick to generate an interrupt each 1 msec
- Set NVIC Group Priority to 4
- Global MSP (MCU Support Package) initialization
*/
HAL_Init();
// set the system clock to be HSE
SystemClock_Config();
// enable GPIO clocks
__GPIOA_CLK_ENABLE();
__GPIOB_CLK_ENABLE();
__GPIOC_CLK_ENABLE();
__GPIOD_CLK_ENABLE();
// enable the CCM RAM
__CCMDATARAMEN_CLK_ENABLE();
#if 0
#if defined(NETDUINO_PLUS_2)
{
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
#if MICROPY_HW_HAS_SDCARD
// Turn on the power enable for the sdcard (PB1)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET);
#endif
// Turn on the power for the 5V on the expansion header (PB2)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET);
}
#endif
#endif
// basic sub-system init
pendsv_init();
timer_tim3_init();
led_init();
switch_init0();
int first_soft_reset = true;
soft_reset:
// check if user switch held to select the reset mode
led_state(1, 0);
led_state(2, 1);
led_state(3, 0);
led_state(4, 0);
uint reset_mode = 1;
#if MICROPY_HW_HAS_SWITCH
if (switch_get()) {
for (uint i = 0; i < 3000; i++) {
if (!switch_get()) {
break;
}
HAL_Delay(20);
if (i % 30 == 29) {
if (++reset_mode > 3) {
reset_mode = 1;
}
led_state(2, reset_mode & 1);
led_state(3, reset_mode & 2);
led_state(4, reset_mode & 4);
}
}
// flash the selected reset mode
for (uint i = 0; i < 6; i++) {
led_state(2, 0);
led_state(3, 0);
led_state(4, 0);
HAL_Delay(50);
led_state(2, reset_mode & 1);
led_state(3, reset_mode & 2);
led_state(4, reset_mode & 4);
HAL_Delay(50);
}
HAL_Delay(400);
}
#endif
#if MICROPY_HW_ENABLE_RTC
if (first_soft_reset) {
rtc_init();
}
#endif
// more sub-system init
#if MICROPY_HW_HAS_SDCARD
if (first_soft_reset) {
sdcard_init();
}
#endif
if (first_soft_reset) {
storage_init();
}
// GC init
gc_init(&_heap_start, &_heap_end);
// Change #if 0 to #if 1 if you want REPL on USART_6 (or another usart)
// as well as on USB VCP
#if 0
pyb_usart_global_debug = pyb_Usart(MP_OBJ_NEW_SMALL_INT(PYB_USART_YA),
MP_OBJ_NEW_SMALL_INT(115200));
#else
pyb_usart_global_debug = NULL;
#endif
// Micro Python init
qstr_init();
mp_init();
mp_obj_list_init(mp_sys_path, 0);
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_));
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_lib));
mp_obj_list_init(mp_sys_argv, 0);
readline_init();
exti_init();
#if MICROPY_HW_HAS_SWITCH
// must come after exti_init
switch_init();
#endif
#if MICROPY_HW_HAS_LCD
// LCD init (just creates class, init hardware by calling LCD())
lcd_init();
#endif
pin_map_init();
// local filesystem init
{
// try to mount the flash
FRESULT res = f_mount(&fatfs0, "0:", 1);
if (reset_mode == 3 || res == FR_NO_FILESYSTEM) {
// no filesystem, or asked to reset it, so create a fresh one
// LED on to indicate creation of LFS
led_state(PYB_LED_R2, 1);
uint32_t start_tick = HAL_GetTick();
res = f_mkfs("0:", 0, 0);
if (res == FR_OK) {
// success creating fresh LFS
} else {
__fatal_error("could not create LFS");
}
// create empty main.py
FIL fp;
f_open(&fp, "0:/main.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// create .inf driver file
f_open(&fp, "0:/pybcdc.inf", FA_WRITE | FA_CREATE_ALWAYS);
f_write(&fp, fresh_pybcdc_inf, sizeof(fresh_pybcdc_inf) - 1 /* don't count null terminator */, &n);
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(start_tick, 200);
led_state(PYB_LED_R2, 0);
} else if (res == FR_OK) {
// mount sucessful
} else {
__fatal_error("could not access LFS");
}
}
// make sure we have a 0:/boot.py
{
FILINFO fno;
#if _USE_LFN
fno.lfname = NULL;
fno.lfsize = 0;
#endif
FRESULT res = f_stat("0:/boot.py", &fno);
if (res == FR_OK) {
if (fno.fattrib & AM_DIR) {
// exists as a directory
// TODO handle this case
// see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation
} else {
// exists as a file, good!
}
} else {
// doesn't exist, create fresh file
// LED on to indicate creation of boot.py
led_state(PYB_LED_R2, 1);
uint32_t start_tick = HAL_GetTick();
FIL fp;
f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(start_tick, 200);
led_state(PYB_LED_R2, 0);
}
}
// root device defaults to internal flash filesystem
uint root_device = 0;
#if defined(USE_DEVICE_MODE)
usb_storage_medium_t usb_medium = USB_STORAGE_MEDIUM_FLASH;
#endif
#if MICROPY_HW_HAS_SDCARD
// if an SD card is present then mount it on 1:/
if (reset_mode == 1 && sdcard_is_present()) {
FRESULT res = f_mount(&fatfs1, "1:", 1);
if (res != FR_OK) {
printf("[SD] could not mount SD card\n");
} else {
// use SD card as root device
root_device = 1;
if (first_soft_reset) {
// use SD card as medium for the USB MSD
#if defined(USE_DEVICE_MODE)
usb_medium = USB_STORAGE_MEDIUM_SDCARD;
#endif
}
}
}
#else
// Get rid of compiler warning if no SDCARD is configured.
(void)first_soft_reset;
#endif
// run <root>:/boot.py, if it exists
if (reset_mode == 1) {
const char *boot_file;
if (root_device == 0) {
boot_file = "0:/boot.py";
} else {
boot_file = "1:/boot.py";
}
FRESULT res = f_stat(boot_file, NULL);
if (res == FR_OK) {
if (!pyexec_file(boot_file)) {
flash_error(4);
}
}
}
// turn boot-up LEDs off
led_state(2, 0);
led_state(3, 0);
led_state(4, 0);
#if defined(USE_HOST_MODE)
// USB host
pyb_usb_host_init();
#elif defined(USE_DEVICE_MODE)
// USB device
if (reset_mode == 1) {
usb_device_mode_t usb_mode = USB_DEVICE_MODE_CDC_MSC;
if (pyb_config_usb_mode != MP_OBJ_NULL) {
if (strcmp(mp_obj_str_get_str(pyb_config_usb_mode), "CDC+HID") == 0) {
usb_mode = USB_DEVICE_MODE_CDC_HID;
}
}
pyb_usb_dev_init(usb_mode, usb_medium);
} else {
pyb_usb_dev_init(USB_DEVICE_MODE_CDC_MSC, usb_medium);
}
#endif
#if MICROPY_HW_ENABLE_RNG
// RNG
rng_init();
#endif
#if MICROPY_HW_ENABLE_TIMER
// timer
//timer_init();
#endif
// I2C
i2c_init();
#if MICROPY_HW_HAS_MMA7660
// MMA accel: init and reset
accel_init();
#endif
#if MICROPY_HW_ENABLE_SERVO
// servo
servo_init();
#endif
#if MICROPY_HW_ENABLE_DAC
// DAC
dac_init();
#endif
// now that everything is initialised, run main script
if (reset_mode == 1 && pyexec_mode_kind == PYEXEC_MODE_FRIENDLY_REPL) {
vstr_t *vstr = vstr_new();
vstr_printf(vstr, "%d:/", root_device);
if (pyb_config_main == MP_OBJ_NULL) {
vstr_add_str(vstr, "main.py");
} else {
vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main));
}
FRESULT res = f_stat(vstr_str(vstr), NULL);
if (res == FR_OK) {
if (!pyexec_file(vstr_str(vstr))) {
flash_error(3);
}
}
vstr_free(vstr);
}
#if 0
#if MICROPY_HW_HAS_WLAN
// wifi
pyb_wlan_init();
pyb_wlan_start();
#endif
#endif
// enter REPL
// REPL mode can change, or it can request a soft reset
for (;;) {
if (pyexec_mode_kind == PYEXEC_MODE_RAW_REPL) {
if (pyexec_raw_repl() != 0) {
break;
}
} else {
if (pyexec_friendly_repl() != 0) {
break;
}
}
}
printf("PYB: sync filesystems\n");
storage_flush();
printf("PYB: soft reboot\n");
first_soft_reset = false;
goto soft_reset;
}
/*!
* @brief Main demo function.
*/
int main (void)
{
tpm_general_config_t driverInfo;
accel_dev_t accDev;
accel_dev_interface_t accDevice;
accel_sensor_data_t accelData;
accel_i2c_interface_t i2cInterface;
tpm_pwm_param_t yAxisParams;
tpm_pwm_param_t xAxisParams;
int16_t xData, yData;
int16_t xAngle, yAngle;
xAxisParams.mode = kTpmEdgeAlignedPWM;
xAxisParams.edgeMode = kTpmHighTrue;
xAxisParams.uFrequencyHZ = 100000u;
xAxisParams.uDutyCyclePercent = 0u;
yAxisParams.mode = kTpmEdgeAlignedPWM;
yAxisParams.edgeMode = kTpmHighTrue;
yAxisParams.uFrequencyHZ = 100000u;
yAxisParams.uDutyCyclePercent = 0u;
// Register callback func for I2C
i2cInterface.i2c_init = I2C_DRV_MasterInit;
i2cInterface.i2c_read = I2C_DRV_MasterReceiveDataBlocking;
i2cInterface.i2c_write = I2C_DRV_MasterSendDataBlocking;
accDev.i2c = &i2cInterface;
accDev.accel = &accDevice;
accDev.slave.baudRate_kbps = BOARD_ACCEL_BAUDRATE;
accDev.slave.address = BOARD_ACCEL_ADDR;
accDev.bus = BOARD_ACCEL_I2C_INSTANCE;
// Initialize standard SDK demo application pins.
hardware_init();
// Accel device driver utilizes the OSA, so initialize it.
OSA_Init();
// Print the initial banner.
PRINTF("Bubble Level Demo!\r\n\r\n");
// Initialize the Accel.
accel_init(&accDev);
// Prepare memory for initialization.
memset(&driverInfo, 0, sizeof(driverInfo));
// Init TPM.
TPM_DRV_Init(BOARD_BUBBLE_TPM_INSTANCE, &driverInfo);
// Set clock for TPM.
TPM_DRV_SetClock(BOARD_BUBBLE_TPM_INSTANCE, kTpmClockSourceModuleClk, kTpmDividedBy2);
// Main loop. Get sensor data and update duty cycle for the TPM timer.
while(1)
{
// Wait 5 ms in between samples (accelerometer updates at 200Hz).
OSA_TimeDelay(5);
// Get new accelerometer data.
accDev.accel->accel_read_sensor_data(&accDev,&accelData);
// Init PWM module with updated configuration.
TPM_DRV_PwmStart(BOARD_BUBBLE_TPM_INSTANCE, &xAxisParams, BOARD_TPM_X_CHANNEL);
TPM_DRV_PwmStart(BOARD_BUBBLE_TPM_INSTANCE, &yAxisParams, BOARD_TPM_Y_CHANNEL);
// Get the X and Y data from the sensor data structure.fxos_data
xData = (int16_t)((accelData.data.accelXMSB << 8) | accelData.data.accelXLSB);
yData = (int16_t)((accelData.data.accelYMSB << 8) | accelData.data.accelYLSB);
// Convert raw data to angle (normalize to 0-90 degrees). No negative angles.
xAngle = abs((int16_t)(xData * 0.011));
yAngle = abs((int16_t)(yData * 0.011));
// Update angles to turn on LEDs when angles ~ 90
if(xAngle > 85) xAngle = 100;
if(yAngle > 85) yAngle = 100;
// Update angles to turn off LEDs when angles ~ 0
if(xAngle < 5) xAngle = 0;
if(yAngle < 5) yAngle = 0;
// Update pwm duty cycle
xAxisParams.uDutyCyclePercent = 100 - xAngle ;
yAxisParams.uDutyCyclePercent = 100 - yAngle ;
// Print out the raw accelerometer data.
PRINTF("x= %d y = %d\r\n", xData, yData);
}
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
/////SOCKET SETUP/////
//resolve arguments from command line
//if (argc < 2) {
// fprintf(stderr,"Error: need argument (component)\n");
// exit(0);
//}
//store arguments into variables
//strcpy(component, argv[1]);
//this is the port number that the edison reserves for the cloud?
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//lets only read acceleration form now
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
//gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
if (count == 3) {
//store accel data in string
x_accel_message = construct_accel_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_accel_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_accel_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
count = 0;
}
printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
int main(void) {
// TODO disable JTAG
// Stack limit should be less than real stack size, so we have a chance
// to recover from limit hit. (Limit is measured in bytes.)
mp_stack_set_limit((char*)&_ram_end - (char*)&_heap_end - 1024);
/* STM32F4xx HAL library initialization:
- Configure the Flash prefetch, instruction and Data caches
- Configure the Systick to generate an interrupt each 1 msec
- Set NVIC Group Priority to 4
- Global MSP (MCU Support Package) initialization
*/
HAL_Init();
// set the system clock to be HSE
SystemClock_Config();
// enable GPIO clocks
__GPIOA_CLK_ENABLE();
__GPIOB_CLK_ENABLE();
__GPIOC_CLK_ENABLE();
__GPIOD_CLK_ENABLE();
// enable the CCM RAM
__CCMDATARAMEN_CLK_ENABLE();
#if 0
#if defined(NETDUINO_PLUS_2)
{
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
#if MICROPY_HW_HAS_SDCARD
// Turn on the power enable for the sdcard (PB1)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET);
#endif
// Turn on the power for the 5V on the expansion header (PB2)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET);
}
#endif
#endif
// basic sub-system init
pendsv_init();
timer_tim3_init();
led_init();
#if MICROPY_HW_HAS_SWITCH
switch_init0();
#endif
int first_soft_reset = true;
soft_reset:
// check if user switch held to select the reset mode
led_state(1, 0);
led_state(2, 1);
led_state(3, 0);
led_state(4, 0);
uint reset_mode = 1;
#if MICROPY_HW_HAS_SWITCH
if (switch_get()) {
for (uint i = 0; i < 3000; i++) {
if (!switch_get()) {
break;
}
HAL_Delay(20);
if (i % 30 == 29) {
if (++reset_mode > 3) {
reset_mode = 1;
}
led_state(2, reset_mode & 1);
led_state(3, reset_mode & 2);
led_state(4, reset_mode & 4);
}
}
// flash the selected reset mode
for (uint i = 0; i < 6; i++) {
led_state(2, 0);
led_state(3, 0);
led_state(4, 0);
HAL_Delay(50);
led_state(2, reset_mode & 1);
led_state(3, reset_mode & 2);
led_state(4, reset_mode & 4);
HAL_Delay(50);
}
HAL_Delay(400);
}
#endif
#if MICROPY_HW_ENABLE_RTC
if (first_soft_reset) {
rtc_init();
}
#endif
// more sub-system init
#if MICROPY_HW_HAS_SDCARD
if (first_soft_reset) {
sdcard_init();
}
#endif
if (first_soft_reset) {
storage_init();
}
// GC init
gc_init(&_heap_start, &_heap_end);
// Micro Python init
mp_init();
mp_obj_list_init(mp_sys_path, 0);
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_)); // current dir (or base dir of the script)
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_flash));
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_flash_slash_lib));
mp_obj_list_init(mp_sys_argv, 0);
// Change #if 0 to #if 1 if you want REPL on UART_6 (or another uart)
// as well as on USB VCP
#if 0
{
mp_obj_t args[2] = {
MP_OBJ_NEW_SMALL_INT(PYB_UART_6),
MP_OBJ_NEW_SMALL_INT(115200),
};
pyb_stdio_uart = pyb_uart_type.make_new((mp_obj_t)&pyb_uart_type, MP_ARRAY_SIZE(args), 0, args);
}
#else
pyb_stdio_uart = NULL;
#endif
// Initialise low-level sub-systems. Here we need to very basic things like
// zeroing out memory and resetting any of the sub-systems. Following this
// we can run Python scripts (eg boot.py), but anything that is configurable
// by boot.py must be set after boot.py is run.
readline_init0();
pin_init0();
extint_init0();
timer_init0();
uart_init0();
#if MICROPY_HW_ENABLE_RNG
rng_init0();
#endif
i2c_init0();
spi_init0();
pyb_usb_init0();
// Initialise the local flash filesystem.
// Create it if needed, and mount in on /flash.
{
// try to mount the flash
FRESULT res = f_mount(&fatfs0, "/flash", 1);
if (reset_mode == 3 || res == FR_NO_FILESYSTEM) {
// no filesystem, or asked to reset it, so create a fresh one
// LED on to indicate creation of LFS
led_state(PYB_LED_R2, 1);
uint32_t start_tick = HAL_GetTick();
res = f_mkfs("/flash", 0, 0);
if (res == FR_OK) {
// success creating fresh LFS
} else {
__fatal_error("could not create LFS");
}
// set label
f_setlabel("/flash/pybflash");
// create empty main.py
FIL fp;
f_open(&fp, "/flash/main.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// create .inf driver file
f_open(&fp, "/flash/pybcdc.inf", FA_WRITE | FA_CREATE_ALWAYS);
f_write(&fp, fresh_pybcdc_inf, sizeof(fresh_pybcdc_inf) - 1 /* don't count null terminator */, &n);
f_close(&fp);
// create readme file
f_open(&fp, "/flash/README.txt", FA_WRITE | FA_CREATE_ALWAYS);
f_write(&fp, fresh_readme_txt, sizeof(fresh_readme_txt) - 1 /* don't count null terminator */, &n);
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(start_tick, 200);
led_state(PYB_LED_R2, 0);
} else if (res == FR_OK) {
// mount sucessful
} else {
__fatal_error("could not access LFS");
}
}
// The current directory is used as the boot up directory.
// It is set to the internal flash filesystem by default.
f_chdrive("/flash");
// Make sure we have a /flash/boot.py. Create it if needed.
{
FILINFO fno;
#if _USE_LFN
fno.lfname = NULL;
fno.lfsize = 0;
#endif
FRESULT res = f_stat("/flash/boot.py", &fno);
if (res == FR_OK) {
if (fno.fattrib & AM_DIR) {
// exists as a directory
// TODO handle this case
// see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation
} else {
// exists as a file, good!
}
} else {
// doesn't exist, create fresh file
// LED on to indicate creation of boot.py
led_state(PYB_LED_R2, 1);
uint32_t start_tick = HAL_GetTick();
FIL fp;
f_open(&fp, "/flash/boot.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(start_tick, 200);
led_state(PYB_LED_R2, 0);
}
}
#if defined(USE_DEVICE_MODE)
usb_storage_medium_t usb_medium = USB_STORAGE_MEDIUM_FLASH;
#endif
#if MICROPY_HW_HAS_SDCARD
// if an SD card is present then mount it on /sd/
if (sdcard_is_present()) {
FRESULT res = f_mount(&fatfs1, "/sd", 1);
if (res != FR_OK) {
printf("[SD] could not mount SD card\n");
} else {
// use SD card as current directory
f_chdrive("/sd");
// TODO these should go before the /flash entries in the path
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_sd));
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR__slash_sd_slash_lib));
if (first_soft_reset) {
// use SD card as medium for the USB MSD
#if defined(USE_DEVICE_MODE)
usb_medium = USB_STORAGE_MEDIUM_SDCARD;
#endif
}
}
}
#endif
// reset config variables; they should be set by boot.py
pyb_config_main = MP_OBJ_NULL;
pyb_config_usb_mode = MP_OBJ_NULL;
// run boot.py, if it exists
// TODO perhaps have pyb.reboot([bootpy]) function to soft-reboot and execute custom boot.py
if (reset_mode == 1) {
const char *boot_py = "boot.py";
FRESULT res = f_stat(boot_py, NULL);
if (res == FR_OK) {
int ret = pyexec_file(boot_py);
if (ret & PYEXEC_FORCED_EXIT) {
goto soft_reset_exit;
}
if (!ret) {
flash_error(4);
}
}
}
// turn boot-up LEDs off
led_state(2, 0);
led_state(3, 0);
led_state(4, 0);
// Now we initialise sub-systems that need configuration from boot.py,
// or whose initialisation can be safely deferred until after running
// boot.py.
#if defined(USE_HOST_MODE)
// USB host
pyb_usb_host_init();
#elif defined(USE_DEVICE_MODE)
// USB device
usb_device_mode_t usb_mode = USB_DEVICE_MODE_CDC_MSC;
// if we are not in reset_mode==1, this config variable will always be NULL
if (pyb_config_usb_mode != MP_OBJ_NULL) {
if (strcmp(mp_obj_str_get_str(pyb_config_usb_mode), "CDC+HID") == 0) {
usb_mode = USB_DEVICE_MODE_CDC_HID;
}
}
pyb_usb_dev_init(usb_mode, usb_medium);
#endif
#if MICROPY_HW_HAS_MMA7660
// MMA accel: init and reset
accel_init();
#endif
#if MICROPY_HW_ENABLE_SERVO
// servo
servo_init();
#endif
#if MICROPY_HW_ENABLE_DAC
// DAC
dac_init();
#endif
mod_network_init();
// At this point everything is fully configured and initialised.
// Run the main script from the current directory.
if (reset_mode == 1 && pyexec_mode_kind == PYEXEC_MODE_FRIENDLY_REPL) {
const char *main_py;
if (pyb_config_main == MP_OBJ_NULL) {
main_py = "main.py";
} else {
main_py = mp_obj_str_get_str(pyb_config_main);
}
FRESULT res = f_stat(main_py, NULL);
if (res == FR_OK) {
int ret = pyexec_file(main_py);
if (ret & PYEXEC_FORCED_EXIT) {
goto soft_reset_exit;
}
if (!ret) {
flash_error(3);
}
}
}
// Main script is finished, so now go into REPL mode.
// The REPL mode can change, or it can request a soft reset.
for (;;) {
if (pyexec_mode_kind == PYEXEC_MODE_RAW_REPL) {
if (pyexec_raw_repl() != 0) {
break;
}
} else {
if (pyexec_friendly_repl() != 0) {
break;
}
}
}
soft_reset_exit:
// soft reset
printf("PYB: sync filesystems\n");
storage_flush();
printf("PYB: soft reboot\n");
timer_deinit();
uart_deinit();
first_soft_reset = false;
goto soft_reset;
}
/*!
* @brief Main demo function.
*/
int main (void)
{
ftm_pwm_param_t xAxisParams, yAxisParams;
accel_dev_t accDev;
accel_dev_interface_t accDevice;
accel_sensor_data_t accelData;
accel_i2c_interface_t i2cInterface;
int16_t xData, yData;
int16_t xAngle, yAngle;
uint32_t ftmModulo;
// Register callback func for I2C
i2cInterface.i2c_init = I2C_DRV_MasterInit;
i2cInterface.i2c_read = I2C_DRV_MasterReceiveDataBlocking;
i2cInterface.i2c_write = I2C_DRV_MasterSendDataBlocking;
accDev.i2c = &i2cInterface;
accDev.accel = &accDevice;
accDev.slave.baudRate_kbps = BOARD_ACCEL_BAUDRATE;
accDev.slave.address = BOARD_ACCEL_ADDR;
accDev.bus = BOARD_ACCEL_I2C_INSTANCE;
// Initialize standard SDK demo application pins.
hardware_init();
// Accel device driver utilizes the OSA, so initialize it.
OSA_Init();
// Initialize the LEDs used by this application.
LED2_EN;
LED3_EN;
// Print the initial banner.
PRINTF("Bubble Level Demo!\r\n\r\n");
// Initialize the Accel.
accel_init(&accDev);
// Turn on the clock to the FTM.
CLOCK_SYS_EnableFtmClock(BOARD_FTM_INSTANCE);
// Initialize the FTM module.
FTM_HAL_Init(BOARD_FTM_BASE);
// Configure the sync mode to software.
FTM_HAL_SetSyncMode(BOARD_FTM_BASE, kFtmUseSoftwareTrig);
// Enable the overflow interrupt.
FTM_HAL_EnableTimerOverflowInt(BOARD_FTM_BASE);
// Set the FTM clock divider to /16.
FTM_HAL_SetClockPs(BOARD_FTM_BASE, kFtmDividedBy16);
// Configure the FTM channel used for the X-axis. Initial duty cycle is 0%.
xAxisParams.mode = kFtmEdgeAlignedPWM;
xAxisParams.edgeMode = kFtmHighTrue;
FTM_HAL_EnablePwmMode(BOARD_FTM_BASE, &xAxisParams, BOARD_FTM_X_CHANNEL);
FTM_HAL_SetChnCountVal(BOARD_FTM_BASE, BOARD_FTM_X_CHANNEL, 0);
// Configure the FTM channel used for the Y-axis. Initial duty cycle is 0%.
yAxisParams.mode = kFtmEdgeAlignedPWM;
yAxisParams.edgeMode = kFtmHighTrue;
FTM_HAL_EnablePwmMode(BOARD_FTM_BASE, &yAxisParams, BOARD_FTM_Y_CHANNEL);
FTM_HAL_SetChnCountVal(BOARD_FTM_BASE, BOARD_FTM_Y_CHANNEL, 0);
// Get the FTM reference clock and calculate the modulo value.
ftmModulo = (CLOCK_SYS_GetFtmSystemClockFreq(BOARD_FTM_INSTANCE) /
(1 << FTM_HAL_GetClockPs(BOARD_FTM_BASE))) /
(BOARD_FTM_PERIOD_HZ - 1);
// Initialize the FTM counter.
FTM_HAL_SetCounterInitVal(BOARD_FTM_BASE, 0);
FTM_HAL_SetMod(BOARD_FTM_BASE, ftmModulo);
// Set the clock source to start the FTM.
FTM_HAL_SetClockSource(BOARD_FTM_BASE, kClock_source_FTM_SystemClk);
// Enable the FTM interrupt at the NVIC level.
INT_SYS_EnableIRQ(BOARD_FTM_IRQ_VECTOR);
// Main loop. Get sensor data and update globals for the FTM timer update.
while(1)
{
// Wait 5 ms in between samples (accelerometer updates at 200Hz).
OSA_TimeDelay(5);
// Get new accelerometer data.
accDev.accel->accel_read_sensor_data(&accDev,&accelData);
// Turn off interrupts (FTM) while updating new duty cycle values.
INT_SYS_DisableIRQGlobal();
// Get the X and Y data from the sensor data structure.
xData = (int16_t)((accelData.data.accelXMSB << 8) | accelData.data.accelXLSB);
yData = (int16_t)((accelData.data.accelYMSB << 8) | accelData.data.accelYLSB);
// Convert raw data to angle (normalize to 0-90 degrees). No negative
// angles.
xAngle = abs((int16_t)(xData * 0.011));
yAngle = abs((int16_t)(yData * 0.011));
// Set values for next FTM ISR udpate. Use 5 degrees as the threshold
// for whether to turn the LED on or not.
g_xValue = (xAngle > 5) ? (uint16_t)((xAngle / 90.0) * ftmModulo) : 0;
g_yValue = (yAngle > 5) ? (uint16_t)((yAngle / 90.0) * ftmModulo) : 0;
// Re-enable interrupts.
INT_SYS_EnableIRQGlobal();
// Print out the raw accelerometer data.
PRINTF("x= %d y = %d\r\n", xData, yData);
}
}
/*******************************************************************
* 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);
}
}
int main(void) {
// TODO disable JTAG
// update the SystemCoreClock variable
SystemCoreClockUpdate();
// set interrupt priority config to use all 4 bits for pre-empting
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
// enable the CCM RAM and the GPIO's
RCC->AHB1ENR |= RCC_AHB1ENR_CCMDATARAMEN | RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN;
#if MICROPY_HW_HAS_SDCARD
{
// configure SDIO pins to be high to start with (apparently makes it more robust)
// FIXME this is not making them high, it just makes them outputs...
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Configure PD.02 CMD line
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}
#endif
#if defined(NETDUINO_PLUS_2)
{
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
#if MICROPY_HW_HAS_SDCARD
// Turn on the power enable for the sdcard (PB1)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_1, Bit_SET);
#endif
// Turn on the power for the 5V on the expansion header (PB2)
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_2, Bit_SET);
}
#endif
// basic sub-system init
sys_tick_init();
pendsv_init();
led_init();
#if MICROPY_HW_ENABLE_RTC
rtc_init();
#endif
// turn on LED to indicate bootup
led_state(PYB_LED_G1, 1);
// more sub-system init
#if MICROPY_HW_HAS_SDCARD
sdcard_init();
#endif
storage_init();
// uncomment these 2 lines if you want REPL on USART_6 (or another usart) as well as on USB VCP
//pyb_usart_global_debug = PYB_USART_YA;
//usart_init(pyb_usart_global_debug, 115200);
int first_soft_reset = true;
soft_reset:
// GC init
gc_init(&_heap_start, &_heap_end);
// Micro Python init
qstr_init();
mp_init();
mp_obj_list_init(mp_sys_path, 0);
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_));
mp_obj_list_append(mp_sys_path, MP_OBJ_NEW_QSTR(MP_QSTR_0_colon__slash_lib));
mp_obj_list_init(mp_sys_argv, 0);
exti_init();
#if MICROPY_HW_HAS_SWITCH
switch_init();
#endif
#if MICROPY_HW_HAS_LCD
// LCD init (just creates class, init hardware by calling LCD())
lcd_init();
#endif
#if MICROPY_HW_ENABLE_SERVO
// servo
servo_init();
#endif
#if MICROPY_HW_ENABLE_TIMER
// timer
timer_init();
#endif
#if MICROPY_HW_ENABLE_RNG
// RNG
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_RNG, ENABLE);
RNG_Cmd(ENABLE);
#endif
pin_map_init();
// add some functions to the builtin Python namespace
mp_store_name(MP_QSTR_help, mp_make_function_n(0, pyb_help));
mp_store_name(MP_QSTR_open, mp_make_function_n(2, pyb_io_open));
// load the pyb module
mp_module_register(MP_QSTR_pyb, (mp_obj_t)&pyb_module);
// check if user switch held (initiates reset of filesystem)
bool reset_filesystem = false;
#if MICROPY_HW_HAS_SWITCH
if (switch_get()) {
reset_filesystem = true;
for (int i = 0; i < 50; i++) {
if (!switch_get()) {
reset_filesystem = false;
break;
}
sys_tick_delay_ms(10);
}
}
#endif
// local filesystem init
{
// try to mount the flash
FRESULT res = f_mount(&fatfs0, "0:", 1);
if (!reset_filesystem && res == FR_OK) {
// mount sucessful
} else if (reset_filesystem || res == FR_NO_FILESYSTEM) {
// no filesystem, so create a fresh one
// TODO doesn't seem to work correctly when reset_filesystem is true...
// LED on to indicate creation of LFS
led_state(PYB_LED_R2, 1);
uint32_t stc = sys_tick_counter;
res = f_mkfs("0:", 0, 0);
if (res == FR_OK) {
// success creating fresh LFS
} else {
__fatal_error("could not create LFS");
}
// create src directory
res = f_mkdir("0:/src");
// ignore result from mkdir
// create empty main.py
FIL fp;
f_open(&fp, "0:/src/main.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_main_py, sizeof(fresh_main_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(stc, 200);
led_state(PYB_LED_R2, 0);
} else {
__fatal_error("could not access LFS");
}
}
// make sure we have a /boot.py
{
FILINFO fno;
FRESULT res = f_stat("0:/boot.py", &fno);
if (res == FR_OK) {
if (fno.fattrib & AM_DIR) {
// exists as a directory
// TODO handle this case
// see http://elm-chan.org/fsw/ff/img/app2.c for a "rm -rf" implementation
} else {
// exists as a file, good!
}
} else {
// doesn't exist, create fresh file
// LED on to indicate creation of boot.py
led_state(PYB_LED_R2, 1);
uint32_t stc = sys_tick_counter;
FIL fp;
f_open(&fp, "0:/boot.py", FA_WRITE | FA_CREATE_ALWAYS);
UINT n;
f_write(&fp, fresh_boot_py, sizeof(fresh_boot_py) - 1 /* don't count null terminator */, &n);
// TODO check we could write n bytes
f_close(&fp);
// keep LED on for at least 200ms
sys_tick_wait_at_least(stc, 200);
led_state(PYB_LED_R2, 0);
}
}
// run /boot.py
if (!pyexec_file("0:/boot.py")) {
flash_error(4);
}
if (first_soft_reset) {
#if MICROPY_HW_HAS_MMA7660
// MMA accel: init and reset address to zero
accel_init();
#endif
}
// turn boot-up LED off
led_state(PYB_LED_G1, 0);
#if MICROPY_HW_HAS_SDCARD
// if an SD card is present then mount it on 1:/
if (sdcard_is_present()) {
FRESULT res = f_mount(&fatfs1, "1:", 1);
if (res != FR_OK) {
printf("[SD] could not mount SD card\n");
} else {
if (first_soft_reset) {
// use SD card as medium for the USB MSD
usbd_storage_select_medium(USBD_STORAGE_MEDIUM_SDCARD);
}
}
}
#endif
#ifdef USE_HOST_MODE
// USB host
pyb_usb_host_init();
#elif defined(USE_DEVICE_MODE)
// USB device
pyb_usb_dev_init(PYB_USB_DEV_VCP_MSC);
#endif
// run main script
{
vstr_t *vstr = vstr_new();
vstr_add_str(vstr, "0:/");
if (pyb_config_source_dir == MP_OBJ_NULL) {
vstr_add_str(vstr, "src");
} else {
vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_source_dir));
}
vstr_add_char(vstr, '/');
if (pyb_config_main == MP_OBJ_NULL) {
vstr_add_str(vstr, "main.py");
} else {
vstr_add_str(vstr, mp_obj_str_get_str(pyb_config_main));
}
if (!pyexec_file(vstr_str(vstr))) {
flash_error(3);
}
vstr_free(vstr);
}
#if MICROPY_HW_HAS_MMA7660
// HID example
if (0) {
uint8_t data[4];
data[0] = 0;
data[1] = 1;
data[2] = -2;
data[3] = 0;
for (;;) {
#if MICROPY_HW_HAS_SWITCH
if (switch_get()) {
data[0] = 0x01; // 0x04 is middle, 0x02 is right
} else {
data[0] = 0x00;
}
#else
data[0] = 0x00;
#endif
accel_start(0x4c /* ACCEL_ADDR */, 1);
accel_send_byte(0);
accel_restart(0x4c /* ACCEL_ADDR */, 0);
for (int i = 0; i <= 1; i++) {
int v = accel_read_ack() & 0x3f;
if (v & 0x20) {
v |= ~0x1f;
}
data[1 + i] = v;
}
accel_read_nack();
usb_hid_send_report(data);
sys_tick_delay_ms(15);
}
}
#endif
#if MICROPY_HW_HAS_WLAN
// wifi
pyb_wlan_init();
pyb_wlan_start();
#endif
pyexec_repl();
printf("PYB: sync filesystems\n");
storage_flush();
printf("PYB: soft reboot\n");
first_soft_reset = false;
goto soft_reset;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data, zero_rate;
int16_t temperature;
float pitch_angle, roll_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char posture_message[20];
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
char serv_message[256];
int count;
int n, n1;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
zero_rate = calc_gyro_offset(gyro, g_res);
portno = 2015;
//create socket
if (argc > 1) //if user supplies IP address as argument
upperbodyIP = argv[1]; //take user input as upper body IP
else
upperbodyIP = "192.168.0.30";
sockfd = socket(AF_INET, SOCK_STREAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname(upperbodyIP); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//WAIT FOR THE OKAY BY THE SERVER//
memset(serv_message, 0, 256);
n = read(sockfd, serv_message, 256);
if (strcmp(serv_message, "START") != 0)
error("strange response from server");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, gyro_data, zero_rate, &pitch_angle, &roll_angle, &yaw_angle);
printf("is moving: %d ", isMoving(gyro_data));
printf("yaw angle: %f ", yaw_angle);
if (count == 3) {
//send posture to main edison
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, roll_angle);
else
curr_posture = UNDEFINED; //else just use the undefined/transition case
prev_posture = curr_posture; //set new value for prev posture
memset(posture_message, 0, sizeof(char)*20);
snprintf(posture_message, 10, "%d,%f", curr_posture, yaw_angle);
//posture_message = construct_message(POS, accel_data, curr_posture);
printf("posture message: %s ", posture_message);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
/*
bzero(message, 256);
printf("waiting for response ");
n1 = read(sockfd, message, 10);
if (n1 < 0)
error("ERROR reading from upper body");
printf("got response\n");
*/
count = 0;
}
printPostureString(curr_posture);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
int main(void) {
Geiger g;
power_initialise();
if(power_battery_level() < 1) {
power_standby();
}
flashstorage_initialise();
buzzer_initialise();
realtime_initialise();
g.initialise();
uint8_t *private_key = ((uint8_t *) &_binary___binary_data_private_key_data_start);
if(private_key[0] != 0) delay_us(1000);
delay_us(10000); // can be removed?
#ifndef DISABLE_ACCEL
accel_init();
#endif
Controller c(g);
switch_initialise();
// if we woke up on an alarm, we're going to be sending the system back.
#ifndef NEVERSLEEP
if(power_get_wakeup_source() == WAKEUP_RTC) {
c.m_sleeping = true;
} else {
buzzer_nonblocking_buzz(0.05);
display_initialise();
const char *devicetag = flashstorage_keyval_get("DEVICETAG");
char revtext[10];
sprintf(revtext,"VERSION: %s ",OS100VERSION);
display_splashscreen(devicetag,revtext);
delay_us(3000000);
display_clear(0);
}
#endif
#ifdef NEVERSLEEP
buzzer_nonblocking_buzz(0.05);
display_initialise();
#endif
GUI m_gui(c);
bool full = flashstorage_log_isfull();
if((full == true) && (c.m_sleeping == false)) {
m_gui.show_dialog("Flash Log","is full",0,0,0);
}
c.set_gui(m_gui);
UserInput u(m_gui);
u.initialise();
serial_initialise();
int8_t utcoffsetmins_n = 0;
const char *utcoffsetmins = flashstorage_keyval_get("UTCOFFSETMINS");
if(utcoffsetmins != 0) {
unsigned int c;
sscanf(utcoffsetmins, "%u", &c);
utcoffsetmins_n = c;
realtime_setutcoffset_mins(utcoffsetmins_n);
}
// Need to refactor out stored settings
if(c.m_sleeping == false) {
const char *sbright = flashstorage_keyval_get("BRIGHTNESS");
if(sbright != 0) {
unsigned int c;
sscanf(sbright, "%u", &c);
display_set_brightness(c);
}
const char *sbeep = flashstorage_keyval_get("GEIGERBEEP");
if(sbeep != 0) {
if(strcmp(sbeep,"true") == 0) { g.set_beep(true); tick_item("Geiger Beep",true); }
else g.set_beep(false);
}
const char *scpmcps = flashstorage_keyval_get("CPMCPSAUTO");
if(scpmcps != 0) {
if(strcmp(scpmcps,"true") == 0) { c.m_cpm_cps_switch = true; tick_item("CPM/CPS Auto",true); }
}
const char *language = flashstorage_keyval_get("LANGUAGE");
if(language != 0) {
if(strcmp(language,"English" ) == 0) { m_gui.set_language(LANGUAGE_ENGLISH); tick_item("English" ,true); } else
if(strcmp(language,"Japanese") == 0) { m_gui.set_language(LANGUAGE_JAPANESE); tick_item("Japanese" ,true); }
} else {
m_gui.set_language(LANGUAGE_ENGLISH);
tick_item("English",true);
}
const char *svrem = flashstorage_keyval_get("SVREM");
if(strcmp(svrem,"REM") == 0) { tick_item("Roentgen",true); }
else { tick_item("Sievert",true);}
}
m_gui.jump_to_screen(1);
m_gui.push_stack(0,1);
for(;;) {
if(power_battery_level() < 1) {
power_standby();
}
//display_draw_text(0,110,"preupdate",0);
c.update();
//display_draw_text(0,110,"prerender",0);
m_gui.render();
//display_draw_text(0,110,"preserial",0);
serial_eventloop();
//display_draw_text(0,110,"preserial",0);
// It might be a good idea to move the following code to Controller.
// Hack to check that captouch is ok, and reset it if not.
bool c = cap_check();
if(c == false) {
display_draw_text(0,90,"CAPFAIL",0);
cap_init();
}
// Screen lock code
uint32_t release1_time = cap_last_press(KEY_BACK);
uint32_t press1_time = cap_last_release(KEY_BACK);
uint32_t release2_time = cap_last_press(KEY_SELECT);
uint32_t press2_time = cap_last_release(KEY_SELECT);
uint32_t current_time = realtime_get_unixtime();
if((release1_time != 0) &&
(release2_time != 0) &&
((current_time-press1_time) > 3) &&
((current_time-press2_time) > 3) &&
cap_ispressed(KEY_BACK ) &&
cap_ispressed(KEY_SELECT)) {
system_gui->toggle_screen_lock();
cap_clear_press();
}
power_wfi();
}
// should never get here
for(int n=0;n<60;n++) {
delay_us(100000);
buzzer_blocking_buzz(1000);
}
return 0;
}
