dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of https://github.com/PX4/Firmware 

Conflicts:
	apps/commander/commander.c
	apps/multirotor_att_control/multirotor_att_control_main.c
	apps/multirotor_att_control/multirotor_rate_control.c
This commit is contained in:
tnaegeli 2012-10-04 16:01:42 +02:00
parent b9de72a8c9 2a06b66845
commit 8dfa66cb97
6 changed files with 84 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -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 */

19
apps/commander/commander.c
View File

@ -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, &current_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, &current_status, mavlink_fd);
//update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
update_state_machine_mode_auto(stat_pub, &current_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, &current_status, mavlink_fd);
}

13
apps/commander/state_machine_helper.c
View File

@ -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) {

4
apps/mavlink/mavlink.c
View File

@ -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);

146
apps/multirotor_att_control/multirotor_att_control_main.c
View File

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

35
apps/multirotor_att_control/multirotor_rate_control.c
View File

@ -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++;