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

Merge branch 'tobi'

This commit is contained in:
Lorenz Meier 2012-10-08 18:02:49 +02:00
parent d068025fcd 0edd4063af
commit a29e8e00fa
6 changed files with 29 additions and 75 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 */

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

2
apps/gps/ubx.c
View File

@ -781,6 +781,8 @@ void *ubx_watchdog_loop(void *args)
} else {
/* gps healthy */
ubx_success_count++;
ubx_healthy = true;
ubx_fail_count = 0;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);

4
apps/mavlink/mavlink.c
View File

@ -1824,7 +1824,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);

4
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -223,6 +223,7 @@ 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);
@ -253,7 +254,6 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
perf_end(mc_loop_perf);
}
@ -321,7 +321,7 @@ int multirotor_att_control_main(int argc, char *argv[])
mc_task = task_spawn("multirotor_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
4096,
6000,
mc_thread_main,
NULL);
exit(0);

82
apps/multirotor_att_control/multirotor_rate_control.c
View File

@ -56,18 +56,21 @@
// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
float yawrate_awu;
float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
float attrate_awu;
float attrate_lim;
@ -79,11 +82,13 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t yawrate_awu;
param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
param_t attrate_awu;
param_t attrate_lim;
};
@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
/* PID parameters */
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->yawrate_awu = param_find("MC_YAWRATE_AWU");
h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
h->attrate_awu = param_find("MC_ATTRATE_AWU");
h->attrate_lim = param_find("MC_ATTRATE_LIM");
@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
{
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
param_get(h->attrate_awu, &(p->attrate_awu));
param_get(h->attrate_lim, &(p->attrate_lim));
@ -157,83 +166,22 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
printf("p.yawrate_p: %8.4f", (double)p.yawrate_p);
}
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch);
float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch - rates[1]);
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * (rates[0] - rate_sp->roll);
float roll_control = p.attrate_p * deltaT * (rate_sp->roll - rates[0]);
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * (rates[2] - rate_sp->yaw);
/*
* compensate the vertical loss of thrust
* when thrust plane has an angle.
* start with a factor of 1.0 (no change)
*/
float zcompensation = 1.0f;
// if (fabsf(att->roll) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->roll);
// }
// if (fabsf(att->pitch) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->pitch);
// }
float motor_thrust = 0.0f;
motor_thrust = rate_sp->thrust;
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
// /* limit yaw rate output */
// if (yaw_rate_control > p.yawrate_lim) {
// yaw_rate_control = p.yawrate_lim;
// yaw_speed_controller.saturated = 1;
// }
// if (yaw_rate_control < -p.yawrate_lim) {
// yaw_rate_control = -p.yawrate_lim;
// yaw_speed_controller.saturated = 1;
// }
// if (pitch_control > p.attrate_lim) {
// pitch_control = p.attrate_lim;
// pitch_controller.saturated = 1;
// }
// if (pitch_control < -p.attrate_lim) {
// pitch_control = -p.attrate_lim;
// pitch_controller.saturated = 1;
// }
// if (roll_control > p.attrate_lim) {
// roll_control = p.attrate_lim;
// roll_controller.saturated = 1;
// }
// if (roll_control < -p.attrate_lim) {
// roll_control = -p.attrate_lim;
// roll_controller.saturated = 1;
// }
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw - rates[2]);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}