void test_received_handler(DictionaryIterator *iter, void *context) {
APP_LOG(APP_LOG_LEVEL_INFO, "data received!");
Tuple *ready_tuple = dict_find(iter, MESSAGE_KEY_AppKeyJSReady);
Tuple *end_tuple = dict_find(iter, MESSAGE_KEY_endMessage);
Tuple *error_tuple = dict_find(iter, MESSAGE_KEY_fail);
if (ready_tuple){
APP_LOG(APP_LOG_LEVEL_INFO, "Ready tuple received");
set_JS_is_ready(true);
no_message_in_progress();
return;
} else if(error_tuple){
APP_LOG(APP_LOG_LEVEL_ERROR, "Error in JS");
received_data(iter, context);
no_message_in_progress();
} else {
received_data(iter, context);
response_received();
}
if(end_tuple){
APP_LOG(APP_LOG_LEVEL_INFO, "End tuple received");
no_message_in_progress();
app_timer_cancel(timer_response);
}
show_log(APP_LOG_LEVEL_WARNING, "data procesed!");
}
// constructor definition of filter service registration inner class
clamd_av_filter::initializer::initializer()
{
clamd_av_filter_ptr dfp(new clamd_av_filter());
if(FI_CONFIG.use_clamd())
{
try
{
boost::asio::io_service io_service;
boost::asio::ip::tcp::resolver resolver(io_service);
boost::asio::ip::tcp::resolver::query query(FI_CONFIG.clamd_host(), FI_CONFIG.clamd_port());
boost::asio::ip::tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
boost::asio::ip::tcp::resolver::iterator end;
boost::asio::ip::tcp::socket socket(io_service);
boost::system::error_code error = boost::asio::error::host_not_found;
while (error && endpoint_iterator != end)
{
socket.close();
socket.connect(*endpoint_iterator++, error);
}
if (error)
throw boost::system::system_error(error);
boost::asio::write(socket, boost::asio::buffer("PING\r\n", 6));
char reply[4];
boost::asio::read(socket, boost::asio::buffer(reply, 4));
std::string received_data(reply);
// Check that response is OK.
socket.close();
if(received_data.substr(0,3) == "PON") {
FI_SERVICES->filter_srv().register_filter(filter_code_,dfp);
LOG4CXX_DEBUG(debug_logger_, "Clamd AV filter is running");
}
else
LOG4CXX_ERROR(debug_logger_, "Clamd AV filter is not running");
}
catch (std::exception& e)
{
LOG4CXX_ERROR(debug_logger_, "Clamd AV filter is not running. Exception: " + (std::string)e.what());
}
}
}
int main (void)
{
// Variables -- Misc
const twim_options_t twi_option_GYRO = twi_opt_GYRO;
const twim_options_t twi_option_ACC = twi_opt_ACC;
const twim_options_t twi_option_MAGN = twi_opt_MAGN;
// Variables -- Control system
float prev_e_z = 0;
float prev_e_roll = 0;
float prev_e_pitch = 0;
float prev_e_yaw = 0;
float eint_z = 0;
float eint_roll = 0;
float eint_pitch = 0;
float eint_yaw = 0;
float pitch_ederiv = 0; // Error derivative
float roll_ederiv = 0;
float prev_deriv_e_roll = 0;
float yaw_ederiv = 0;
float z_ederiv = 0;
float e = 0;
float dt; // Time between samples
unsigned long t1 = 0;
unsigned long t2 = 0;
float u1, u2, u3, u4 = 0;
float m1, m2, m3, m4 = 0;
float g_force = 240;
int xcount = 0;
float gain = 1;
int time_to_update_data = 0;
int deriv_count=0;
const usart_options_t usart_option = usart_opt;
const usart_options_t usart_option_2 = usart_opt_2;
// Setting up the board
pcl_configure_clocks(&pcl_freq_param);
board_init();
// Initialize Bluetooth
configure_AT(FPBA_HZ, usart_option);
irq_initialize_vectors();
cpu_irq_enable();
Disable_global_interrupt();
// Initialize interrupt vectors.
INTC_init_interrupts();
INTC_register_interrupt(&usart_int_handler, USART_IRQ, AVR32_INTC_INT0);
USART->ier = AVR32_USART_IER_RXRDY_MASK;
// Enable all interrupts.
Enable_global_interrupt();
// Enable Ultrasonic sensors
enable_Ultrasonic();
// Initialize motors
initialize_PWM();
tc_start(tc1, PWM_MOTOR1_CHANNEL);
tc_start(tc0, PWM_MOTOR2_CHANNEL);
tc_start(tc1, PWM_MOTOR3_CHANNEL);
tc_start(tc1, PWM_MOTOR4_CHANNEL);
// Waits for button press here after battery is connected
while (gpio_get_pin_value(GPIO_PUSH_BUTTON_0));
delay_ms(1000);
/*
while(1){
while (motor2_speed<60)
{
motor2_speed += 1;
update_Motors();
delay_ms(300);
}
while (motor2_speed>0)
{
motor2_speed -= 1;
update_Motors();
delay_ms(300);
}
}
while (gpio_get_pin_value(GPIO_PUSH_BUTTON_0));*/
//delay_ms(3000);
// Initialize RTC
//AVR32_RTC->ctrl & AVR32_RTC_CTRL_BUSY_MASK = 0;
rtc_init(&AVR32_RTC, RTC_OSC_32KHZ, 1);
rtc_enable(&AVR32_RTC);
// Initialize IMU parts
initialize_IMU();
// TESTING ONLY
float test_array[2000];
float test_array_2[2000];
float test_array_3[2000];
for (int k = 0; k < FILTER_LENGTH; k++){
FIFO_deriv_roll[k] = roll;
}
time_to_update_data = 0;
// Control system code begins
while(1)
{
t1 = rtc_get_value(&AVR32_RTC); // Current time in ms
dt = ((float)(t1-t2))*0.001*0.125; // dt in seconds
if (t1 < t2) // Timer overflowed
{
dt = ((float)(t1 + rtc_get_top_value(&AVR32_RTC))*0.001*0.125);
}
t2 = t1;
get_Angles();
//pitch = 0;
yaw = 0;
e = des_z-z; //calculating height error
eint_z = eint_z + ((e+prev_e_z)/2)*dt; //calculate error integral term
z_ederiv = (e - prev_e_z)/dt; //calculate error derivative term
u1 =(KP_Z*e) + (KI_Z*eint_z) + (KD_Z*z_ederiv)+g_force; //calculating control output
prev_e_z=e;
//ROLL
e = des_roll-roll; //calculating roll error
eint_roll = eint_roll + ((e+prev_e_roll)/2)*dt; //calculate error integral term
prev_e_roll=e;
for (int i = 0; i < (FILTER_LENGTH - 1); i++)
{
FIFO_deriv_roll[i] = FIFO_deriv_roll[i+1];
}
FIFO_deriv_roll[FILTER_LENGTH-1] = e;
roll_ederiv = (FIFO_deriv_roll[FILTER_LENGTH-1] - FIFO_deriv_roll[0])/(FILTER_LENGTH*dt);
u2 =(KP_ROLL*e) + (KI_ROLL*eint_roll) + (KD_ROLL*roll_ederiv); //calculating control output
if(xcount < 2000){
test_array[xcount] = roll_ederiv;
test_array_2[xcount] = roll;
test_array_3[xcount] = g_roll;
}
xcount++;
//PITCH
e = des_pitch-pitch; //calculating pitch error
eint_pitch = eint_pitch + ((e+prev_e_pitch)/2)*dt; //calculate error integral term
pitch_ederiv = (e - prev_e_pitch)/dt; //calculate error derivative term
u3 =(KP_PITCH*e) + (KI_PITCH*eint_pitch) + (KD_PITCH*pitch_ederiv); //calculating control output
prev_e_pitch=e;
//YAW
e = des_yaw-yaw; //calculating yaw error
eint_yaw = eint_yaw + ((e+prev_e_yaw)/2)*dt; //calculate error integral term
yaw_ederiv = (e - prev_e_yaw)/dt; //calculate error derivative term
u4 =(KPYAW*e) + (KIYAW*eint_yaw) + (KDYAW*yaw_ederiv); //calculating control output
prev_e_yaw=e;
//MOTOR SPEEDS
m1=(0.25*u1+0.5*u2+0.25*u4)*gain;
m2=(0.25*u1+0.5*u3-0.25*u4)*gain;
m3=(0.25*u1-0.5*u2+0.25*u4)*gain;
m4=(0.25*u1-0.5*u3-0.25*u4)*gain;
if (m1 > 95)
m1 = 95;
else if (m1 < 5)
m1 = 5;
if (m2 > 95)
{
m2 = 95;
}
else if (m2 < 5)
{
m2 = 5;
}
if (m3 > 95)
{
m3 = 95;
}
else if (m3 < 5)
{
m3 = 5;
}
if (m4 > 95)
{
m4 = 95;
}
else if (m4 < 5)
{
m4 = 5;
}
motor1_speed = m2; //m2
motor2_speed = m1; //m3
motor3_speed = m4; //m4
motor4_speed = m3; //m1..... the imu was turned 90 degrees....
update_Motors();
//Bluetooth Send
if (time_to_update_data > 3)
{
// Get Ultrasonic data
update_Ultrasonic();
// Send out update through Bluetooth
meas_roll = roll;
meas_pitch = pitch;
meas_yaw = yaw;
transmitted_data();
time_to_update_data = 0;
received_data();
}
else
{
delay_ms(7);
}
time_to_update_data++;
}
}
