diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-09 16:31:04 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-09 16:31:04 +0200 |
commit | 613e12fcac07a17e6b9d25b05f58c8a1b9587f5e (patch) | |
tree | b6ce6ef89c1f4bbef7ada6f32a3342bb727d4d61 /apps/multirotor_att_control | |
parent | f292b03772ddf9a0ae72615248c65959a110d8e2 (diff) | |
download | px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.tar.gz px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.tar.bz2 px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.zip |
working offboard
Diffstat (limited to 'apps/multirotor_att_control')
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 62 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 9 |
2 files changed, 39 insertions, 32 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 40f1bbfde..7d5821d3f 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -165,8 +165,30 @@ mc_thread_main(int argc, char *argv[]) /** STEP 1: Define which input is the dominating control input */ - - if (state.flag_control_manual_enabled) { + if (state.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + printf("thrust_rate=%8.4f\n",offboard_sp.p4); + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + printf("thrust_att=%8.4f\n",offboard_sp.p4); + 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); + } + + /* decide wether we want rate or position input */ + } + else if (state.flag_control_manual_enabled) { /* manual inputs, from RC control or joystick */ if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) { @@ -188,7 +210,7 @@ mc_thread_main(int argc, char *argv[]) } /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - + if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -199,39 +221,19 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (state.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - 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); - } - - /* decide wether we want rate or position input */ } /** 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); - // } + 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); + } if (state.flag_control_rates_enabled) { diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 372b378d1..7532dffa2 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -144,6 +144,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { + static float roll_control_last=0; + static float pitch_control_last=0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -172,10 +174,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]); + + float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last); + pitch_control_last=pitch_control; /* control roll (left/right) output */ - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] ); + float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last); + roll_control_last=roll_control; /* control yaw rate */ float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); |