/**This gets all current values and writes them to min or max
*/
uint8_t get_value_s(void)
{
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
uint8_t i;
for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
//see if the value is bigger or smaller than 1500 (roughly neutral)
//and write to the appropriate array
if (global_data_rc_channels->chan[i].raw != 0 &&
global_data_rc_channels->chan[i].raw < 1500) {
min_values_servo[i] = global_data_rc_channels->chan[i].raw;
} else if (global_data_rc_channels->chan[i].raw != 0 &&
global_data_rc_channels->chan[i].raw > 1500) {
max_values_servo[i] = global_data_rc_channels->chan[i].raw;
} else {
max_values_servo[i] = 0;
min_values_servo[i] = 0;
}
}
global_data_unlock(&global_data_rc_channels->access_conf);
return 0;
} else
return -1;
}
static void *sensors_raw_receiveloop(void * arg) //runs as a pthread and listens messages from raw sensor
{
uint16_t counter = 0;
uint64_t timestamp;
while(1)
{
if(0 == global_data_wait(&global_data_sensors_raw.access_conf)) //only send if pthread_cond_timedwait received a con signal
{
// TODO: send data to mavlink
// if(counter%100 == 0)
// {
//mavlink_msg_statustext_send(chan,0,"sensor information incoming");
timestamp = global_data_get_timestamp_useconds();
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, timestamp, global_data_sensors_raw.accelerometer_raw[0], global_data_sensors_raw.accelerometer_raw[1], global_data_sensors_raw.accelerometer_raw[2], global_data_sensors_raw.gyro_raw[0], global_data_sensors_raw.gyro_raw[1], global_data_sensors_raw.gyro_raw[2], global_data_sensors_raw.magnetometer_raw[0], global_data_sensors_raw.magnetometer_raw[1], global_data_sensors_raw.magnetometer_raw[2]);
// }
}
// else
// {
// mavlink_msg_statustext_send(chan,0,"timeout");
// }
global_data_unlock(&global_data_sensors_raw.access_conf);
counter++;
}
}
void write_data_s(void)
{
// global_data_lock(&global_data_rc_channels->access_conf);
// uint8_t i;
// for(i=0; i < NR_CHANNELS; i++){
// //Write the data to global_data_rc_channels (if not 0)
// if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){
// global_data_rc_channels->chan[i].scaling_factor =
// 10000/((max_values_servo[i] - min_values_servo[i])/2);
//
// global_data_rc_channels->chan[i].mid = mid_values_servo[i];
// }
//
// printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
// i,
// global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
// min_values_servo[i], max_values_servo[i],
// global_data_rc_channels->chan[i].scaling_factor,
// global_data_rc_channels->chan[i].mid);
// }
// global_data_unlock(&global_data_rc_channels->access_conf);
//Write to the Parameter storage
global_data_lock(&global_data_parameter_storage->access_conf);
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0];
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1];
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2];
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3];
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0];
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1];
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2];
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3];
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0];
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1];
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2];
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3];
global_data_unlock(&global_data_parameter_storage->access_conf);
usleep(3e6);
uint8_t i;
for (i = 0; i < NR_CHANNELS; i++) {
printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
i,
min_values_servo[i], max_values_servo[i],
global_data_rc_channels->chan[i].scaling_factor,
global_data_rc_channels->chan[i].mid);
}
}
static void *heartbeatloop(void * arg)
{
/* initialize waypoint manager */
mavlink_wpm_init(&wpm);
/* initialize parameter storage */
mavlink_pm_reset_params(&pm);
int lowspeed_counter = 0;
int result_sys_status_trylock;
while(1) {
// sleep
usleep(50000);
// 1 Hz
if (lowspeed_counter == 10)
{
/* Send heartbeat */
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_GENERIC,MAV_MODE_PREFLIGHT,custom_mode,MAV_STATE_UNINIT);
/* Send status */
result_sys_status_trylock = global_data_trylock(&global_data_sys_status.access_conf);
if(0 == result_sys_status_trylock) mavlink_msg_sys_status_send(chan, global_data_sys_status.onboard_control_sensors_present, global_data_sys_status.onboard_control_sensors_enabled, global_data_sys_status.onboard_control_sensors_health, global_data_sys_status.load, global_data_sys_status.voltage_battery, global_data_sys_status.current_battery, global_data_sys_status.battery_remaining, global_data_sys_status.drop_rate_comm, global_data_sys_status.errors_comm, global_data_sys_status.errors_count1, global_data_sys_status.errors_count2, global_data_sys_status.errors_count3, global_data_sys_status.errors_count4);
global_data_unlock(&global_data_sys_status.access_conf);
if (!(mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
// System is not armed, blink at 1 Hz
led_toggle(LED_BLUE);
}
lowspeed_counter = 0;
}
lowspeed_counter++;
// Send one parameter
mavlink_pm_queued_send();
if (mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// System is armed, blink at 10 Hz
led_toggle(LED_BLUE);
}
usleep(50000);
}
}
static void *gps_receiveloop(void * arg) //runs as a pthread and listens messages from GPS
{
while(1)
{
if(0 == global_data_wait(&global_data_gps.access_conf)) //only send if pthread_cond_timedwait received a con signal
{
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, global_data_gps.time_usec, global_data_gps.fix_type, global_data_gps.lat, global_data_gps.lon, global_data_gps.alt, global_data_gps.eph, global_data_gps.epv, global_data_gps.vel, global_data_gps.cog, global_data_gps.satellites_visible);
mavlink_msg_gps_status_send(MAVLINK_COMM_0, global_data_gps.satellites_visible, global_data_gps.satellite_prn, global_data_gps.satellite_used, global_data_gps.satellite_elevation, global_data_gps.satellite_azimuth, global_data_gps.satellite_snr);
}
// else
// {
// mavlink_msg_statustext_send(chan,0,"timeout");
// }
global_data_unlock(&global_data_gps.access_conf);
}
}
/**This sets the middle values
*/
uint8_t set_mid_s(void)
{
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
uint8_t i;
for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
if (i == global_data_rc_channels->function[ROLL] ||
i == global_data_rc_channels->function[YAW] ||
i == global_data_rc_channels->function[PITCH]) {
mid_values_servo[i] = global_data_rc_channels->chan[i].raw;
} else {
mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2;
}
}
global_data_unlock(&global_data_rc_channels->access_conf);
return 0;
} else
return -1;
}
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
static float lat_next;
static float lon_next;
static float pitch_current;
static float thrust_total;
if (initialized == false) {
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
global_data_unlock(&global_data_parameter_storage->access_conf);
}
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
//TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// if(counter % 5 == 0)
// {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout);
// }
pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction);
}
att_sp.roll = roll_desired;
att_sp.pitch = pitch_desired;
counter++;
}
/****************************************************************************
* Public Functions
****************************************************************************/
void handleMessage(mavlink_message_t * msg) {
//check for terminate command
if(msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
{
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
if (cmd.target_system == mavlink_system.sysid && ((cmd.target_component == mavlink_system.compid) || (cmd.target_component == MAV_COMP_ID_ALL)))
{
bool terminate_link = false;
bool reboot = false;
/* result of the command */
uint8_t result = MAV_RESULT_UNSUPPORTED;
/* supported command handling start */
/* request to set different system mode */
if (cmd.command == MAV_CMD_DO_SET_MODE)
{
mavlink_system.mode = cmd.param1;
}
/* request to arm */
// XXX
/* request for an autopilot reboot */
if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 1.0f)
{
reboot = true;
result = MAV_RESULT_ACCEPTED;
mavlink_msg_statustext_send(chan,0,"Rebooting autopilot.. ");
}
/* request for a link shutdown */
if (cmd.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd.param1 == 3.0f)
{
terminate_link = true;
result = MAV_RESULT_ACCEPTED;
mavlink_msg_statustext_send(chan,0,"Terminating MAVLink.. ");
}
/* supported command handling stop */
/* send any requested ACKs */
if (cmd.confirmation > 0)
{
mavlink_msg_command_ack_send(chan, cmd.command, result);
}
/* the two termination / reset commands need special handling */
if (terminate_link)
{
printf("MAVLink: Terminating.. \n");
fflush(stdout);
/* sleep 100 ms to allow UART buffer to empty */
led_off(LED_BLUE);
led_on(LED_AMBER);
int i;
for (i = 0; i < 5; i++)
{
led_toggle(LED_AMBER);
usleep(20000);
}
//terminate other threads:
pthread_cancel(heartbeat_thread);
//terminate this thread (receive_thread)
pthread_exit(NULL);
}
if (reboot)
{
printf("MAVLink: Rebooting system.. \n");
fflush(stdout);
/* sleep 100 ms to allow UART buffer to empty */
led_off(LED_BLUE);
led_on(LED_AMBER);
int i;
for (i = 0; i < 5; i++)
{
led_toggle(LED_BLUE);
led_toggle(LED_AMBER);
usleep(20000);
}
//terminate other threads:
pthread_cancel(heartbeat_thread);
// Reset whole system
/* Resetting CPU */
// FIXME Need check for ARM architecture here
#ifndef NVIC_AIRCR
#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C))
#endif
/* Set the SYSRESETREQ bit to force a reset */
NVIC_AIRCR = 0x05fa0004;
/* Spinning until the board is really reset */
while(true);
}
}
}
/* Handle quadrotor motor setpoints */
if(msg->msgid == MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT)
{
mavlink_set_quad_motors_setpoint_t quad_motors_setpoint;
mavlink_msg_set_quad_motors_setpoint_decode(msg, &quad_motors_setpoint);
if(quad_motors_setpoint.target_system == mavlink_system.sysid)
{
global_data_lock(&global_data_quad_motors_setpoint.access_conf);
global_data_quad_motors_setpoint.motor_front_nw = quad_motors_setpoint.motor_front_nw;
global_data_quad_motors_setpoint.motor_right_ne = quad_motors_setpoint.motor_right_ne;
global_data_quad_motors_setpoint.motor_back_se = quad_motors_setpoint.motor_back_se;
global_data_quad_motors_setpoint.motor_left_sw = quad_motors_setpoint.motor_left_sw;
global_data_quad_motors_setpoint.counter++;
global_data_quad_motors_setpoint.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_quad_motors_setpoint.access_conf);
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_quad_motors_setpoint.access_conf);
}
}
}
static void *sensors_receiveloop(void * arg) //runs as a pthread and listens messages from GPS
{
struct timespec tp;
int counter = 0;
FILE * logfile;
logfile = fopen ("/mnt/sdcard/logfile001.txt","w");
if (logfile!=NULL)
{
clock_gettime(CLOCK_REALTIME,&tp);
fprintf (logfile, "Logging of raw sensor data started at %i seconds and %i nanoseconds \n",tp.tv_sec,tp.tv_nsec);
fprintf (logfile, "Seconds\tNanoseconds\tGyrX\tGyrY\tGyrZ\tGyrCount\tAccX\tAccY\tAccZ\tAccCount\tMagX\tMagY\tMagZ\tMagCount\tPress1\tPress2\tPressCount \n");
}
else{
printf("failed to open logfile");
return;
}
uint64_t timestamp;
/*
int16_t gyro_raw[3]; // l3gd20
uint16_t gyro_raw_counter;
int16_t accelerometer_raw[3]; // bma180
uint16_t accelerometer_raw_counter;
int16_t magnetometer_raw[3]; //hmc5883l
uint16_t magnetometer_raw_counter;
uint32_t pressure_sensor_raw[2]; //ms5611
uint16_t pressure_sensor_raw_counter;*/
while(counter<10000)
{
if(0 == global_data_wait(&global_data_sensors_raw.access_conf)) //only send if pthread_cond_timedwait received a con signal
{
/*for(int i=0; i<3; i++){
gyro_raw[i]=global_data_sensors_raw.gyro_raw[i];
}
gyro_raw_counter=global_data_sensors_raw.gyro_raw_counter;
for(int i=0; i<3; i++){
accelerometer_raw[i]=global_data_sensors_raw.accelerometer_raw[i];
}
accelerometer_raw_counter=global_data_sensors_raw.accelerometer_raw_counter;
for(int i=0; i<3; i++){
magnetometer_raw[i]=global_data_sensors_raw.magnetometer_raw[i];
}
magnetometer_raw_counter=global_data_sensors_raw.magnetometer_raw_counter;
for(int i=0; i<2; i++){
pressure_sensor_raw[i]=global_data_sensors_raw.pressure_sensor_raw[i];
}
pressure_sensor_raw_counter=global_data_sensors_raw.pressure_sensor_raw_counter;*/
//clock_gettime(CLOCK_REALTIME,&tp);
timestamp = global_data_get_timestamp_useconds();
fprintf(logfile,"%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\t%i\n",timestamp,global_data_sensors_raw.gyro_raw[0],global_data_sensors_raw.gyro_raw[1],global_data_sensors_raw.gyro_raw[2],global_data_sensors_raw.gyro_raw_counter,global_data_sensors_raw.accelerometer_raw[0],global_data_sensors_raw.accelerometer_raw[1],global_data_sensors_raw.accelerometer_raw[2],global_data_sensors_raw.accelerometer_raw_counter,global_data_sensors_raw.magnetometer_raw[0],global_data_sensors_raw.magnetometer_raw[1],global_data_sensors_raw.magnetometer_raw[2],global_data_sensors_raw.magnetometer_raw_counter,global_data_sensors_raw.pressure_sensor_raw[0],global_data_sensors_raw.pressure_sensor_raw[1],global_data_sensors_raw.pressure_sensor_raw_counter);
counter++;
}
global_data_unlock(&global_data_sensors_raw.access_conf);
}
clock_gettime(CLOCK_REALTIME,&tp);
fprintf (logfile, "Logging of raw sensor data ended at %i seconds and %i nanoseconds \n",tp.tv_sec,tp.tv_nsec);
fclose(logfile);
printf("written to logfile001.txt");
}
static void *control_loop(void *arg)
{
// Set thread name
prctl(1, "fixedwing_control attitude", getpid());
control_outputs.mode = HIL_MODE + 16;
control_outputs.nav_mode = 0;
while (1) {
/************************************
* Read global data structures here
************************************/
// Get parameters
get_parameters();
#define MUTE
#ifndef MUTE
///// DEBUG OUTPUT ////////
// printf("Throttle: \tChannel %i\t Value %i\n", global_data_rc_channels->function[THROTTLE], global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].scale);
// printf("Throttle_correct: \tChannel %i\t Value %i\n", THROTTLE, global_data_rc_channels->chan[THROTTLE].scale);
// printf("Override: Channel %i\t Value %i\n", global_data_rc_channels->function[OVERRIDE], global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale);
// printf("Override_correct: \tChannel %i\t Value %i\n", OVERRIDE, global_data_rc_channels->chan[OVERRIDE].scale);
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
printf("Current Altitude: %i\n\n", (int)plane_data.alt);
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
(int)(1000 * plane_data.rollspeed), (int)(1000 * plane_data.pitchspeed), (int)(1000 * plane_data.yawspeed));
printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
// printf("\nGlobal attitude outputs \n, %i\n%i\n%i\n", global_data_fixedwing_control->attitude_control_output[ROLL],
// global_data_fixedwing_control->attitude_control_output[PITCH], global_data_fixedwing_control->attitude_control_output[THROTTLE]);
///////////////////////////
#endif
// position values
if (global_data_trylock(&global_pos.access_conf) == 0) {
plane_data.lat = global_pos.lat / 10000000;
plane_data.lon = global_pos.lon / 10000000;
plane_data.alt = global_pos.alt / 1000;
plane_data.vx = global_pos.vx / 100;
plane_data.vy = global_pos.vy / 100;
plane_data.vz = global_pos.vz / 100;
}
global_data_unlock(&global_pos.access_conf);
// attitude values
if (global_data_trylock(&att.access_conf) == 0) {
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.hdg = att.yaw - 180;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
}
global_data_unlock(&att.access_conf);
// Set plane mode
set_plane_mode();
// Calculate the P,Q,R body rates of the aircraft
calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
// Calculate the body frame angles of the aircraft
//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);
// Calculate the output values
control_outputs.roll_ailerons = calc_roll_ail();
control_outputs.pitch_elevator = calc_pitch_elev();
//control_outputs.yaw_rudder = calc_yaw_rudder();
control_outputs.throttle = calc_throttle();
/*******************************************
* Send data to global data structure
******************************************/
if (global_data_rc_channels->chan[global_data_rc_channels->function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
if (plane_data.mode == TAKEOFF) {
global_data_fixedwing_control->attitude_control_output[ROLL] = 0;
global_data_fixedwing_control->attitude_control_output[PITCH] = 5000;
global_data_fixedwing_control->attitude_control_output[THROTTLE] = 10000;
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
if (plane_data.mode == CRUISE) {
global_data_fixedwing_control->attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons);
global_data_fixedwing_control->attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator);
global_data_fixedwing_control->attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle);
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
global_data_fixedwing_control->counter++;
global_data_fixedwing_control->timestamp = hrt_absolute_time();
#error Either publish here or above, not at two locations
orb_publish()
}
// 20 Hz loop
usleep(100000);
}
return NULL;
}
static void *ubx_watchdog_loop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0")
{
/* Retrieve file descriptor */
int fd = *((int *)arg);
while(1)
{
/* if some values are to old reconfigure gps */
int i;
pthread_mutex_lock(&ubx_mutex);
bool all_okay = true;
uint64_t timestamp_now = global_data_get_timestamp_milliseconds();
for(i = 0; i < UBX_NO_OF_MESSAGES; i++)
{
if(timestamp_now - ubx_state->last_message_timestamps[i] > GPS_WATCHDOG_CRITICAL_TIME_MILLISECONDS)
{
// printf("Warning: GPS ubx message %d not received for a long time\n", i);
all_okay = false;
}
}
pthread_mutex_unlock(&ubx_mutex);
if(!all_okay)
{
/* gps error */
printf("GPS Watchdog detected gps not running or having problems\n");
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled |= 1 << 5;
global_data_sys_status.onboard_control_sensors_health &= ~(1 << 5);
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
/* trying to reconfigure the gps configuration */
configure_gps_ubx(fd);
fflush(stdout);
sleep(1);
// int killres = pthread_kill(&ubx_thread, SIGKILL);
// printf("killres=%d",killres);
// sleep(1);
// pthread_create (&ubx_thread, NULL, ubx_loop, (void *)&fd);
}
else
{
/* gps healthy */
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled |= 1 << 5;
global_data_sys_status.onboard_control_sensors_health |= 1 << 5;
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
}
usleep(GPS_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
}
static void *ubx_loop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0")
{
/* Initialize ubx state */
// ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
// ubx_decode_init();
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Initialize gps stuff */
int buffer_size = 1000;
// nmeaINFO * info = malloc(sizeof(nmeaINFO));
bool health_set = false;
char * gps_rx_buffer = malloc(buffer_size*sizeof(char));
/* gps parser (nmea) */
// nmeaPARSER parser;
// nmea_parser_init(&parser);
// nmea_zero_INFO(info);
// float lat_dec = 0;
// float lon_dec = 0;
/* custom (mediatek custom) */
// gps_bin_custom_state_t * mtk_state = malloc(sizeof(gps_bin_custom_state_t));
// mtk_decode_init(mtk_state);
// mtk_state->print_errors = false;
// if( !strcmp("custom",mode) )
// {
// printf("\t%s: custom mode\n",APPNAME);
// // configure_gps_custom(fd); // ?
//
//
// // while(1)
// //
// // if (configure_gps_ubx(fd, ubx_state) != 0)
// //
// // {
// // //TODO: execute custom read
// // }
//
// }
// else if( !strcmp("ubx",mode) )
// {
printf("\t%s: ubx mode\n",APPNAME);
//set parameters for ubx
//ubx state
gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
printf("%s: ubx_state created\n",APPNAME);
ubx_decode_init();
ubx_state->print_errors = false;
int config_not_finished = 1; //is set to 0 as soon as all configurations are completed
bool configured = false;
/* set parameters for ubx */
if (configure_gps_ubx(fd) != 0)
{
printf("Configuration of gps module to ubx failed\n");
/* Write shared variable sys_status */
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled &= ~(1 << 5);
global_data_sys_status.onboard_control_sensors_health &= ~(1 << 5);
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
}
else
{
printf("Configuration of gps module to ubx successful\n");
/* Write shared variable sys_status */
global_data_lock(&global_data_sys_status.access_conf);
global_data_sys_status.onboard_control_sensors_present |= 1 << 5;//TODO: write wrapper for bitmask
global_data_sys_status.onboard_control_sensors_enabled |= 1 << 5;
global_data_sys_status.counter++;
global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
global_data_unlock(&global_data_sys_status.access_conf);
}
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_sys_status.access_conf);
while(1)
{
read_gps_ubx(fd, gps_rx_buffer, buffer_size, &ubx_mutex);
//
// /* set health to true if config is finished after certain time (only executed once) */
// if(config_not_finished == 0 && counter >= GPS_COUNTER_LIMIT && false == health_set)
// {
// global_data_lock(&global_data_sys_status.access_conf);
// global_data_sys_status.onboard_control_sensors_health |= 1 << 5;
// global_data_sys_status.counter++;
// global_data_sys_status.timestamp = global_data_get_timestamp_milliseconds();
// global_data_unlock(&global_data_sys_status.access_conf);
//
// health_set = true;
//
// printf("%s: gps configuration successful\n",APPNAME);
// }
// else if (config_not_finished != 0 && counter >= GPS_COUNTER_LIMIT)
// {
// //reset state machine
// ubx_decode_init(ubx_state);
// ubx_state->print_errors = false;
// ubx_state->last_config_message_sent = UBX_CONFIGURE_NOTHING;
// ubx_state->last_ack_message_received = UBX_CONFIGURE_NOTHING;
// ubx_state->last_config_failed = false;
// config_not_finished = 1; //is set to 0 as soon as all configurations are completed
// bool configured = false;
// counter = 0;
//
//
// printf("%s: gps configuration probably failed, exiting now\n",APPNAME);
//// sleep(1);
//// return 0;
// }
/* Inform the other processes that there is new gps data available */
global_data_broadcast(&global_data_gps.access_conf);
}
// }
// else if( !strcmp("nmea",mode) )
// {
// printf("\t%s: nmea mode\n",APPNAME);
// }
// while(1)
// {
// if( !strcmp("nmea",mode) ) //TODO: implement use of global_data-gps also in nmea mode (currently only in ubx mode)
// {
// printf("\t%s: nmea mode\n");
// //get gps data into info
// read_gps_nmea(fd, gps_rx_buffer, buffer_size, info, &parser);
// //convert latitude longitude
// lat_dec = ndeg2degree(info->lat);
// lon_dec = ndeg2degree(info->lon);
//
// //Test output
//// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inview:%d\n", (int)(lat_dec*1e6), (int)(lon_dec*1e6), (int)(info->elv*1e6), info->sig, info->fix, info->satinfo.inview);
// }
//// else if ( !strcmp("ubx",mode) )
//// {
////
//// //get gps data into info
//// read_gps_ubx(fd, gps_rx_buffer, buffer_size, ubx_state); //TODO: atm using the info struct from the nmea library, once the gps/mavlink structures are clear--> use own struct
////// lat_dec = info->lat;
////// lon_dec = info->lon;
////// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inuse:%d, PDOP:%d\n", (int)(lat_dec*1e6), (int)(lon_dec*1e6), (int)(info->elv*1e6), info->sig, info->fix, info->satinfo.inuse, (int)(info->PDOP*1e4));
//// }
// else if ( !strcmp("custom",mode) ) //TODO: implement use of global_data-gps also in custom mode (currently only in ubx mode)
// {
// //info is used as storage of the gps information. lat lon are already in fractional degree format * 10e6
// //see custom.h/mtk_parse for more information on which variables are stored in info
// nmea_zero_INFO(info);
//
// //get gps data into info
// read_gps_custom(fd, gps_rx_buffer, buffer_size, info, mtk_state);
//
// //Test output
//// printf("Lat:%d, Lon:%d,Elev:%d, Sig:%d, Fix:%d, Inview:%d\n", (int)(info->lat), (int)info->lon, (int)info->elv, info->sig, info->fix, info->satinfo.inview);
//
// }
//
//
//
//
//
// }
free(gps_rx_buffer);
// free(info);
//close port
close_port(fd);
//destroy gps parser
// nmea_parser_destroy(&parser);
return 0;
}
