aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJulian Oes <joes@student.ethz.ch>2012-09-25 21:35:02 +0200
committerJulian Oes <joes@student.ethz.ch>2012-09-25 21:35:02 +0200
commitabbe998506e4ba49bbf6a9a9ae731b1eec521db6 (patch)
tree181fb20a6d091e2c516bb0b8409571093bdac7d1
parent0eae48d480edae2f22fc1f486f26609a49c9d69e (diff)
downloadpx4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.tar.gz
px4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.tar.bz2
px4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.zip
ardrone in the air again (workaround: rate controller disabled)
-rw-r--r--apps/ardrone_interface/ardrone_interface.c11
-rw-r--r--apps/ardrone_interface/ardrone_motor_control.c9
-rw-r--r--apps/multirotor_att_control/multirotor_att_control_main.c16
-rw-r--r--apps/multirotor_att_control/multirotor_attitude_control.c7
4 files changed, 35 insertions, 8 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index 8d77e7502..f12f9cb47 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -239,14 +239,15 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* declare and safely initialize all structs */
struct vehicle_status_s state;
- memset(&state, 0, sizeof(state));
+ //memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
- memset(&actuator_controls, 0, sizeof(actuator_controls));
+ //memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
@@ -328,7 +329,13 @@ int ardrone_interface_thread_main(int argc, char *argv[])
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
+
+
+
+ //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]);
+
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
+
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 787db1877..cbf9600a5 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -368,6 +368,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
+ //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
+
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */
@@ -387,15 +389,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
-
+ //printf("0 silent\n");
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
-
+ //printf("1 starting\n");
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
-
+ //printf("2 working\n");
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
+ //printf("3 full\n");
}
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 21c720096..70b9d8e28 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
-static struct actuator_controls_s actuators;
+
static orb_advert_t actuator_pub;
/**
@@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg)
{
prctl(PR_SET_NAME, "mc rate control", getpid());
+ struct actuator_controls_s actuators;
+
int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
@@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg)
gyro_lp[1] = gyro_report.y;
gyro_lp[2] = gyro_report.z;
- multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
- orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+// multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
+// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// }
}
}
@@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[])
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
+ struct actuator_controls_s actuators;
+
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[])
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.thrust = manual.throttle;
+ //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
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);
@@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[])
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+// printf("publish actuator\n");
+
+// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]);
+
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);
+// printf("publish attitude\n");
}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 2129915d1..c25e96856 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -312,6 +312,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
actuators->control[3] = motor_thrust;
}
+// if(motor_skip_counter%20 == 0)
+// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]);
+
+
// XXX change yaw rate to yaw pos controller
if (rates_sp) {
rates_sp->roll = roll_control;
@@ -320,5 +324,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
rates_sp->thrust = motor_thrust;
}
+// if(motor_skip_counter%20 == 0)
+// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust);
+
motor_skip_counter++;
}