fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed to publishing armed state in commander

Browse Source
This commit is contained in:
Lorenz Meier 2012-09-02 11:33:52 +02:00
parent e9373752d1
commit cae070c73e
4 changed files with 9 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
apps/commander/state_machine_helper.c
View File

@ -43,6 +43,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
@ -198,6 +199,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
ret = OK;
}
if (invalid_state) {

20
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -117,7 +117,6 @@ 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);
armed.armed = false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* register the perf counter */
@ -151,25 +150,13 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
} else {
if (state.flag_system_armed) {
att_sp.thrust = manual.throttle;
armed.armed = true;
} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
armed.armed = false;
} else {
/* limit motor throttle to zero for an unknown mode */
armed.armed = false;
}
att_sp.thrust = manual.throttle;
}
multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode);
multirotor_control_attitude(&att_sp, &att, &actuators);
/* publish the result */
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@ -179,8 +166,6 @@ mc_thread_main(int argc, char *argv[])
printf("[multirotor att control] stopping, disarming motors.\n");
/* kill all outputs */
armed.armed = false;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
@ -190,7 +175,6 @@ mc_thread_main(int argc, char *argv[])
close(state_sub);
close(manual_sub);
close(actuator_pub);
close(armed_pub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);

3
apps/multirotor_att_control/multirotor_attitude_control.c
View File

@ -188,8 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
struct actuator_controls_s *actuators, bool verbose)
const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;

4
apps/multirotor_att_control/multirotor_attitude_control.h
View File

@ -54,8 +54,6 @@
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose);
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */