void init_timer0(void) { T0TC=0; T0TCR=0x0; //Reset timer0 T0MCR=0x3; //Interrupt on match MR0 and reset counter T0PR=0; T0PC=0; //Prescale Counter = 0 T0MR0=peripheralClockFrequency()/ControllerCyclesPerSecond; // /200 => 200 Hz Period T0TCR=0x1; //Set timer0 }
// COMPUTER VISION GROUP // Pelican Command Communications - HL Comm // Charlie De Vivero 2010 void HLC_Protocol(char control) { //printf(" hlc_PROTOCOL, %d \n\r", timeCount); /*for (unsigned short i = 0; i < 256; i++) { printf("%c", HLC_buffer[i]); } printf("\n\r");*/ // test code // if (timeCount % 100 > 50) { //if (timeCount % 100 ==1) { //printf(" Swithc control State 0X%x , control %c \n\r" ,controlState,control); switch (controlState) { case HLC_START_SEQ: //printf("%c start seq, 0x%x\n\r", control, controlState); if (control == '$') controlState = HLC_REQ_SEQ1; else if (control == '#') controlState = HLC_CMD_SEQ1; else controlState = HLC_START_SEQ; break; case HLC_REQ_SEQ1: //printf("%c req seq 1, 0x%x\n\r", control, controlState); if (control == '>') controlState = HLC_REQ_SEQ2; else controlState = HLC_START_SEQ; break; case HLC_REQ_SEQ2: //printf("%c req seq 2, 0x%x\n\r", control, controlState); if (control == 'v' && verboseMode == 0) { controlState = HLC_REQ_SEQ2; verboseMode = 1; } else if (control == RC_DATA_id) { if (verboseMode) { printf("RC_DATA -\n\r"); printf("Pitch (Channel 1) = %d\n\r",RO_RC_Data.channel[0]); printf("Roll (Channel 2) = %d\n\r",RO_RC_Data.channel[1]); printf("Thrust (Channel 3) = %d\n\r",RO_RC_Data.channel[2]); printf("Yaw (Channel 4) = %d\n\r",RO_RC_Data.channel[3]); printf("Serial (Channel 5) = %d\n\r",RO_RC_Data.channel[4]); printf("Auto (Channel 6) = %d\n\r",RO_RC_Data.channel[5]); } else { char packet[sizeof(HEADER) + sizeof(RC_DATA) + sizeof(FOOTER)]; HEADER *header = (HEADER *)packet; RC_DATA *rc_data = (RC_DATA *)&packet[sizeof(HEADER)]; FOOTER *footer = (FOOTER *)&packet[sizeof(HEADER) + sizeof(RC_DATA)]; // printf("On send packets,\n\r"); rc_data->pitch_ch = RO_RC_Data.channel[0]; // printf("Pitch ch \n\r"); rc_data->roll_ch = RO_RC_Data.channel[1]; // printf("Roll ch \n\r"); rc_data->thrust_ch = RO_RC_Data.channel[2]; rc_data->yaw_ch = RO_RC_Data.channel[3]; rc_data->ser_ch = RO_RC_Data.channel[4]; rc_data->auto_ch = RO_RC_Data.channel[5]; // printf("auto ch \n\r"); // printf("Header packets, 0x%x\n\r", control, controlState); //strcpy(header.start,"$>");//header->start = "$>"; header->start[0] = '$'; header->start[1] = '>'; header->length = (short)sizeof(RC_DATA); header->id = RC_DATA_id; // printf("footer packets, 0x%x\n\r", control, controlState); footer->checksum = HLC_Checksum((char *)&rc_data, sizeof(RC_DATA)); footer->stop[0] = '<'; footer->stop[1] = '@'; //strcpy(header.start,"<@");////footer->stop = "<@"; UART_SendPacket_raw(packet, sizeof(packet)); /* // printf("Assembling packets, 0x%x\n\r", control, controlState); int index = 0; for (int i = 0; i < sizeof(HEADER); i++) packet[index + i] = *((char *)&header + i); index += sizeof(HEADER); for (int i = 0; i < sizeof(RC_DATA); i++) packet[index + i] = *((char *)&rc_data + i); index += sizeof(RC_DATA); for (int i = 0; i < sizeof(FOOTER); i++) packet[index + i] = *((char *)&footer + i); index += sizeof(FOOTER); // printf("before send packets, 0x%x\n\r", control, controlState); UART_SendPacket_raw(packet, index); // printf("after send packets, 0x%x\n\r", control, controlState); */ } verboseMode = 0; controlState = HLC_START_SEQ; } else if (control == STATE_id) { if (verboseMode) { /*printf("STATE -\n\r"); printf("Pitch (deg) = %f\n\r",LL_1khz_attitude_data.angle_pitch * 0.01); printf("Roll (deg) = %f\n\r",LL_1khz_attitude_data.angle_roll * 0.01); printf("Yaw (deg) = %f\n\r",LL_1khz_attitude_data.angle_yaw * 0.01); printf("Pitch Vel (deg/s) = %f\n\r",LL_1khz_attitude_data.angvel_pitch * 0.015); printf("Roll Vel (deg/s) = %f\n\r",LL_1khz_attitude_data.angvel_roll * 0.015); printf("Yaw Vel (deg/s) = %f\n\r",LL_1khz_attitude_data.angvel_yaw * 0.015); printf("Latitude = %d\n\r",LL_1khz_attitude_data.latitude_best_estimate); printf("Longitude = %d\n\r",LL_1khz_attitude_data.longitude_best_estimate); printf("Height (m) = %f\n\r",LL_1khz_attitude_data.height * 0.001); printf("X Vel = %d\n\r",LL_1khz_attitude_data.speed_x_best_estimate); printf("Y Vel = %d\n\r",LL_1khz_attitude_data.speed_y_best_estimate); printf("Z Vel (m/s) = %f\n\r",LL_1khz_attitude_data.dheight * 0.001); printf("X Acc (g) = %f\n\r",LL_1khz_attitude_data.acc_x * 0.001); printf("Y Acc (g) = %f\n\r",LL_1khz_attitude_data.acc_y * 0.001); printf("Z Acc (g) = %f\n\r",LL_1khz_attitude_data.acc_z * 0.001); printf("Flight mode = %d\n\r",LL_1khz_attitude_data.flightMode); */ } else { char packet[sizeof(HEADER) + sizeof(STATE) + sizeof(FOOTER)]; HEADER *header = (HEADER *)packet; STATE *state = (STATE *)&packet[sizeof(HEADER)]; FOOTER *footer = (FOOTER *)&packet[sizeof(HEADER) + sizeof(STATE)]; state->pitch_ang = LL_1khz_attitude_data.angle_pitch; state->roll_ang = LL_1khz_attitude_data.angle_roll; state->yaw_ang = LL_1khz_attitude_data.angle_yaw; state->pitch_vel = LL_1khz_attitude_data.angvel_pitch; state->roll_vel = LL_1khz_attitude_data.angvel_roll; state->yaw_vel = LL_1khz_attitude_data.angvel_yaw; state->latitude = LL_1khz_attitude_data.latitude_best_estimate; //LL_1khz_control_input.latitude; state->longitude = LL_1khz_attitude_data.longitude_best_estimate; // LL_1khz_control_input.longitude; state->height = LL_1khz_attitude_data.height; // LL_1khz_control_input.height; state->x_vel = (short)(x_vel / peripheralClockFrequency()); // LL_1khz_attitude_data.speed_x_best_estimate; state->y_vel = (short)(y_vel / peripheralClockFrequency()); // LL_1khz_attitude_data.speed_y_best_estimate; state->z_vel = LL_1khz_attitude_data.dheight; state->x_acc = LL_1khz_attitude_data.acc_x; state->y_acc = LL_1khz_attitude_data.acc_y; state->z_acc = LL_1khz_attitude_data.acc_z; state->flight_mode = LL_1khz_attitude_data.flightMode; header->start[0] = '$'; header->start[1] = '>'; //strcpy(header.start,"$>");//header.start = "$>"; header->length = (short)sizeof(STATE); header->id = STATE_id; footer->checksum = HLC_Checksum((char *)&state, sizeof(STATE)); footer->stop[0] = '<'; footer->stop[1] = '@'; //strcpy(footer.stop,"<@");//footer.stop = "<@"; UART_SendPacket_raw(packet, sizeof(packet)); /* int index = 0; for (int i = 0; i < sizeof(HEADER); i++) packet[index + i] = *((char *)&header + i); index += sizeof(HEADER); for (int i = 0; i < sizeof(STATE); i++) packet[index + i] = *((char *)&state + i); index += sizeof(STATE); for (int i = 0; i < sizeof(FOOTER); i++) packet[index + i] = *((char *)&footer + i); index += sizeof(FOOTER); UART_SendPacket_raw(packet, index); */ } verboseMode = 0; controlState = HLC_START_SEQ; } else if (control == SENSOR_id) { if (verboseMode) { /*printf("SENSOR -\n\r"); printf("Mag X Reading = %d\n\r",LL_1khz_attitude_data.mag_x); printf("Mag Y Reading = %d\n\r",LL_1khz_attitude_data.mag_y); printf("Mag Z Reading = %d\n\r",LL_1khz_attitude_data.mag_z); printf("Camera Status = %d\n\r",LL_1khz_attitude_data.cam_status); printf("Camera Pitch = %d\n\r",LL_1khz_attitude_data.cam_status); printf("Camera Roll = %d\n\r",LL_1khz_attitude_data.cam_angle_roll); printf("Battery (V) = %f\n\r",LL_1khz_attitude_data.battery_voltage1 * 0.001); printf("Flight Time = %d\n\r",LL_1khz_attitude_data.flight_time); printf("CPU Load = %d\n\r",LL_1khz_attitude_data.cpu_load); */ } else { char packet[sizeof(HEADER) + sizeof(SENSOR) + sizeof(FOOTER)]; HEADER *header = (HEADER *)packet; SENSOR *sensor = (SENSOR *)&packet[sizeof(HEADER)]; FOOTER *footer = (FOOTER *)&packet[sizeof(HEADER) + sizeof(SENSOR)]; sensor->mag_x = LL_1khz_attitude_data.mag_x; sensor->mag_y = LL_1khz_attitude_data.mag_y; sensor->mag_z = LL_1khz_attitude_data.mag_z; sensor->cam_status = LL_1khz_attitude_data.cam_status; sensor->cam_pitch = LL_1khz_attitude_data.cam_angle_pitch; sensor->cam_roll = LL_1khz_attitude_data.cam_angle_roll; sensor->battery = LL_1khz_attitude_data.battery_voltage1; sensor->flight_time = LL_1khz_attitude_data.flight_time; sensor->cpu_load = LL_1khz_attitude_data.cpu_load; //strcpy(header.start,"$>");//header.start = "$>"; header->start[0] = '$'; header->start[1] = '>'; header->length = (short)sizeof(SENSOR); header->id = SENSOR_id; footer->checksum = HLC_Checksum((char *)&sensor, sizeof(SENSOR)); footer->stop[0] = '<'; footer->stop[1] = '@'; //strcpy(footer.stop,"<@");//footer.stop = "<@"; UART_SendPacket_raw(packet, sizeof(packet)); /* int index = 0; for (int i = 0; i < sizeof(HEADER); i++) packet[index + i] = *((char *)&header + i); index += sizeof(HEADER); for (int i = 0; i < sizeof(SENSOR); i++) packet[index + i] = *((char *)&sensor + i); index += sizeof(SENSOR); for (int i = 0; i < sizeof(FOOTER); i++) packet[index + i] = *((char *)&footer + i); index += sizeof(FOOTER); // UART_SendPacket_raw(packet, index); */ } verboseMode = 0; controlState = HLC_START_SEQ; } else if (control == AUX_id) { if (verboseMode) { /* printf("AUX -\n\r"); printf("System Flags = %d\n\r",LL_1khz_attitude_data.system_flags); printf("RC Data = %d\n\r",LL_1khz_attitude_data.RC_data[0]); printf("Dummy 333Hz = %d\n\r",LL_1khz_attitude_data.dummy_333Hz_1); printf("Motor Data = %d\n\r",LL_1khz_attitude_data.motor_data[0]); printf("Battery2 (V) = %f\n\r",LL_1khz_attitude_data.battery_voltage2 * 0.001); printf("Status = %d\n\r",LL_1khz_attitude_data.status); printf("Status2 = %d\n\r",LL_1khz_attitude_data.status2); */ } else { char packet[sizeof(HEADER) + sizeof(AUX) + sizeof(FOOTER)]; HEADER *header = (HEADER *)packet; AUX *aux = (AUX *)&packet[sizeof(HEADER)]; FOOTER *footer = (FOOTER *)&packet[sizeof(HEADER) + sizeof(AUX)]; aux->sys_flags = LL_1khz_attitude_data.system_flags; for (int i = 0; i < sizeof(aux->RC_data); i++) aux->RC_data[i] = LL_1khz_attitude_data.RC_data[i]; aux->dummy_333Hz = LL_1khz_attitude_data.dummy_333Hz_1; for (int i = 0; i < sizeof(aux->motor_data); i++) aux->motor_data[i] = LL_1khz_attitude_data.motor_data[i]; aux->battery2 = LL_1khz_attitude_data.battery_voltage2; aux->status = LL_1khz_attitude_data.status; aux->status2 = LL_1khz_attitude_data.status2; //strcpy(header.start,"$>"); header->start[0] = '$'; header->start[1] = '>'; //header.start = "$>"; header->length = (short)sizeof(AUX); header->id = AUX_id; footer->checksum = HLC_Checksum((char *)&aux, (int)sizeof(AUX)); //footer.stop = "<@"; //strcpy(footer.stop,"<@"); footer->stop[0] = '<'; footer->stop[1] = '@'; UART_SendPacket_raw(packet, sizeof(packet)); /* int index = 0; for (int i = 0; i < sizeof(HEADER); i++) packet[index + i] = *((char *)&header + i); index += sizeof(HEADER); for (int i = 0; i < sizeof(AUX); i++) packet[index + i] = *((char *)&aux + i); index += sizeof(AUX); for (int i = 0; i < sizeof(FOOTER); i++) packet[index + i] = *((char *)&footer + i); index += sizeof(FOOTER); // UART_SendPacket_raw(packet, index); //*/ } verboseMode = 0; controlState = HLC_START_SEQ; } else controlState = HLC_START_SEQ; break; case HLC_CMD_SEQ1: //printf("%c cmd seq 1, 0x%x\n\r", control, controlState); if (control == '>') controlState = HLC_CMD_SEQ2; else controlState = HLC_START_SEQ; break; case HLC_CMD_SEQ2: //printf("%c cmd seq 2, 0x%x\n\r", control, controlState); if (control == 'D') { WO_SDK.ctrl_enabled = 0x00; controlState = HLC_START_SEQ; } else if (control == 'N') { // printf("Pch: 0x%x\n\r", WO_CTRL_Input.pitch); // printf("Rol: 0x%x\n\r", WO_CTRL_Input.roll); // printf("Yaw: 0x%x\n\r", WO_CTRL_Input.yaw); // printf("Thr: 0x%x\n\r", WO_CTRL_Input.thrust); controlState = HLC_CMD_NAUT; } else controlState = HLC_START_SEQ; byteIndex = 0; break; case HLC_CMD_NAUT: //printf("%c cmd nat, 0x%x\n\r", control, controlState); packetSize = 9; ctrlEnabled = 0x01; ctrlMode = 0x00; WO_CTRL_Input.ctrl = byteIndex == 0 ? (unsigned char)control : WO_CTRL_Input.ctrl; *((char *)&WO_CTRL_Input.pitch + 0) = byteIndex == 1 ? (unsigned char)control : *((char *)&WO_CTRL_Input.pitch + 0); *((char *)&WO_CTRL_Input.pitch + 1) = byteIndex == 2 ? (unsigned char)control : *((char *)&WO_CTRL_Input.pitch + 1); *((char *)&WO_CTRL_Input.roll + 0) = byteIndex == 3 ? (unsigned char)control : *((char *)&WO_CTRL_Input.roll + 0); *((char *)&WO_CTRL_Input.roll + 1) = byteIndex == 4 ? (unsigned char)control : *((char *)&WO_CTRL_Input.roll + 1); *((char *)&WO_CTRL_Input.yaw + 0) = byteIndex == 5 ? (unsigned char)control : *((char *)&WO_CTRL_Input.yaw + 0); *((char *)&WO_CTRL_Input.yaw + 1) = byteIndex == 6 ? (unsigned char)control : *((char *)&WO_CTRL_Input.yaw + 1); *((char *)&WO_CTRL_Input.thrust + 0)= byteIndex == 7 ? (unsigned char)control : *((char *)&WO_CTRL_Input.thrust + 0); *((char *)&WO_CTRL_Input.thrust + 1)= byteIndex == 8 ? (unsigned char)control : *((char *)&WO_CTRL_Input.thrust + 1); controlState = byteIndex == packetSize ? HLC_START_SEQ : HLC_CMD_NAUT; if( WO_CTRL_Input.pitch> 2000) { WO_CTRL_Input.pitch= 2000; } if( WO_CTRL_Input.pitch< -2000) { WO_CTRL_Input.pitch= -2000; } if( WO_CTRL_Input.roll> 2000) { WO_CTRL_Input.roll= 2000; } if( WO_CTRL_Input.roll< -2000) { WO_CTRL_Input.roll= -2000; } if( WO_CTRL_Input.yaw> 1500) { WO_CTRL_Input.yaw= 1500; } if( WO_CTRL_Input.yaw< -1500) { WO_CTRL_Input.yaw= -1500; } if( WO_CTRL_Input.thrust> 3200) { WO_CTRL_Input.thrust= 3200; } if( WO_CTRL_Input.thrust<0) { WO_CTRL_Input.thrust= 0; } //WO_CTRL_Input.roll=ntohs(WO_CTRL_Input.roll); // WO_CTRL_Input.yaw=ntohs(WO_CTRL_Input.yaw); // WO_CTRL_Input.thrust=ntohs(WO_CTRL_Input.thrust); byteIndex++; break; } // } }
/********************************************************** MAIN **********************************************************/ int main(void) { static int vbat1, vbat2; int vbat; static int bat_cnt = 0, bat_warning = 1000; static char bat_warning_enabled = 1; #ifdef GPS_BEEP static unsigned int gps_beep_cnt; #endif IDISABLE; init(); LL_write_init(); beeper(OFF); HL_Status.up_time = 0; printf("\n\nProgramm is running ... \n"); printf("Processor Clock Frequency: %d Hz\n", processorClockFrequency()); printf("Peripheral Clock Frequency: %d Hz\n", peripheralClockFrequency()); IENABLE; packetsTemp = packets; LED(1, ON); GPS_init_status = GPS_STARTUP; // GPS_init_status = GPS_NEEDS_CONFIGURATION; sdkInit(); while (1) { if (mainloop_trigger > 0) { if (GPS_timeout < ControllerCyclesPerSecond) GPS_timeout++; else if (GPS_timeout == ControllerCyclesPerSecond) { GPS_timeout = ControllerCyclesPerSecond + 1; GPS_Data.status = 0; GPS_Data.numSV = 0; // if (GPS_init_status == GPS_STARTUP) //no GPS packet after startup // { GPS_init_status = GPS_NEEDS_CONFIGURATION; // } } mainloop_cnt++; if (++bat_cnt == 100) bat_cnt = 0; //battery monitoring vbat1 = (vbat1 * 29 + (ADC0Read(VOLTAGE_1) * 9872 / 579)) / 30; //voltage in mV //*9872/579 HL_Status.battery_voltage_1 = vbat1; HL_Status.battery_voltage_2 = vbat2; vbat = vbat1; if (vbat < BATTERY_WARNING_VOLTAGE) //decide if it's really an empty battery { if (bat_warning < ControllerCyclesPerSecond * 2) bat_warning++; else bat_warning_enabled = 1; } else { if (bat_warning > 10) bat_warning -= 5; else { bat_warning_enabled = 0; beeper(OFF);//IOCLR1 = (1<<17); //Beeper off } } if (bat_warning_enabled) { if (bat_cnt > ((vbat - 9000) / BAT_DIV)) beeper(ON);//IOSET1 = (1<<17); //Beeper on else beeper(OFF);//IOCLR1 = (1<<17); //Beeper off } #ifdef GPS_BEEP //GPS_Beep if((GPS_Data.status&0xFF)!=3) //no lock { gps_beep_cnt++; if(gps_beep_cnt>=1500) gps_beep_cnt=0; if(gps_beep_cnt<20) beeper(ON); //IOSET1 = (1<<17); //Beeper on else if(gps_beep_cnt==40) beeper(OFF);// IOCLR1 = (1<<17); //Beeper off } #endif if (mainloop_trigger) mainloop_trigger--; mainloop(); } } return 0; }