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;
}
