aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/ardrone_interface/ardrone_interface.c4
-rw-r--r--apps/commander/commander.c32
-rw-r--r--apps/commander/state_machine_helper.c34
-rw-r--r--apps/mavlink/mavlink.c10
-rw-r--r--apps/mavlink_onboard/mavlink.c6
-rw-r--r--apps/mavlink_onboard/mavlink_receiver.c2
-rw-r--r--apps/multirotor_att_control/multirotor_att_control_main.c41
7 files changed, 86 insertions, 43 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index aeaf830de..264041e65 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -331,7 +331,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
- if (armed.armed && !armed.lockdown) {
+ //XXX fix this
+ //if (armed.armed && !armed.lockdown) {
+ if (state.flag_fmu_armed) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 2fdcf4ce3..a3e8fb745 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -60,13 +60,9 @@
#include <debug.h>
#include <sys/prctl.h>
#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
#include <math.h>
#include <poll.h>
+
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/battery_status.h>
@@ -80,11 +76,19 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
-#include <mavlink/mavlink_log.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+
+#include "state_machine_helper.h"
+
/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
#include <drivers/drv_accel.h>
@@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-#include <systemlib/cpuload.h>
+
extern struct system_load_s system_load;
/* Decouple update interval and hysteris counters, all depends on intervals */
@@ -120,6 +124,8 @@ extern struct system_load_s system_load;
#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */
+
/* File descriptors */
static int leds;
static int buzzer;
@@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
+
+
current_status.navigation_state = NAVIGATION_STATE_INIT;
current_status.arming_state = ARMING_STATE_INIT;
current_status.hil_state = HIL_STATE_OFF;
+ current_status.flag_hil_enabled = false;
current_status.flag_fmu_armed = false;
current_status.flag_io_armed = false; // XXX read this from somewhere
@@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[])
last_local_position_time = local_position.timestamp;
}
+ /* set the condition to valid if there has recently been a local position estimate */
+ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) {
+ current_status.condition_local_position_valid = true;
+ } else {
+ current_status.condition_local_position_valid = false;
+ }
+
/* update battery status */
orb_check(battery_sub, &new_data);
if (new_data) {
@@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[])
// current_status.state_machine == SYSTEM_STATE_AUTO ||
// current_status.state_machine == SYSTEM_STATE_MANUAL)) {
// /* armed */
-// led_toggle(LED_BLUE);
+ led_toggle(LED_BLUE);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 742b7fe07..79394e2ba 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -40,19 +40,20 @@
#include <stdio.h>
#include <unistd.h>
+#include <stdint.h>
#include <stdbool.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
-
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) {
int ret = ERROR;
@@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* allow going back from INIT for calibration */
if (current_state->arming_state == ARMING_STATE_STANDBY) {
ret = OK;
- current_state->flag_system_armed = false;
+ current_state->flag_fmu_armed = false;
}
break;
case ARMING_STATE_STANDBY:
@@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* sensors need to be initialized for STANDBY state */
if (current_state->condition_system_sensors_initialized) {
ret = OK;
- current_state->flag_system_armed = false;
+ current_state->flag_fmu_armed = false;
} else {
mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized");
}
@@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for arming? */
ret = OK;
- current_state->flag_system_armed = true;
+ current_state->flag_fmu_armed = true;
}
break;
case ARMING_STATE_ARMED_ERROR:
@@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
/* XXX conditions for an error state? */
ret = OK;
- current_state->flag_system_armed = true;
+ current_state->flag_fmu_armed = true;
}
break;
case ARMING_STATE_STANDBY_ERROR:
@@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta
|| current_state->arming_state == ARMING_STATE_INIT
|| current_state->arming_state == ARMING_STATE_ARMED_ERROR) {
ret = OK;
- current_state->flag_system_armed = false;
+ current_state->flag_fmu_armed = false;
}
break;
default:
@@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = false;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = false;
}
break;
@@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = true;
}
}
break;
@@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = false;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = true;
}
break;
@@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = false;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
break;
@@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
current_state->flag_control_attitude_enabled = true;
current_state->flag_control_velocity_enabled = true;
current_state->flag_control_position_enabled = true;
+ current_state->flag_control_manual_enabled = false;
}
}
break;
@@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
//void publish_armed_status(const struct vehicle_status_s *current_status)
//{
// struct actuator_armed_s armed;
-// armed.armed = current_status->flag_system_armed;
+// armed.armed = current_status->flag_fmu_armed;
// /* lock down actuators if required, only in HIL */
// armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
@@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-//// current_status->flag_system_armed) {
+//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
@@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
// }
//
// /* vehicle is disarmed, mode requests arming */
-// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only arm in standby state */
// // XXX REMOVE
// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
@@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
// }
//
// /* vehicle is armed, mode requests disarming */
-// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 342328728..c443d5a4a 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -230,10 +230,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
-// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
-//
-// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED;
-// }
+ if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+ || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) {
+
+ *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+ }
//
// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) {
//
diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c
index fbfce7dc9..057a573b1 100644
--- a/apps/mavlink_onboard/mavlink.c
+++ b/apps/mavlink_onboard/mavlink.c
@@ -296,6 +296,12 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
+ if (v_status->flag_control_velocity_enabled) {
+ *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+ } else {
+ *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
+ }
+
// switch (v_status->state_machine) {
// case SYSTEM_STATE_PREFLIGHT:
// if (v_status->flag_preflight_gyro_calibration ||
diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c
index 0acbea675..2d406fb9f 100644
--- a/apps/mavlink_onboard/mavlink_receiver.c
+++ b/apps/mavlink_onboard/mavlink_receiver.c
@@ -328,4 +328,4 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
return thread;
-} \ No newline at end of file
+}
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 79ca9fe2d..3329c5c48 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[])
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
- bool flag_system_armed = false;
+ bool flag_fmu_armed = false;
+ bool flag_control_position_enabled = false;
+ bool flag_control_velocity_enabled = false;
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
@@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[])
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
float failsafe_throttle = 0.0f;
-
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
@@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[])
/* initialize to current yaw if switching to manual or att control */
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
- state.flag_fmu_armed != flag_system_armed) {
+ state.flag_fmu_armed != flag_fmu_armed) {
att_sp.yaw_body = att.yaw;
}
@@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[])
// * settings as well.
// */
//
-// /* only move setpoint if manual input is != 0 */
-// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-// rates_sp.yaw = manual.yaw;
-// control_yaw_position = false;
-// first_time_after_yaw_speed_control = true;
-//
-// } else {
-// if (first_time_after_yaw_speed_control) {
-// att_sp.yaw_body = att.yaw;
-// first_time_after_yaw_speed_control = false;
-// }
-//
-// control_yaw_position = true;
-// }
+ /* only move setpoint if manual input is != 0 */
+ if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+ rates_sp.yaw = manual.yaw;
+ control_yaw_position = false;
+ first_time_after_yaw_speed_control = true;
+
+ } else {
+ if (first_time_after_yaw_speed_control) {
+ att_sp.yaw_body = att.yaw;
+ first_time_after_yaw_speed_control = false;
+ }
+
+ control_yaw_position = true;
+ }
// }
// }
@@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[])
/* STEP 2: publish the controller output */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
@@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[])
/* update state */
flag_control_attitude_enabled = state.flag_control_attitude_enabled;
flag_control_manual_enabled = state.flag_control_manual_enabled;
- flag_system_armed = state.flag_fmu_armed;
- // XXX add some logic to this
+ flag_control_position_enabled = state.flag_control_position_enabled;
+ flag_control_velocity_enabled = state.flag_control_velocity_enabled;
+ flag_fmu_armed = state.flag_fmu_armed;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 12:07:09 +0000