int ctl_Init(char *client_addr)
{
int rc;
//----- Initialize PID controller -----//
//some saturation for controller
setpoint.pitch_roll_max=DEG2RAD(10); //degrees
setpoint.throttle_min=0.05;//0.5
setpoint.throttle_max=0.95; //0.85
//init pid pitch/roll //Kp, Ki, Kd, imax
pid_Init(&pid_roll, 0.33,0.0,0.25,1); //[0.35 0.25] with additional part [0.33 0.25] without ...
pid_Init(&pid_pitch, 0.33,0.0,0.25,1);
throttle=0.000;
//----- Initialize AHRS system -----//
printf("Initialize Attitude.. \n");
//rc = att_Init(&att);
//if(rc) return rc;
if(navdata_init()) printf("navdata initialized\n");
navdata_flattrim(&ahrsdata);
//printf("bias_after:%f\t%f\t%f\n",gyros_offset[0],gyros_offset[1],gyros_offset[2]);
sleep(1);
//----- Initialize UDP -----//
// Udp logger
printf("client\n");
udpClient_Init(&udpNavLog, "192.168.43.176", 9930); // Update the IP of ground station to send data
//udpClient_Init(&udpNavLog, "192.168.1.4", 9930);
//navLog_Send();
printf("udpClient_Init\n", rc);
//----- Start motor thread -----//
printf("Initialize motor\n");
rc = mot_Init();
if(rc) return rc;
//----- Start ctl thread -----//
printf("Initialize ctl_thread_main\n");
rc = pthread_create(&ctl_thread, NULL, ctl_thread_main, NULL);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
return 202;
}
else printf("rc = %d\n", rc);
}
/**
* \brief Start navdata thread
* \return 0 if success, -1 if error
*/
int navdata_connect()
{
if (!stopped_navdata)
return -1;
addr_drone_navdata.sin_family = AF_INET;
addr_drone_navdata.sin_addr.s_addr = inet_addr(WIFI_ARDRONE_IP);
addr_drone_navdata.sin_port = htons(PORT_NAVDATA);
addr_client_navdata.sin_family = AF_INET;
addr_client_navdata.sin_addr.s_addr = htonl(INADDR_ANY);
addr_client_navdata.sin_port = htons(PORT_NAVDATA);
sock_navdata = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sock_navdata < 0) {
fprintf(stderr, "[~][navdata] Can't establish socket \n");
return -1;
}
if (bind(sock_navdata, (struct sockaddr*)&addr_client_navdata, sizeof(addr_client_navdata)) < 0) {
fprintf(stderr, "[~][navdata] Can't bind socket to port %d\n", PORT_NAVDATA);
return -1;
}
nav_channel = jakopter_com_add_channel(CHANNEL_NAVDATA, sizeof(data));
if (navdata_init() < 0) {
perror("[~][navdata] Init sequence failed");
return -1;
}
pthread_mutex_lock(&mutex_stopped);
stopped_navdata = false;
pthread_mutex_unlock(&mutex_stopped);
if(pthread_create(&navdata_thread, NULL, navdata_routine, NULL) < 0) {
perror("[~][navdata] Can't create thread");
return -1;
}
return 0;
}
void imu_impl_init(void) {
navdata_init();
}
void navdata_update()
{
static bool_t last_checksum_wrong = FALSE;
// Check if initialized
if (!nav_port.isInitialized) {
navdata_init();
return;
}
// Start reading
navdata_read();
// while there is something interesting to do...
while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
{
if (nav_port.buffer[0] == NAVDATA_START_BYTE)
{
assert(sizeof navdata == NAVDATA_PACKET_SIZE);
memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
// Calculating the checksum
uint16_t checksum = 0;
for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
}
// When checksum is incorrect
if(navdata.chksum != checksum) {
printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
nav_port.checksum_errors++;
}
// When we already dropped a packet or checksum is correct
if(last_checksum_wrong || navdata.chksum == checksum) {
// Invert byte order so that TELEMETRY works better
uint8_t tmp;
uint8_t* p = (uint8_t*) &(navdata.pressure);
tmp = p[0];
p[0] = p[1];
p[1] = tmp;
p = (uint8_t*) &(navdata.temperature_pressure);
tmp = p[0];
p[0] = p[1];
p[1] = tmp;
baro_update_logic();
#ifdef USE_SONAR
if (navdata.ultrasound < 10000)
{
sonar_meas = navdata.ultrasound;
ins_update_sonar();
}
#endif
navdata_imu_available = TRUE;
last_checksum_wrong = FALSE;
nav_port.packetsRead++;
}
// Crop the buffer
navdata_cropbuffer(NAVDATA_PACKET_SIZE);
}
else
{
// find start byte, copy all data from startbyte to buffer origin, update bytesread
uint8_t * pint;
pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
if (pint != NULL) {
navdata_cropbuffer(pint - nav_port.buffer);
} else {
// if the start byte was not found, it means there is junk in the buffer
nav_port.bytesRead = 0;
}
}
}
}
