diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index d2e0efb560..159a70701e 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -257,7 +257,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { .fd = sub_raw, .events = POLLIN }, { .fd = sub_params, .events = POLLIN } }; - int ret = poll(fds, 1, 1000); + int ret = poll(fds, 2, 1000); if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f29cef165b..c569a6aa4a 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) struct sensor_combined_s raw; /* 30 seconds */ - const uint64_t calibration_interval_us = 45 * 1000000; + int calibration_interval_ms = 30 * 1000; unsigned int calibration_counter = 0; float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; @@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes."); + mavlink_log_info(mavlink_fd, "[commander] Please rotate around X"); uint64_t calibration_start = hrt_absolute_time(); - while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) { + while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; @@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] mag cal canceled"); break; } } + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + /* disable calibration mode */ status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); @@ -1307,19 +1309,12 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - //update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1d24d6a6d..1e82bc417b 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -501,9 +501,10 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = true; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -516,7 +517,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -529,7 +532,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur { int old_mode = current_status->flight_mode; current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = true; + current_status->flag_control_manual_enabled = true; //XXX + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index cec2593c1f..f36fb009d4 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1802,7 +1802,9 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_pm_queued_send(); /* sleep quarter the time */ usleep(25000); - mavlink_pm_queued_send(); + if (baudrate > 57600) { + mavlink_pm_queued_send(); + } /* sleep 10 ms */ usleep(10000); diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 9961b35f82..40f1bbfde5 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -82,68 +82,6 @@ static orb_advert_t actuator_pub; static struct vehicle_status_s state; -/** - * Perform rate control right after gyro reading - */ -static void *rate_control_thread_main(void *arg) -{ - prctl(PR_SET_NAME, "mc rate control", getpid()); - - struct actuator_controls_s actuators; - - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - -// struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - struct vehicle_attitude_s vehicle_attitude; - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; - - while (true) { -// /* rate control at maximum rate */ -// /* wait for a sensor update, check for exit condition every 1000 ms */ -// int ret = poll(&fds, 1, 1000); -// -// if (ret < 0) { -// /* XXX this is seriously bad - should be an emergency */ -// } else if (ret == 0) { -// /* XXX this means no sensor data - should be critical or emergency */ -// printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); -// } else { - /* get current angular rate */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude); - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); - } - - /* perform local lowpass */ - - /* apply controller */ -// if (state.flag_control_rates_enabled) { - /* lowpass gyros */ - // XXX - gyro_lp[0] = vehicle_attitude.rollspeed; - gyro_lp[1] = vehicle_attitude.pitchspeed; - gyro_lp[2] = vehicle_attitude.yawspeed; - - //multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - printf(".\n"); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); -// } -// } - usleep(5000); - } - - return NULL; -} - static int mc_thread_main(int argc, char *argv[]) { @@ -187,9 +125,10 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -197,13 +136,6 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); - /* ready, spawn pthread */ - pthread_attr_t rate_control_attr; - pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 4096); - pthread_t rate_control_thread; - pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -236,13 +168,24 @@ mc_thread_main(int argc, char *argv[]) if (state.flag_control_manual_enabled) { /* manual inputs, from RC control or joystick */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller - /* set yaw rate */ - rates_sp.yaw = manual.yaw; - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); + + if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } + + if (state.flag_control_attitude_enabled) { + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + } /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -280,31 +223,36 @@ mc_thread_main(int argc, char *argv[]) /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ -// /* run attitude controller */ -// if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { -// multirotor_control_attitude(&att_sp, &att, NULL, &actuators); -// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); -// } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { -// multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); -// orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); -// } - float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + /* run attitude controller */ + // if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + // multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + // orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + // } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + // multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + // orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + // } - gyro_lp[0] = att.rollspeed; - gyro_lp[1] = att.pitchspeed; - gyro_lp[2] = att.yawspeed; - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; // XXX Hack, remove, switch to yaw rate controller - /* set yaw rate */ - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + if (state.flag_control_rates_enabled) { - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + float gyro[3] = {0.0f, 0.0f, 0.0f}; + + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* apply controller */ + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } perf_end(mc_loop_perf); } @@ -326,8 +274,6 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - pthread_join(rate_control_thread, NULL); - fflush(stdout); exit(0); } diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 1d400f51bf..372b378d19 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -155,32 +155,33 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + parameters_init(&h); + parameters_update(&h, &p); + initialized = true; + } /* load new parameters with lower rate */ - - if (motor_skip_counter % 500 == 0) { + if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); + printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p); } /* calculate current control outputs */ - float setpointXrate; - float setpointYrate; - float setpointZrate; - float setRollRate=rate_sp->roll; - float setPitchRate=rate_sp->pitch; - float setYawRate=rate_sp->yaw; + + /* control pitch (forward) output */ + float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]); + /* control roll (left/right) output */ + float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] ); - //x-axis - setpointXrate=p.attrate_p*(setRollRate-rates[0]); - //Y-axis - setpointYrate=p.attrate_p*(setPitchRate-rates[1]); - //Z-axis - setpointZrate=p.yawrate_p*(setYawRate-rates[2]); + /* control yaw rate */ + float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); - actuators->control[0] = setpointXrate; //roll - actuators->control[1] = setpointYrate; //pitch - actuators->control[2] = setpointZrate; //yaw + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; actuators->control[3] = rate_sp->thrust; motor_skip_counter++;