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

pulled from PX4 master

Browse Source
This commit is contained in:
Roman Bapst
2014-11-10 17:16:30 +01:00
parent 43ebaf287c 4e8e6e653b
commit 31238516b5
140 changed files with 4577 additions and 1221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+32 -18
View File
@@ -83,7 +83,7 @@
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
@@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = {
0.0f,
1.0f,
@@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) {
res = ERROR;
break;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(3);
continue;
}
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
/* inform user about already handled side */
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(4);
continue;
}
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
@@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0;
}
}
src/modules/commander/airspeed_calibration.cpp
+17 -8
View File
@@ -61,6 +61,15 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
@@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
close(diff_pres_sub);
return ERROR;
}
@@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
src/modules/commander/commander.cpp
+393 -82
View File
@@ -128,8 +128,6 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT (10 * 1000 * 1000)
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
@@ -209,7 +207,9 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub);
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
orb_advert_t *home_pub);
/**
* Mainloop of commander.
@@ -232,7 +232,8 @@ void print_reject_arm(const char *msg);
void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
@@ -262,7 +263,7 @@ int commander_main(int argc, char *argv[])
daemon_task = task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2950,
3200,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -395,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -408,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
}
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
&& (cmd->target_component != 0))) { // component_id 0: valid for all components
return false;
}
@@ -539,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
} else {
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
(double)cmd->param4,
(double)cmd->param5,
(double)cmd->param6,
(double)cmd->param7);
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
(double)cmd->param4,
(double)cmd->param5,
(double)cmd->param6,
(double)cmd->param7);
}
}
break;
@@ -555,11 +558,44 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe");
warnx("forcing failsafe (termination)");
} else {
armed_local->force_failsafe = false;
warnx("disabling failsafe");
warnx("disabling failsafe (termination)");
}
/* param2 is currently used for other failsafe modes */
status_local->engine_failure_cmd = false;
status_local->data_link_lost_cmd = false;
status_local->gps_failure_cmd = false;
status_local->rc_signal_lost_cmd = false;
if ((int)cmd->param2 <= 0) {
/* reset all commanded failure modes */
warnx("reset all non-flighttermination failsafe commands");
} else if ((int)cmd->param2 == 1) {
/* trigger engine failure mode */
status_local->engine_failure_cmd = true;
warnx("engine failure mode commanded");
} else if ((int)cmd->param2 == 2) {
/* trigger data link loss mode */
status_local->data_link_lost_cmd = true;
warnx("data link loss mode commanded");
} else if ((int)cmd->param2 == 3) {
/* trigger gps loss mode */
status_local->gps_failure_cmd = true;
warnx("gps loss mode commanded");
} else if ((int)cmd->param2 == 4) {
/* trigger rc loss mode */
status_local->rc_signal_lost_cmd = true;
warnx("rc loss mode commanded");
}
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
@@ -611,6 +647,35 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
transition_result_t res = TRANSITION_DENIED;
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
main_state_pre_offboard = status_local->main_state;
}
if (cmd->param1 > 0.5f) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
status_local->offboard_control_set_by_command = false;
} else {
/* Set flag that offboard was set via command, main state is not overridden by rc */
status_local->offboard_control_set_by_command = true;
}
} else {
/* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */
res = main_state_transition(status_local, main_state_pre_offboard);
status_local->offboard_control_set_by_command = false;
}
}
break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -659,6 +724,12 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T");
param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
/* welcome user */
warnx("starting");
@@ -746,9 +817,12 @@ int commander_thread_main(int argc, char *argv[])
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
status.circuit_breaker_engaged_airspd_check = false;
status.circuit_breaker_engaged_enginefailure_check = false;
status.circuit_breaker_engaged_gpsfailure_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
@@ -774,11 +848,14 @@ int commander_thread_main(int argc, char *argv[])
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
orb_advert_t mission_pub = -1;
mission_s mission;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
mission.current_seq);
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq);
mission.dataman_id, mission.count, mission.current_seq);
} else {
const char *missionfail = "reading mission state failed";
warnx("%s", missionfail);
@@ -851,11 +928,13 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to telemetry status topics */
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM];
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_last_heartbeat[i] = 0;
telemetry_last_dl_loss[i] = 0;
telemetry_lost[i] = true;
}
@@ -941,6 +1020,15 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t arming_ret;
int32_t datalink_loss_enabled = false;
int32_t datalink_loss_timeout = 10;
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
/* Thresholds for engine failure detection */
int32_t ef_throttle_thres = 1.0f;
int32_t ef_current2throttle_thres = 0.0f;
int32_t ef_time_thres = 1000.0f;
uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
@@ -988,8 +1076,14 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
status_changed = true;
@@ -1001,6 +1095,12 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_takeoff_alt, &takeoff_alt);
param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
}
orb_check(sp_man_sub, &updated);
@@ -1021,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
status.offboard_control_signal_lost = false;
status_changed = true;
}
} else {
if (!status.offboard_control_signal_lost) {
status.offboard_control_signal_lost = true;
@@ -1039,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[])
/* perform system checks when new telemetry link connected */
if (mavlink_fd &&
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
telemetry_last_heartbeat[i] == 0 &&
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
(void)rc_calibration_check(mavlink_fd);
}
@@ -1054,6 +1155,27 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
/* Check if the barometer is healthy and issue a warning in the GCS if not so.
* Because the barometer is used for calculating AMSL altitude which is used to ensure
* vertical separation from other airtraffic the operator has to know when the
* barometer is inoperational.
* */
if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
/* handle the case where baro was regained */
if (status.barometer_failure) {
status.barometer_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "baro healthy");
}
} else {
if (!status.barometer_failure) {
status.barometer_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "baro failed");
}
}
}
orb_check(diff_pres_sub, &updated);
@@ -1069,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[])
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status.condition_power_input_valid = false;
} else {
status.condition_power_input_valid = true;
}
@@ -1092,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[])
/* disarm if safety is now on and still armed */
if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
arming_state_changed = true;
}
@@ -1138,9 +1263,8 @@ int commander_thread_main(int argc, char *argv[])
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
&status_changed);
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
@@ -1190,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[])
local_eph_good = false;
}
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
&& local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
&(status.condition_local_altitude_valid), &status_changed);
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
@@ -1222,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[])
/* get throttle (if armed), as we only care about energy negative throttle also counts */
float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah,
throttle);
}
}
@@ -1279,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
//struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
struct stat statbuf;
on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
@@ -1290,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
}
status_changed = true;
}
@@ -1318,7 +1450,8 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -1346,11 +1479,31 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize map projection if gps is valid */
if (!map_projection_global_initialized()
&& (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
&& (gps_position.eph < eph_threshold)
&& (gps_position.epv < epv_threshold)
&& hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) {
/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
}
/* check if GPS fix is ok */
if (status.circuit_breaker_engaged_gpsfailure_check ||
(gps_position.fix_type >= 3 &&
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
/* handle the case where gps was regained */
if (status.gps_failure) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps regained");
}
} else {
if (!status.gps_failure) {
status.gps_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps fix lost");
}
}
/* start mission result check */
@@ -1358,10 +1511,30 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
/* Check for geofence violation */
if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
//XXX: make this configurable to select different actions (e.g. navigation modes)
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of navigator request or geofence");
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
}
/* RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
@@ -1386,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY :
ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
stick_off_counter = 0;
} else {
@@ -1412,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[])
*/
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
@@ -1436,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
mavlink_log_info(mavlink_fd, "DISARMED by RC");
}
arming_state_changed = true;
} else if (arming_ret == TRANSITION_DENIED) {
@@ -1471,16 +1652,29 @@ int commander_thread_main(int argc, char *argv[])
/* data links check */
bool have_link = false;
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
/* handle the case where data link was regained */
if (telemetry_lost[i]) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_critical(mavlink_fd, "data link %i regained", i);
telemetry_lost[i] = false;
have_link = true;
} else if (!telemetry_lost[i]) {
/* telemetry was healthy also in last iteration
* we don't have to check a timeout */
have_link = true;
}
have_link = true;
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_critical(mavlink_fd, "data link %i lost", i);
telemetry_lost[i] = true;
@@ -1499,10 +1693,42 @@ int commander_thread_main(int argc, char *argv[])
if (!status.data_link_lost) {
mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
}
}
/* Check engine failure
* only for fixed wing for now
*/
if (!status.circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false &&
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres &&
battery.current_a / actuator_controls.control[3] <
ef_current2throttle_thres) ||
(status.engine_failure))) {
/* potential failure, measure time */
if (timestamp_engine_healthy > 0 &&
hrt_elapsed_time(&timestamp_engine_healthy) >
ef_time_thres * 1e6 &&
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "Engine Failure");
}
} else {
/* no failure reset flag */
timestamp_engine_healthy = hrt_absolute_time();
if (status.engine_failure) {
status.engine_failure = false;
status_changed = true;
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1516,6 +1742,57 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* Check for failure combinations which lead to flight termination */
if (armed.armed) {
/* At this point the data link and the gps system have been checked
* If we are not in a manual (RC stick controlled mode)
* and both failed we want to terminate the flight */
if (status.main_state != MAIN_STATE_MANUAL &&
status.main_state != MAIN_STATE_ACRO &&
status.main_state != MAIN_STATE_ALTCTL &&
status.main_state != MAIN_STATE_POSCTL &&
((status.data_link_lost && status.gps_failure) ||
(status.data_link_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of data link loss && gps failure");
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
}
}
/* At this point the rc signal and the gps system have been checked
* If we are in manual (controlled with RC):
* if both failed we want to terminate the flight */
if ((status.main_state == MAIN_STATE_ACRO ||
status.main_state == MAIN_STATE_MANUAL ||
status.main_state == MAIN_STATE_ALTCTL ||
status.main_state == MAIN_STATE_POSCTL) &&
((status.rc_signal_lost && status.gps_failure) ||
(status.rc_signal_lost_cmd && status.gps_failure_cmd))) {
armed.force_failsafe = true;
status_changed = true;
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
warnx("Flight termination because of RC signal loss && gps failure");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
}
}
}
hrt_abstime t1 = hrt_absolute_time();
/* print new state */
@@ -1550,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
}
arming_state_changed = false;
}
@@ -1557,7 +1835,8 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished);
mission_result.finished,
mission_result.stay_in_failsafe);
// TODO handle mode changes by commands
if (main_state_changed) {
@@ -1593,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available
&& safety.safety_off))) {
/* play tune when armed */
set_tune(TONE_ARMING_WARNING_TUNE);
arm_tune_played = true;
@@ -1763,9 +2043,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* if offboard is set allready by a mavlink command, abort */
if (status.offboard_control_set_by_command) {
return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
}
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD");
@@ -1787,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} else {
res = main_state_transition(status_local, MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
@@ -1801,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
print_reject_mode(status_local, "POSCTL");
}
// fallback to ALTCTL
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@@ -1825,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_RTL");
print_reject_mode(status_local, "AUTO_RTL");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
@@ -1841,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_LOITER");
print_reject_mode(status_local, "AUTO_LOITER");
} else {
res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
@@ -1850,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
break; // changed successfully or already in this state
}
print_reject_mode(status_local, "AUTO_MISSION");
print_reject_mode(status_local, "AUTO_MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
// fallback to LOITER if home position not set
res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
}
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
// fallback to POSCTL
res = main_state_transition(status_local, MAIN_STATE_POSCTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
@@ -1947,6 +2234,7 @@ set_control_mode()
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
@@ -1955,22 +2243,30 @@ set_control_mode()
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_force_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
//XXX: the flags could depend on sp_offboard.ignore
break;
default:
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
@@ -1979,6 +2275,7 @@ set_control_mode()
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
}
break;
case NAVIGATION_STATE_POSCTL:
@@ -1997,6 +2294,7 @@ set_control_mode()
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
@@ -2008,6 +2306,18 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
@@ -2175,7 +2485,8 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
src/modules/commander/commander_params.c
+66
View File
@@ -105,3 +105,69 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
/** Datalink loss time threshold
*
* After this amount of seconds without datalink the data link lost mode triggers
*
* @group commander
* @unit second
* @min 0
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/** Datalink regain time threshold
*
* After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
* flag is set back to false
*
* @group commander
* @unit second
* @min 0
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
/** Engine Failure Throttle Threshold
*
* Engine failure triggers only above this throttle value
*
* @group commander
* @min 0.0f
* @max 1.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/** Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current/throttle value
*
* @group commander
* @min 0.0f
* @max 7.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
/** Engine Failure Time Threshold
*
* Engine failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time
*
* @group commander
* @unit second
* @min 0.0f
* @max 7.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/** RC loss time threshold
*
* After this amount of seconds without RC connection the rc lost flag is set to true
*
* @group commander
* @unit second
* @min 0
* @max 35
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
src/modules/commander/commander_tests/commander_tests.cpp
+1 -3
View File
@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
stateMachineHelperTest();
return 0;
return stateMachineHelperTest() ? 0 : -1;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+5 -8
View File
@@ -49,7 +49,7 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual void runTests(void);
virtual bool run_tests(void);
private:
bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
return true;
}
void StateMachineHelperTest::runTests(void)
bool StateMachineHelperTest::run_tests(void)
{
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
return (_tests_failed == 0);
}
void stateMachineHelperTest(void)
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->runTests();
test->printResults();
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
src/modules/commander/commander_tests/state_machine_helper_test.h
+1 -1
View File
@@ -39,6 +39,6 @@
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void stateMachineHelperTest(void);
bool stateMachineHelperTest(void);
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
src/modules/commander/gyro_calibration.cpp
+4 -1
View File
@@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale = {
0.0f,
src/modules/commander/mag_calibration.cpp
+1 -1
View File
@@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0;
src/modules/commander/state_machine_helper.cpp
+32 -23
View File
@@ -443,7 +443,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -457,11 +458,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost && armed) {
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -497,14 +498,29 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
/* Finished handling commands which have priority , now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -528,31 +544,20 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
} else if (status->rc_signal_lost && !stay_in_failsafe) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
} else if (!stay_in_failsafe){
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
@@ -593,8 +598,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
src/modules/dataman/dataman.c
+1 -1
View File
@@ -797,7 +797,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
src/modules/dataman/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -7
View File
@@ -382,6 +382,7 @@ FixedwingEstimator::FixedwingEstimator() :
_local_pos({}),
_gps({}),
_wind({}),
_distance{},
_gyro_offsets({}),
_accel_offsets({}),
@@ -596,12 +597,12 @@ FixedwingEstimator::check_filter_state()
const char* const feedback[] = { 0,
"NaN in states, resetting",
"stale IMU data, resetting",
"stale sensor data, resetting",
"got initial position lock",
"excessive gyro offsets",
"GPS velocity divergence",
"velocity diverted, check accel config",
"excessive covariances",
"unknown condition"};
"unknown condition, resetting"};
// Print out error condition
if (check) {
@@ -613,7 +614,7 @@ FixedwingEstimator::check_filter_state()
}
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
}
struct estimator_status_report rep;
@@ -782,7 +783,7 @@ FixedwingEstimator::task_main()
while (!_task_should_exit) {
/* wait for up to 500ms for data */
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
@@ -1084,7 +1085,7 @@ FixedwingEstimator::task_main()
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
@@ -1369,7 +1370,7 @@ FixedwingEstimator::task_main()
if (newRangeData) {
_ekf->fuseRngData = true;
_ekf->useRangeFinder = true;
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
_ekf->GroundEKF();
}
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+8 -7
View File
@@ -119,6 +119,7 @@ AttPosEKF::AttPosEKF() :
inhibitWindStates(true),
inhibitMagStates(true),
inhibitGndState(true),
inhibitScaleState(true),
onGround(true),
staticMode(true),
@@ -2560,15 +2561,15 @@ void AttPosEKF::CovarianceInit()
P[22][22] = sq(0.5f);
}
float AttPosEKF::ConstrainFloat(float val, float min, float max)
float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
{
float ret;
if (val > max) {
ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
} else if (val < min) {
ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
if (val > max_val) {
ret = max_val;
ekf_debug("> max: %8.4f, val: %8.4f", (double)max_val, (double)val);
} else if (val < min_val) {
ret = min_val;
ekf_debug("< min: %8.4f, val: %8.4f", (double)min_val, (double)val);
} else {
ret = val;
}
src/modules/fw_att_control/fw_att_control_main.cpp
+15 -5
View File
@@ -721,8 +721,14 @@ void FixedwingAttitudeControl::task_main() {
float pitch_sp = _parameters.pitchsp_offset_rad;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled
|| _vcontrol_mode.flag_control_position_enabled) {
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_velocity_enabled ||
_vcontrol_mode.flag_control_position_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body
@@ -890,9 +896,13 @@ void FixedwingAttitudeControl::task_main() {
}
}
/* throttle passed through */
_actuators.control[3] =
(isfinite(throttle_sp)) ? throttle_sp : 0.0f;
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double) throttle_sp);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+123 -88
View File
@@ -88,7 +88,6 @@
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
@@ -141,11 +140,11 @@ private:
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _control_mode_sub; /**< vehicle status subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
@@ -156,21 +155,21 @@ private:
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
bool land_noreturn_vertical;
bool land_stayonground;
bool land_motor_lim;
bool land_onslope;
bool land_useterrain;
/* takeoff/launch states */
LaunchDetectionResult launch_detection_state;
@@ -241,7 +240,9 @@ private:
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
float range_finder_rel_alt;
float land_flare_pitch_min_deg;
float land_flare_pitch_max_deg;
int land_use_terrain_estimate;
} _parameters; /**< local copies of interesting parameters */
@@ -287,7 +288,9 @@ private:
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
param_t range_finder_rel_alt;
param_t land_flare_pitch_min_deg;
param_t land_flare_pitch_max_deg;
param_t land_use_terrain_estimate;
} _parameter_handles; /**< handles for interesting parameters */
@@ -304,21 +307,20 @@ private:
void control_update();
/**
* Check for changes in vehicle status.
* Check for changes in control mode
*/
void vehicle_control_mode_poll();
/**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
/**
* Check for airspeed updates.
*/
bool vehicle_airspeed_poll();
/**
* Check for range finder updates.
*/
bool range_finder_poll();
/**
* Check for position updates.
*/
@@ -340,9 +342,9 @@ private:
void navigation_capabilities_publish();
/**
* Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
/**
* Control position.
@@ -412,10 +414,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
@@ -429,10 +431,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
_range_finder(),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -442,6 +444,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
land_useterrain(false),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
landingslope(),
@@ -480,7 +483,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
@@ -581,8 +586,9 @@ FixedwingPositionControl::parameters_update()
}
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
@@ -639,16 +645,27 @@ FixedwingPositionControl::parameters_update()
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
bool vstatus_updated;
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_control_mode_sub, &vstatus_updated);
orb_check(_control_mode_sub, &updated);
if (vstatus_updated) {
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
}
void
FixedwingPositionControl::vehicle_status_poll()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
@@ -675,20 +692,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
return airspeed_updated;
}
bool
FixedwingPositionControl::range_finder_poll()
{
/* check if there is a range finder measurement */
bool range_finder_updated;
orb_check(_range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
}
return range_finder_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -826,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
float rel_alt_estimated = current_alt - land_setpoint_alt;
/* only use range finder if:
* parameter (range_finder_use_relative_alt) > 0
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
return rel_alt_estimated;
if (!isfinite(global_pos.terrain_alt)) {
return land_setpoint_alt;
}
return range_finder.distance;
/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
* for the whole landing */
if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
if(!land_useterrain) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
land_useterrain = true;
}
return global_pos.terrain_alt;
} else {
return land_setpoint_alt;
}
}
bool
@@ -945,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
wp_distance_save = 0.0f;
}
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
@@ -984,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Vertical landing control */
//xxx: using the tecs altitude controller for slope control for now
// /* do not go down too early */
// if (wp_distance > 50.0f) {
// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
* equal to _pos_sp_triplet.current.alt */
float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ?
_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -1015,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -1033,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true;
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
flare_pitch_angle_rad, math::radians(15.0f),
math::radians(_parameters.land_flare_pitch_min_deg),
math::radians(_parameters.land_flare_pitch_max_deg),
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed,
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
@@ -1059,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
float altitude_desired_rel;
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
@@ -1069,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
} else {
/* continue horizontally */
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel :
_global_pos.alt - terrain_alt;
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
@@ -1081,7 +1096,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_pos_sp_triplet.current.alt + relative_alt,
_global_pos.alt,
ground_speed);
}
@@ -1210,16 +1225,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
/* Copy thrust and pitch values from tecs
* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
} else {
/* Copy thrust and pitch values from tecs */
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
}
else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
}
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
@@ -1249,13 +1268,15 @@ FixedwingPositionControl::task_main()
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
/* rate limit control mode updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vehicle_status_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -1294,9 +1315,12 @@ FixedwingPositionControl::task_main()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
/* check vehicle status for changes to publication state */
vehicle_status_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -1326,7 +1350,6 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
@@ -1390,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state()
land_stayonground = false;
land_motor_lim = false;
land_onslope = false;
land_useterrain = false;
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
@@ -1408,7 +1432,12 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Force the slow downwards spiral */
limitOverride.enablePitchMinOverride(-1.0f);
limitOverride.enablePitchMaxOverride(5.0f);
} else if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
@@ -1424,7 +1453,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* No underspeed protection in landing mode */
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Force the slow downwards spiral */
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
}
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+21 -5
View File
@@ -412,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
/**
* Relative altitude threshold for range finder measurements for use during landing
* Enable or disable usage of terrain estimate during landing
*
* range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
* set to < 0 to disable
* the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
* 0: disabled, 1: enabled
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
PARAM_DEFINE_INT32(FW_LND_USETER, 0);
/**
* Flare, minimum pitch
*
* Minimum pitch during flare, a positive sign means nose up
* Applied once FW_LND_TLALT is reached
*
*/
PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
/**
* Flare, maximum pitch
*
* Maximum pitch during flare, a positive sign means nose up
* Applied once FW_LND_TLALT is reached
*
*/
PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
src/modules/mavlink/mavlink.c
+8 -5
View File
@@ -68,14 +68,17 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
* control mode
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
50,
MAV_TYPE_FIXED_WING,
0,
0,
0
50
}; // System ID, 1-255, Component/Subsystem ID, 1-255
/*
src/modules/mavlink/mavlink_ftp.cpp
+249 -43
View File
@@ -34,6 +34,7 @@
/// @file mavlink_ftp.cpp
/// @author px4dev, Don Gagne <don@thegagnes.com>
#include <crc32.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
@@ -180,12 +181,16 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workList(payload);
break;
case kCmdOpenFile:
errorCode = _workOpen(payload, false);
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, true);
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
@@ -200,6 +205,14 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
@@ -208,6 +221,11 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
@@ -222,6 +240,7 @@ out:
warnx("FTP: ack\n");
#endif
} else {
int r_errno = errno;
warnx("FTP: nak %u", errorCode);
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
@@ -229,7 +248,7 @@ out:
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = errno;
payload->data[1] = r_errno;
}
}
@@ -396,27 +415,27 @@ MavlinkFTP::_workList(PayloadHeader* payload)
/// @brief Responds to an Open command
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
{
int session_index = _find_unused_session();
if (session_index < 0) {
warnx("FTP: Open failed - out of sessions\n");
return kErrNoSessionsAvailable;
}
char *filename = _data_as_cstring(payload);
uint32_t fileSize = 0;
if (!create) {
struct stat st;
if (stat(filename, &st) != 0) {
return kErrFailErrno;
}
fileSize = st.st_size;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
char *filename = _data_as_cstring(payload);
uint32_t fileSize = 0;
struct stat st;
if (stat(filename, &st) != 0) {
// fail only if requested open for read
if (oflag & O_RDONLY)
return kErrFailErrno;
else
st.st_size = 0;
}
fileSize = st.st_size;
int fd = ::open(filename, oflag);
if (fd < 0) {
return kErrFailErrno;
@@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
_session_fds[session_index] = fd;
payload->session = session_index;
if (create) {
payload->size = 0;
} else {
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
}
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
return kErrNone;
}
@@ -470,29 +485,33 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
int session_index = payload->session;
// look up session
auto session = getSession(hdr->session);
if (session == nullptr) {
return kErrNoSession;
if (!_valid_session(session_index)) {
return kErrInvalidSession;
}
// append to file
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
if (result < 0) {
// XXX might also be no space, I/O, etc.
return kErrNotAppend;
}
hdr->size = result;
return kErrNone;
#else
return kErrUnknownCommand;
// Seek to the specified position
#ifdef MAVLINK_FTP_DEBUG
warnx("seek %d", payload->offset);
#endif
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");
return kErrFailErrno;
}
int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
if (bytes_written < 0) {
// Negative return indicates error other than eof
warnx("write fail %d", bytes_written);
return kErrFailErrno;
}
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = bytes_written;
return kErrNone;
}
/// @brief Responds to a RemoveFile command
@@ -510,6 +529,81 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
}
}
/// @brief Responds to a TruncateFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
{
char file[kMaxDataLength];
const char temp_file[] = "/fs/microsd/.trunc.tmp";
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
payload->size = 0;
// emulate truncate(file, payload->offset) by
// copying to temp and overwrite with O_TRUNC flag.
struct stat st;
if (stat(file, &st) != 0) {
return kErrFailErrno;
}
if (!S_ISREG(st.st_mode)) {
errno = EISDIR;
return kErrFailErrno;
}
// check perms allow us to write (not romfs)
if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
errno = EROFS;
return kErrFailErrno;
}
if (payload->offset == (unsigned)st.st_size) {
// nothing to do
return kErrNone;
}
else if (payload->offset == 0) {
// 1: truncate all data
int fd = ::open(file, O_TRUNC | O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
::close(fd);
return kErrNone;
}
else if (payload->offset > (unsigned)st.st_size) {
// 2: extend file
int fd = ::open(file, O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
::close(fd);
return kErrFailErrno;
}
bool ok = 1 == ::write(fd, "", 1);
::close(fd);
return (ok)? kErrNone : kErrFailErrno;
}
else {
// 3: truncate
if (_copy_file(file, temp_file, payload->offset) != 0) {
return kErrFailErrno;
}
if (_copy_file(temp_file, file, payload->offset) != 0) {
return kErrFailErrno;
}
if (::unlink(temp_file) != 0) {
return kErrFailErrno;
}
return kErrNone;
}
}
/// @brief Responds to a Terminate command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
@@ -542,6 +636,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
return kErrNone;
}
/// @brief Responds to a Rename command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRename(PayloadHeader* payload)
{
char oldpath[kMaxDataLength];
char newpath[kMaxDataLength];
char *ptr = _data_as_cstring(payload);
size_t oldpath_sz = strlen(ptr);
if (oldpath_sz == payload->size) {
// no newpath
errno = EINVAL;
return kErrFailErrno;
}
strncpy(oldpath, ptr, kMaxDataLength);
strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
if (rename(oldpath, newpath) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
}
/// @brief Responds to a RemoveDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
@@ -572,6 +693,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
}
}
/// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
{
char file_buf[256];
uint32_t checksum = 0;
ssize_t bytes_read;
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
int fd = ::open(file_buf, O_RDONLY);
if (fd < 0) {
return kErrFailErrno;
}
do {
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
if (bytes_read < 0) {
int r_errno = errno;
::close(fd);
errno = r_errno;
return kErrFailErrno;
}
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
} while (bytes_read == sizeof(file_buf));
::close(fd);
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = checksum;
return kErrNone;
}
/// @brief Returns true if the specified session is a valid open session
bool
MavlinkFTP::_valid_session(unsigned index)
@@ -645,3 +799,55 @@ MavlinkFTP::_return_request(Request *req)
_unlock_request_queue();
}
/// @brief Copy file (with limited space)
int
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
{
char buff[512];
int src_fd = -1, dst_fd = -1;
int op_errno = 0;
src_fd = ::open(src_path, O_RDONLY);
if (src_fd < 0) {
return -1;
}
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
if (dst_fd < 0) {
op_errno = errno;
::close(src_fd);
errno = op_errno;
return -1;
}
while (length > 0) {
ssize_t bytes_read, bytes_written;
size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
bytes_read = ::read(src_fd, buff, blen);
if (bytes_read == 0) {
// EOF
break;
}
else if (bytes_read < 0) {
warnx("cp: read");
op_errno = errno;
break;
}
bytes_written = ::write(dst_fd, buff, bytes_read);
if (bytes_written != bytes_read) {
warnx("cp: short write");
op_errno = errno;
break;
}
length -= bytes_written;
}
::close(src_fd);
::close(dst_fd);
errno = op_errno;
return (length > 0)? -1 : 0;
}
src/modules/mavlink/mavlink_ftp.h
+11 -3
View File
@@ -89,13 +89,17 @@ public:
kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions
kCmdListDirectory, ///< List files in <path> from <offset>
kCmdOpenFile, ///< Opens file at <path> for reading, returns <session>
kCmdOpenFileRO, ///< Opens file at <path> for reading, returns <session>
kCmdReadFile, ///< Reads <size> bytes from <offset> in <session>
kCmdCreateFile, ///< Creates file at <path> for writing, returns <session>
kCmdWriteFile, ///< Appends <size> bytes to file in <session>
kCmdWriteFile, ///< Writes <size> bytes to <offset> in <session>
kCmdRemoveFile, ///< Remove file at <path>
kCmdCreateDirectory, ///< Creates directory at <path>
kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty
kCmdOpenFileWO, ///< Opens file at <path> for writing, returns <session>
kCmdTruncateFile, ///< Truncate file at <path> to <offset> length
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
@@ -138,9 +142,10 @@ private:
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
int _copy_file(const char *src_path, const char *dst_path, ssize_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, bool create);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
@@ -148,6 +153,9 @@ private:
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload);
ErrorCode _workTruncateFile(PayloadHeader *payload);
ErrorCode _workRename(PayloadHeader *payload);
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests
Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work
src/modules/mavlink/mavlink_main.cpp
+20 -8
View File
@@ -123,6 +123,7 @@ Mavlink::Mavlink() :
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_forward_externalsp(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
@@ -166,8 +167,10 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
_system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -483,6 +486,7 @@ void Mavlink::mavlink_update_system(void)
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
}
/* update system and component id */
@@ -522,13 +526,18 @@ void Mavlink::mavlink_update_system(void)
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
_system_type = system_type;
}
int32_t use_hil_gps;
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
int32_t forward_externalsp;
param_get(_param_forward_externalsp, &forward_externalsp);
_forward_externalsp = (bool)forward_externalsp;
}
int Mavlink::get_system_id()
@@ -748,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -756,7 +765,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
_last_write_try_time = hrt_absolute_time();
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -813,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
pthread_mutex_lock(&_send_mutex);
int buf_free = get_free_tx_buf();
unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
if (buf_free < TX_BUFFER_GAP) {
if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -1396,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 20.0f);
configure_stream("OPTICAL_FLOW", 5.0f);
break;
case MAVLINK_MODE_ONBOARD:
@@ -1404,6 +1413,9 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("ATTITUDE", 50.0f);
configure_stream("GLOBAL_POSITION_INT", 50.0f);
configure_stream("CAMERA_CAPTURE", 2.0f);
configure_stream("ATTITUDE_TARGET", 10.0f);
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream("VFR_HUD", 10.0f);
break;
default:
@@ -1624,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2700,
2900,
(main_t)&Mavlink::start_helper,
(const char **)argv);
src/modules/mavlink/mavlink_main.h
+9 -1
View File
@@ -137,6 +137,8 @@ public:
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
@@ -232,7 +234,7 @@ public:
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(const void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
@@ -263,6 +265,8 @@ public:
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
unsigned get_system_type() { return _system_type; }
protected:
Mavlink *next;
@@ -275,6 +279,7 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
@@ -349,6 +354,9 @@ private:
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
unsigned _system_type;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
src/modules/mavlink/mavlink_messages.cpp
+43 -5
View File
@@ -162,6 +162,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break;
case NAVIGATION_STATE_AUTO_RTL:
/* fallthrough */
case NAVIGATION_STATE_AUTO_RCRECOVER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -170,6 +172,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
@@ -298,7 +302,7 @@ protected:
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
msg.type = mavlink_system.type;
msg.type = _mavlink->get_system_type();
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
@@ -337,11 +341,18 @@ private:
/* do not allow top copying this class */
MavlinkStreamStatustext(MavlinkStreamStatustext &);
MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
FILE *fp = nullptr;
protected:
explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
~MavlinkStreamStatustext() {
if (fp) {
fclose(fp);
}
}
void send(const hrt_abstime t)
{
if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
@@ -355,6 +366,31 @@ protected:
strncpy(msg.text, logmsg.text, sizeof(msg.text));
_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
/* write log messages in first instance to disk */
if (_mavlink->get_instance_id() == 0) {
if (fp) {
fputs(msg.text, fp);
fputs("\n", fp);
fsync(fileno(fp));
} else {
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[64] = "";
timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
struct tm t;
gmtime_r(&gps_time_sec, &t);
// XXX we do not want to interfere here with the SD log app
strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
}
}
}
}
}
@@ -1317,15 +1353,17 @@ protected:
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (mavlink_system.type) {
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
src/modules/mavlink/mavlink_receiver.cpp
+298 -8
View File
@@ -37,6 +37,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
/* XXX trim includes */
@@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_flow_pub(-1),
_range_pub(-1),
_offboard_control_sp_pub(-1),
_local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
_att_sp_pub(-1),
_rates_sp_pub(-1),
_force_sp_pub(-1),
_pos_sp_triplet_pub(-1),
_vicon_position_pub(-1),
_vision_position_pub(-1),
_telemetry_status_pub(-1),
@@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
handle_message_set_position_target_local_ned(msg);
break;
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_message_set_attitude_target(msg);
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate(msg);
break;
@@ -381,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
f.timestamp = hrt_absolute_time();
f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.flow_raw_x = flow.integrated_x;
f.flow_raw_y = flow.integrated_y;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
@@ -403,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.ground_distance;
r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
@@ -474,6 +482,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
mavlink_set_position_target_local_ned_t set_position_target_local_ned;
mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
struct offboard_control_setpoint_s offboard_control_sp;
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
set_position_target_local_ned.target_system == 0) &&
(mavlink_system.compid == set_position_target_local_ned.target_component ||
set_position_target_local_ned.target_component == 0)) {
/* convert mavlink type (local, NED) to uORB offboard control struct */
switch (set_position_target_local_ned.coordinate_frame) {
case MAV_FRAME_LOCAL_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
break;
case MAV_FRAME_LOCAL_OFFSET_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
break;
case MAV_FRAME_BODY_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
break;
case MAV_FRAME_BODY_OFFSET_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
break;
default:
/* invalid setpoint, avoid publishing */
return;
}
offboard_control_sp.position[0] = set_position_target_local_ned.x;
offboard_control_sp.position[1] = set_position_target_local_ned.y;
offboard_control_sp.position[2] = set_position_target_local_ned.z;
offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
offboard_control_sp.yaw = set_position_target_local_ned.yaw;
offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
/* If we are in force control mode, for now set offboard mode to force control */
if (offboard_control_sp.isForceSetpoint) {
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
}
/* set ignore flags */
for (int i = 0; i < 9; i++) {
offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
}
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
OFB_IGN_BIT_YAW;
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
OFB_IGN_BIT_YAWRATE;
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
if (_control_mode.flag_control_offboard_enabled) {
if (offboard_control_sp.isForceSetpoint &&
offboard_control_sp_ignore_position_all(offboard_control_sp) &&
offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
/* The offboard setpoint is a force setpoint only, directly writing to the force
* setpoint topic and not publishing the setpoint triplet topic */
struct vehicle_force_setpoint_s force_sp;
force_sp.x = offboard_control_sp.acceleration[0];
force_sp.y = offboard_control_sp.acceleration[1];
force_sp.z = offboard_control_sp.acceleration[2];
//XXX: yaw
if (_force_sp_pub < 0) {
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
} else {
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
}
} else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
struct position_setpoint_triplet_s pos_sp_triplet;
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = offboard_control_sp.position[0];
pos_sp_triplet.current.y = offboard_control_sp.position[1];
pos_sp_triplet.current.z = offboard_control_sp.position[2];
} else {
pos_sp_triplet.current.position_valid = false;
}
/* set the local vel values if the setpoint type is 'local pos' and none
* of the local vel fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
} else {
pos_sp_triplet.current.velocity_valid = false;
}
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
pos_sp_triplet.current.acceleration_is_force =
offboard_control_sp.isForceSetpoint;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
if (_pos_sp_triplet_pub < 0) {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
&pos_sp_triplet);
} else {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
&pos_sp_triplet);
}
}
}
}
}
}
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
@@ -513,6 +704,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
{
mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
struct offboard_control_setpoint_s offboard_control_sp;
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_attitude_target.target_system ||
set_attitude_target.target_system == 0) &&
(mavlink_system.compid == set_attitude_target.target_component ||
set_attitude_target.target_component == 0)) {
for (int i = 0; i < 4; i++) {
offboard_control_sp.attitude[i] = set_attitude_target.q[i];
}
offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
/* set correct ignore flags for body rate fields: copy from mavlink message */
for (int i = 0; i < 3; i++) {
offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
}
/* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
/* set correct ignore flags for attitude field: copy from mavlink message */
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
offboard_control_sp.timestamp = hrt_absolute_time();
offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
if (_control_mode.flag_control_offboard_enabled) {
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
att_sp.q_d_valid = true;
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
}
}
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
///XXX add support for ignoring individual axes
if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_rates_setpoint_s rates_sp;
rates_sp.timestamp = hrt_absolute_time();
rates_sp.roll = set_attitude_target.body_roll_rate;
rates_sp.pitch = set_attitude_target.body_pitch_rate;
rates_sp.yaw = set_attitude_target.body_yaw_rate;
rates_sp.thrust = set_attitude_target.thrust;
if (_att_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
} else {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
}
}
}
}
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
@@ -534,6 +822,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
tstatus.system_id = msg->sysid;
tstatus.component_id = msg->compid;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
@@ -1106,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
struct sched_param param;
(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2900);
src/modules/mavlink/mavlink_receiver.h
+5 -1
View File
@@ -71,6 +71,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
#include "mavlink_ftp.h"
@@ -117,6 +118,8 @@ private:
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_set_attitude_target(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
@@ -145,10 +148,11 @@ private:
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _offboard_control_sp_pub;
orb_advert_t _local_pos_sp_pub;
orb_advert_t _global_vel_sp_pub;
orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub;
orb_advert_t _force_sp_pub;
orb_advert_t _pos_sp_triplet_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _telemetry_status_pub;
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
+12 -8
View File
@@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest()
}
/// @brief Called before every test to initialize the FTP Server.
void MavlinkFtpTest::init(void)
void MavlinkFtpTest::_init(void)
{
_ftp_server = new MavlinkFTP;;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
@@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void)
}
/// @brief Called after every test to take down the FTP Server.
void MavlinkFtpTest::cleanup(void)
void MavlinkFtpTest::_cleanup(void)
{
delete _ftp_server;
@@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *dir = "/foo"; // non-existent file
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void)
// Test case data files are created for specific boundary conditions
ut_compare("Test case data files are out of date", test->length, st.st_size);
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void)
}
/// @brief Runs all the unit tests
void MavlinkFtpTest::runTests(void)
bool MavlinkFtpTest::run_tests(void)
{
ut_run_test(_ack_test);
ut_run_test(_bad_opcode_test);
@@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void)
ut_run_test(_removedirectory_test);
ut_run_test(_createdirectory_test);
ut_run_test(_removefile_test);
return (_tests_failed == 0);
}
ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
+5 -4
View File
@@ -46,10 +46,7 @@ public:
MavlinkFtpTest();
virtual ~MavlinkFtpTest();
virtual void init(void);
virtual void cleanup(void);
virtual void runTests(void);
virtual bool run_tests(void);
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
@@ -65,6 +62,9 @@ public:
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
private:
virtual void _init(void);
virtual void _cleanup(void);
bool _ack_test(void);
bool _bad_opcode_test(void);
bool _bad_datasize_test(void);
@@ -105,3 +105,4 @@ private:
static const char _unittest_microsd_file[];
};
bool mavlink_ftp_test(void);
src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
+1 -6
View File
@@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
int mavlink_tests_main(int argc, char *argv[])
{
MavlinkFtpTest* test = new MavlinkFtpTest;
test->runTests();
test->printResults();
return 0;
return mavlink_ftp_test() ? 0 : -1;
}
src/modules/mavlink/mavlink_tests/module.mk
+2
View File
@@ -45,4 +45,6 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 5000
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST
src/modules/mc_pos_control/mc_pos_control_main.cpp
+213 -41
View File
@@ -76,6 +76,7 @@
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
#define MIN_DIST 0.01f
/**
* Multicopter position control app start / stop handling function
@@ -179,6 +180,7 @@ private:
bool _reset_pos_sp;
bool _reset_alt_sp;
bool _mode_auto;
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
@@ -219,6 +221,11 @@ private:
*/
void reset_alt_sp();
/**
* Check if position setpoint is too far from current position and adjust it if needed.
*/
void limit_pos_sp_offset();
/**
* Set position setpoint using manual control
*/
@@ -229,6 +236,14 @@ private:
*/
void control_offboard(float dt);
bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
/**
* Set position setpoint for AUTO
*/
void control_auto(float dt);
/**
* Select between barometric and global (AMSL) altitudes
*/
@@ -283,7 +298,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_ref_timestamp(0),
_reset_pos_sp(true),
_reset_alt_sp(true)
_reset_alt_sp(true),
_mode_auto(false)
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
@@ -533,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp()
}
}
void
MulticopterPositionControl::limit_pos_sp_offset()
{
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
}
void
MulticopterPositionControl::control_manual(float dt)
{
@@ -614,7 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_pos_sp(2) = _pos_sp_triplet.current.z;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
@@ -624,6 +662,11 @@ MulticopterPositionControl::control_offboard(float dt)
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
}
if (_pos_sp_triplet.current.yaw_valid) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
@@ -647,6 +690,170 @@ MulticopterPositionControl::control_offboard(float dt)
}
}
bool
MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
{
/* project center of sphere on line */
/* normalized AB */
math::Vector<3> ab_norm = line_b - line_a;
ab_norm.normalize();
math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
float cd_len = (sphere_c - d).length();
/* we have triangle CDX with known CD and CX = R, find DX */
if (sphere_r > cd_len) {
/* have two roots, select one in A->B direction from D */
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
res = d + ab_norm * dx_len;
return true;
} else {
/* have no roots, return D */
res = d;
return false;
}
}
void
MulticopterPositionControl::control_auto(float dt)
{
if (!_mode_auto) {
_mode_auto = true;
/* reset position setpoint on AUTO mode activation */
reset_pos_sp();
reset_alt_sp();
}
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
/* find X - cross point of L1 sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than L1 radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
if (_pos_sp_triplet.next.valid) {
math::Vector<3> next_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
&next_sp.data[0], &next_sp.data[1]);
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
if ((next_sp - curr_sp).length() > MIN_DIST) {
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* cos(b), b = angle pos - curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than L1 radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
}
}
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (near) {
/* L1 sphere crosses trajectory */
} else {
/* copter is too far from trajectory */
/* if copter is behind prev waypoint, go directly to prev waypoint */
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
pos_sp_s = prev_sp_s;
}
/* if copter is in front of curr waypoint, go directly to curr waypoint */
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
}
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
}
}
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
}
void
MulticopterPositionControl::task_main()
{
@@ -750,41 +957,16 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_pos_sp();
reset_alt_sp();
}
control_auto(dt);
}
/* fill local position setpoint */
@@ -846,16 +1028,6 @@ MulticopterPositionControl::task_main()
_vel_sp(2) = _params.land_speed;
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in non-manual modes */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
if (vel_sp_norm > 1.0f) {
_vel_sp /= vel_sp_norm;
}
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
@@ -1132,9 +1304,9 @@ MulticopterPositionControl::task_main()
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
src/modules/navigator/geofence.cpp
+59 -5
View File
@@ -48,6 +48,7 @@
#include <ctype.h>
#include <nuttx/config.h>
#include <unistd.h>
#include <mavlink/mavlink_log.h>
/* Oddly, ERROR is not defined for C++ */
@@ -62,7 +63,12 @@ Geofence::Geofence() :
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
param_geofence_on(this, "ON")
_param_geofence_on(this, "ON"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
_outside_counter(0),
_mavlinkFd(-1)
{
/* Load initial params */
updateParams();
@@ -74,23 +80,69 @@ Geofence::~Geofence()
}
bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
bool Geofence::inside(const struct vehicle_global_position_s &global_position)
{
return inside(vehicle->lat, vehicle->lon, vehicle->alt);
return inside(global_position.lat, global_position.lon, global_position.alt);
}
bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
{
return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
}
bool Geofence::inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
updateParams();
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(double)gps_position.alt * 1.0e-3);
}
} else {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position, baro_altitude_amsl);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
baro_altitude_amsl);
}
}
}
bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} {
_outside_counter++;
if(_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (param_geofence_on.get() != 1)
if (_param_geofence_on.get() != 1)
return true;
if (valid()) {
if (!isEmpty()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
if (altitude > _altitude_max || altitude < _altitude_min) {
return false;
}
/*Horizontal check */
/* Adaptation of algorithm originally presented as
@@ -280,8 +332,10 @@ Geofence::loadFromFile(const char *filename)
{
_verticesCount = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
} else {
warnx("Geofence: import error");
mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
}
return ERROR;
src/modules/navigator/geofence.h
+48 -15
View File
@@ -42,6 +42,9 @@
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_combined.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
@@ -49,30 +52,32 @@
class Geofence : public control::SuperBlock
{
private:
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt param_geofence_on;
public:
Geofence();
~Geofence();
/* Altitude mode, corresponding to the param GF_ALTMODE */
enum {
GF_ALT_MODE_WGS84 = 0,
GF_ALT_MODE_AMSL = 1
};
/* Source, corresponding to the param GF_SOURCE */
enum {
GF_SOURCE_GLOBALPOS = 0,
GF_SOURCE_GPS = 1
};
/**
* Return whether craft is inside geofence.
* Return whether system is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
* @return true: system is inside fence, false: system is outside fence
*/
bool inside(const struct vehicle_global_position_s *craft);
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl);
bool inside_polygon(double lat, double lon, float altitude);
int clearDm();
@@ -88,6 +93,34 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
int getAltitudeMode() { return _param_altitude_mode.get(); }
int getSource() { return _param_source.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt _param_geofence_on;
control::BlockParamInt _param_altitude_mode;
control::BlockParamInt _param_source;
control::BlockParamInt _param_counter_threshold;
uint8_t _outside_counter;
int _mavlinkFd;
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position);
bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
};
src/modules/navigator/geofence_params.c
+36
View File
@@ -58,3 +58,39 @@
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ON, 1);
/**
* Geofence altitude mode
*
* Select which altitude reference should be used
* 0 = WGS84, 1 = AMSL
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ALTMODE, 0);
/**
* Geofence source
*
* Select which position source should be used. Selecting GPS instead of global position makes sure that there is
* no dependence on the position estimator
* 0 = global position, 1 = GPS
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_SOURCE, 0);
/**
* Geofence counter limit
*
* Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered
*
* @min -1
* @max 10
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_COUNT, -1);
src/modules/navigator/mission.cpp
+75 -58
View File
@@ -37,6 +37,7 @@
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <sys/types.h>
@@ -62,18 +63,16 @@
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_param_takeoff_alt(this, "TAKEOFF_ALT"),
_param_dist_1wp(this, "DIST_1WP"),
_param_altmode(this, "ALTMODE"),
_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff(false),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false),
@@ -115,6 +114,7 @@ Mission::on_inactive()
update_offboard_mission();
}
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
@@ -149,8 +149,19 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
} else {
/* else just report that item reached */
if (_mission_type == MISSION_TYPE_OFFBOARD) {
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
set_mission_item_reached();
}
}
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
@@ -223,7 +234,7 @@ Mission::update_offboard_mission()
_current_offboard_mission_index = 0;
}
report_current_offboard_mission_item();
set_current_offboard_mission_item();
}
@@ -361,6 +372,10 @@ Mission::set_mission_items()
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
@@ -377,10 +392,11 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached();
report_mission_finished();
set_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
return;
@@ -417,7 +433,7 @@ Mission::set_mission_items()
takeoff_alt += _navigator->get_home_position()->alt;
}
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
@@ -425,32 +441,46 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
} else {
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
} else {
/* skip takeoff */
_takeoff = false;
}
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to read next mission item */
struct mission_item_s mission_item_next;
@@ -462,6 +492,10 @@ Mission::set_mission_items()
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
@@ -668,45 +702,28 @@ Mission::save_offboard_mission_state()
}
void
Mission::report_mission_item_reached()
Mission::set_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
Mission::set_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
save_offboard_mission_state();
}
void
Mission::report_mission_finished()
Mission::set_mission_finished()
{
_mission_result.finished = true;
publish_mission_result();
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
}
src/modules/navigator/mission.h
+9 -16
View File
@@ -37,6 +37,7 @@
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_MISSION_H
@@ -130,24 +131,19 @@ private:
void save_offboard_mission_state();
/**
* Report that a mission item has been reached
* Set a mission item as reached
*/
void report_mission_item_reached();
void set_mission_item_reached();
/**
* Rport the current mission item
* Set the current offboard mission item
*/
void report_current_offboard_mission_item();
void set_current_offboard_mission_item();
/**
* Report that the mission is finished if one exists or that none exists
* Set that the mission is finished if one exists or that none exists
*/
void report_mission_finished();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
void set_mission_finished();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
@@ -159,11 +155,8 @@ private:
int _current_onboard_mission_index;
int _current_offboard_mission_index;
bool _need_takeoff;
bool _takeoff;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
bool _takeoff; /**< takeoff state flag */
enum {
MISSION_TYPE_NONE,
src/modules/navigator/mission_feasibility_checker.cpp
+1 -1
View File
@@ -120,7 +120,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return false;
}
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
return false;
}
src/modules/navigator/module.mk
+9 -3
View File
@@ -46,13 +46,19 @@ SRCS = navigator_main.cpp \
loiter.cpp \
rtl.cpp \
rtl_params.c \
offboard.cpp \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c
geofence_params.c \
datalinkloss.cpp \
datalinkloss_params.c \
rcloss.cpp \
rcloss_params.c \
enginefailure.cpp \
gpsfailure.cpp \
gpsfailure_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/modules/navigator/navigator.h
+55 -8
View File
@@ -35,6 +35,7 @@
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_H
@@ -50,20 +51,25 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include "navigator_mode.h"
#include "mission.h"
#include "loiter.h"
#include "rtl.h"
#include "offboard.h"
#include "datalinkloss.h"
#include "enginefailure.h"
#include "gpsfailure.h"
#include "rcloss.h"
#include "geofence.h"
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 4
#define NAVIGATOR_MODE_ARRAY_SIZE 7
class Navigator : public control::SuperBlock
{
@@ -100,6 +106,17 @@ public:
*/
void load_fence_from_file(const char *filename);
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
* Example: mode that is triggered on gps failure
*/
void publish_att_sp();
/**
* Setters
*/
@@ -112,11 +129,15 @@ public:
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -131,25 +152,35 @@ private:
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _gps_pos_sub; /**< gps position subscription */
int _sensor_combined_sub; /**< sensor combined subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _offboard_control_sp_sub; /*** offboard control subscription */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
orb_advert_t _att_sp_pub; /**< publish att sp
used only in very special failsafe modes
when pos control is deactivated */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
vehicle_gps_position_s _gps_pos; /**< gps position */
sensor_combined_s _sensor_combined; /**< sensor values */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
mission_result_s _mission_result;
vehicle_attitude_setpoint_s _att_sp;
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -157,14 +188,18 @@ private:
Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
Offboard _offboard; /**< class that handles offboard */
RCLoss _rcLoss; /**< class that handles RTL according to
OBC rules (rc loss mode) */
DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
EngineFailure _engineFailure; /**< class that handles the engine failure mode
(FW only!) */
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
@@ -173,11 +208,23 @@ private:
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */
control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve gps position
*/
void gps_position_update();
/**
* Retrieve sensor values
*/
void sensor_combined_update();
/**
* Retrieve home position
*/
src/modules/navigator/navigator_main.cpp
+130 -16
View File
@@ -40,6 +40,7 @@
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -67,7 +68,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <drivers/drv_baro.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
@@ -85,6 +86,7 @@
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
#define GEOFENCE_CHECK_INTERVAL 200000
namespace navigator
{
@@ -98,43 +100,56 @@ Navigator::Navigator() :
_navigator_task(-1),
_mavlink_fd(-1),
_global_pos_sub(-1),
_gps_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_capabilities_sub(-1),
_offboard_control_sp_sub(-1),
_control_mode_sub(-1),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_att_sp_pub(-1),
_vstatus{},
_control_mode{},
_global_pos{},
_gps_pos{},
_sensor_combined{},
_home_pos{},
_mission_item{},
_nav_caps{},
_pos_sp_triplet{},
_mission_result{},
_att_sp{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_navigation_mode(nullptr),
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_offboard(this, "OFF"),
_rcLoss(this, "RCL"),
_dataLinkLoss(this, "DLL"),
_engineFailure(this, "EF"),
_gpsFailure(this, "GPSF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
_param_rcloss_obc(this, "RCL_OBC")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_offboard;
_navigation_mode_array[3] = &_dataLinkLoss;
_navigation_mode_array[4] = &_engineFailure;
_navigation_mode_array[5] = &_gpsFailure;
_navigation_mode_array[6] = &_rcLoss;
updateParams();
}
@@ -170,6 +185,18 @@ Navigator::global_position_update()
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
void
Navigator::gps_position_update()
{
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
void
Navigator::sensor_combined_update()
{
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
void
Navigator::home_position_update()
{
@@ -221,6 +248,7 @@ Navigator::task_main()
warnx("Initializing..");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
@@ -232,6 +260,7 @@ Navigator::task_main()
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
@@ -240,6 +269,8 @@ Navigator::task_main()
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -247,12 +278,13 @@ Navigator::task_main()
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
/* copy all topics first time */
vehicle_status_update();
vehicle_control_mode_update();
global_position_update();
gps_position_update();
sensor_combined_update();
home_position_update();
navigation_capabilities_update();
params_update();
@@ -264,7 +296,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[6];
struct pollfd fds[8];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
@@ -279,6 +311,10 @@ Navigator::task_main()
fds[4].events = POLLIN;
fds[5].fd = _param_update_sub;
fds[5].events = POLLIN;
fds[6].fd = _sensor_combined_sub;
fds[6].events = POLLIN;
fds[7].fd = _gps_pos_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@@ -303,6 +339,21 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static bool have_geofence_position_data = false;
/* gps updated */
if (fds[7].revents & POLLIN) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
}
/* sensors combined updated */
if (fds[6].revents & POLLIN) {
sensor_combined_update();
}
/* parameters updated */
if (fds[5].revents & POLLIN) {
params_update();
@@ -332,9 +383,21 @@ Navigator::task_main()
/* global position updated */
if (fds[0].revents & POLLIN) {
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
}
/* Check geofence violation */
if (!_geofence.inside(&_global_pos)) {
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
publish_mission_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -342,6 +405,9 @@ Navigator::task_main()
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_mission_result.geofence_violated = false;
publish_mission_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -355,6 +421,7 @@ Navigator::task_main()
case NAVIGATION_STATE_POSCTL:
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
case NAVIGATION_STATE_OFFBOARD:
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
@@ -364,14 +431,30 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_LOITER:
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
} else {
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
_navigation_mode = &_rtl;
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
if (_param_datalinkloss_obc.get() != 0) {
_navigation_mode = &_dataLinkLoss;
} else {
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_OFFBOARD:
_navigation_mode = &_offboard;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_navigation_mode = &_gpsFailure;
break;
default:
_navigation_mode = nullptr;
@@ -442,7 +525,7 @@ Navigator::status()
// warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
// }
if (_fence_valid) {
if (_geofence.valid()) {
warnx("Geofence is valid");
/* TODO: needed? */
// warnx("Vertex longitude latitude");
@@ -450,7 +533,7 @@ Navigator::status()
// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else {
warnx("Geofence not set");
warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
}
}
@@ -534,3 +617,34 @@ int navigator_main(int argc, char *argv[])
return 0;
}
void
Navigator::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.reached = false;
_mission_result.finished = false;
}
void
Navigator::publish_att_sp()
{
/* lazily publish the attitude sp only once available */
if (_att_sp_pub > 0) {
/* publish att sp*/
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
src/modules/navigator/navigator_mode.cpp
+4 -1
View File
@@ -43,7 +43,7 @@
#include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
SuperBlock(navigator, name),
_navigator(navigator),
_first_run(true)
{
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
if (_first_run) {
/* first run */
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
_navigator->publish_mission_result();
on_activation();
} else {
src/modules/navigator/navigator_params.c
+54
View File
@@ -37,6 +37,7 @@
* Parameters for navigator in general
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -64,3 +65,56 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
/**
* Set OBC mode for data link loss
*
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
/**
* Set OBC mode for rc loss
*
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
/**
* Airfield home Lat
*
* Latitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
/**
* Airfield home Lon
*
* Longitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
/**
* Airfield home alt
*
* Altitude of airfield home waypoint
*
* @unit m
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
src/modules/navigator/offboard.cpp
-129
View File
@@ -1,129 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file offboard.cpp
*
* Helper class for offboard commands
*
* @author Julian Oes <julian@oes.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator.h"
#include "offboard.h"
Offboard::Offboard(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_offboard_control_sp({0})
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
Offboard::~Offboard()
{
}
void
Offboard::on_inactive()
{
}
void
Offboard::on_activation()
{
}
void
Offboard::on_active()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
bool updated;
orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
if (updated) {
update_offboard_control_setpoint();
}
/* copy offboard setpoints to the corresponding topics */
if (_navigator->get_control_mode()->flag_control_position_enabled
&& _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
/* position control */
pos_sp_triplet->current.x = _offboard_control_sp.p1;
pos_sp_triplet->current.y = _offboard_control_sp.p2;
pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
pos_sp_triplet->current.z = -_offboard_control_sp.p4;
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->current.position_valid = true;
_navigator->set_position_setpoint_triplet_updated();
} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
&& _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
/* velocity control */
pos_sp_triplet->current.vx = _offboard_control_sp.p2;
pos_sp_triplet->current.vy = _offboard_control_sp.p1;
pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
pos_sp_triplet->current.vz = _offboard_control_sp.p4;
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
pos_sp_triplet->current.valid = true;
pos_sp_triplet->current.velocity_valid = true;
_navigator->set_position_setpoint_triplet_updated();
}
}
void
Offboard::update_offboard_control_setpoint()
{
orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
}
src/modules/navigator/rtl.cpp
+3 -3
View File
@@ -58,9 +58,9 @@
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY")
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false)
{
/* load initial params */
updateParams();
src/modules/px4iofirmware/controls.c
+61 -18
View File
@@ -43,6 +43,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
#include "px4io.h"
@@ -51,11 +52,62 @@
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
static int _dsm_fd;
static uint16_t rc_value_override = 0;
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
{
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
uint8_t n_bytes = 0;
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
/* get data from FD and attempt to parse with DSM and ST24 libs */
uint8_t st24_rssi, rx_count;
uint16_t st24_channel_count = 0;
*st24_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
if (*st24_updated) {
*rssi = st24_rssi;
r_raw_rc_count = st24_channel_count;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
return (*dsm_updated | *st24_updated);
}
void
controls_init(void)
{
@@ -65,7 +117,7 @@ controls_init(void)
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
_dsm_fd = dsm_init("/dev/ttyS0");
/* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
@@ -116,20 +168,8 @@ controls_tick() {
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
bool dsm_updated, st24_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@@ -191,7 +231,7 @@ controls_tick() {
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
@@ -278,6 +318,9 @@ controls_tick() {
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
/* pick out override channel, indicated by special mapping */
rc_value_override = SIGNED_TO_REG(scaled);
}
}
}
@@ -371,7 +414,7 @@ controls_tick() {
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
if (override) {
@@ -379,7 +422,7 @@ controls_tick() {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
if (dsm_updated || sbus_updated || ppm_updated || st24_updated)
mixer_tick();
} else {
src/modules/px4iofirmware/dsm.c
+8 -2
View File
@@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
*
* @param[out] values pointer to per channel array of decoded values
* @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data
* @param[out] n_butes number of bytes read
* @param[out] bytes pointer to the buffer of read bytes
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values)
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
{
ssize_t ret;
hrt_abstime now;
@@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values)
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
if (ret < 1) {
return false;
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
}
dsm_last_rx_time = now;
src/modules/px4iofirmware/mixer.cpp
+47 -34
View File
@@ -58,14 +58,7 @@ extern "C" {
/*
* Maximum interval in us before FMU signal is considered lost
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
#define OVERRIDE 4
#define FMU_INPUT_DROP_LIMIT_US 500000
/* current servo arm/disarm state */
static bool mixer_servos_armed = false;
@@ -98,7 +91,8 @@ mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
if ((system_state.fmu_data_received_time == 0) ||
hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -109,6 +103,9 @@ mixer_tick(void)
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
/* this flag is never cleared once OK */
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
}
/* default to failsafe mixing - it will be forced below if flag is set */
@@ -139,7 +136,9 @@ mixer_tick(void)
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
/* do not enter manual override if we asked for termination failsafe and FMU is lost */
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -154,6 +153,44 @@ mixer_tick(void)
}
}
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Check if failsafe termination is set - if yes,
* set the force failsafe flag once entering the first
* failsafe condition.
*/
if ( /* if we have requested flight termination style failsafe (noreturn) */
(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
/* and we ended up in a failsafe condition */
(source == MIX_FAILSAFE) &&
/* and we should be armed, so we intended to provide outputs */
should_arm &&
/* and FMU is initialized */
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
/*
* Check if we should force failsafe - and do it if we have to
*/
@@ -170,30 +207,6 @@ mixer_tick(void)
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Run the mixers.
*/
src/modules/px4iofirmware/module.mk
+2 -1
View File
@@ -14,7 +14,8 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
../systemlib/pwm_limit/pwm_limit.c
../systemlib/pwm_limit/pwm_limit.c \
../../lib/rc/st24.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c
src/modules/px4iofirmware/protocol.h
+6 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -111,6 +111,8 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -180,6 +182,7 @@
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
@@ -218,6 +221,7 @@ enum { /* DSM bind states */
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
/* autopilot control values, -10000..10000 */
@@ -243,6 +247,7 @@ enum { /* DSM bind states */
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
src/modules/px4iofirmware/px4io.c
+6
View File
@@ -124,6 +124,9 @@ heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
#ifdef GPIO_LED4
LED_RING(heartbeat);
#endif
}
static uint64_t reboot_time;
@@ -191,6 +194,9 @@ user_start(int argc, char *argv[])
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
src/modules/px4iofirmware/px4io.h
+2 -1
View File
@@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit;
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s))
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
@@ -215,7 +216,7 @@ extern uint16_t adc_measure(unsigned channel);
extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
src/modules/px4iofirmware/registers.c
+24 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
/*
* If the failsafe termination flag is set, do not allow the autopilot to unset it
*/
value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
/*
* If failsafe termination is enabled and force failsafe bit is set, do not allow
* the autopilot to clear it.
*/
if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) {
value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
}
r_setup_arming = value;
break;
@@ -589,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
dsm_bind(value & 0x0f, (value >> 4) & 0xF);
break;
case PX4IO_P_SETUP_FORCE_SAFETY_ON:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
}
break;
case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
if (value == PX4IO_FORCE_SAFETY_MAGIC) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
@@ -672,7 +692,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
src/modules/px4iofirmware/sbus.c
+70 -25
View File
@@ -57,11 +57,12 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
-+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV)
-+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks)
*/
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -87,13 +89,15 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
{
if (sbus_fd < 0)
if (sbus_fd < 0) {
sbus_fd = open(device, O_RDWR | O_NONBLOCK);
}
if (sbus_fd >= 0) {
struct termios t;
@@ -113,16 +117,49 @@ sbus_init(const char *device)
} else {
debug("S.Bus: open failed");
}
return sbus_fd;
}
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
char a = 'A';
write(sbus_fd, &a, 1);
}
uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
uint8_t offset = 0;
uint16_t value;
hrt_abstime now;
now = hrt_absolute_time();
if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
last_txframe_time = now;
uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
/* 16 is sbus number of servos/channels minus 2 single bit channels.
* currently ignoring single bit channels. */
for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
/*protect from out of bounds values and limit to 11 bits*/
if (value > 0x07ff ) {
value = 0x07ff;
}
while (offset >= 8) {
++byteindex;
offset -= 8;
}
oframe[byteindex] |= (value << (offset)) & 0xff;
oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
offset += 11;
}
write(sbus_fd, oframe, SBUS_FRAME_SIZE);
}
}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
@@ -167,8 +204,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count);
/* if the read failed for any reason, just give up here */
if (ret < 1)
if (ret < 1) {
return false;
}
last_rx_time = now;
@@ -180,8 +218,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
/*
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
if (partial_frame_count < SBUS_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -228,7 +267,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
@@ -237,23 +277,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
}
switch (frame[24]) {
case 0x00:
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
@@ -283,7 +327,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET;
}
/* decode switch channels if data fields are wide enough */
@@ -304,16 +348,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
/* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true;
*sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
} else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
src/modules/sdlog2/sdlog2.c
+36 -4
View File
@@ -90,6 +90,7 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/encoders.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -628,6 +629,9 @@ void sdlog2_start_log()
perf_print_all(perf_fd);
close(perf_fd);
/* reset performance counters to get in-flight min and max values in post flight log */
perf_reset_all();
logging_enabled = true;
}
@@ -951,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct servorail_status_s servorail_status;
struct satellite_info_s sat_info;
struct wind_estimate_s wind_estimate;
struct encoders_s encoders;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -993,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GS1B_s log_GS1B;
struct log_TECS_s log_TECS;
struct log_WIND_s log_WIND;
struct log_ENCD_s log_ENCD;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1030,12 +1036,12 @@ int sdlog2_thread_main(int argc, char *argv[])
int system_power_sub;
int servorail_status_sub;
int wind_sub;
int encoders_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1054,9 +1060,6 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
@@ -1065,6 +1068,25 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
/* add new topics HERE */
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
if (_extended_logging) {
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
}
/* close non-needed fd's */
/* close stdin */
close(0);
/* close stdout */
close(1);
thread_running = true;
@@ -1649,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(WIND);
}
/* --- ENCODERS --- */
if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
log_msg.msg_type = LOG_ENCD_MSG;
log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1];
log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1];
LOGBUFFER_WRITE_AND_COUNT(ENCD);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+19 -8
View File
@@ -241,15 +241,15 @@ struct log_GPSP_s {
#define LOG_ESC_MSG 18
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
uint16_t esc_voltage;
uint16_t esc_current;
uint16_t esc_rpm;
uint16_t esc_temperature;
float esc_voltage;
float esc_current;
int32_t esc_rpm;
float esc_temperature;
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
@@ -407,6 +407,16 @@ struct log_VISN_s {
float qw;
};
/* --- ENCODERS - ENCODER DATA --- */
#define LOG_ENCD_MSG 39
struct log_ENCD_s {
int64_t cnt0;
float vel0;
int64_t cnt1;
float vel1;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -452,7 +462,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
@@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
src/modules/systemlib/circuit_breaker.c
+41
View File
@@ -95,6 +95,47 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
*/
PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
/**
* Circuit breaker for flight termination
*
* Setting this parameter to 121212 will disable the flight termination action.
* --> The IO driver will not do flight termination if requested by the FMU
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 121212
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
/**
* Circuit breaker for engine failure detection
*
* Setting this parameter to 284953 will disable the engine failure detection.
* If the aircraft is in engine failure mode the enine failure flag will be
* set to healthy
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 284953
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
/**
* Circuit breaker for gps failure detection
*
* Setting this parameter to 240024 will disable the gps failure detection.
* If the aircraft is in gps failure mode the gps failure flag will be
* set to healthy
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 240024
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
src/modules/systemlib/circuit_breaker.h
+3
View File
@@ -53,6 +53,9 @@
#define CBRK_RATE_CTRL_KEY 140253
#define CBRK_IO_SAFETY_KEY 22027
#define CBRK_AIRSPD_CHK_KEY 162128
#define CBRK_FLIGHTTERM_KEY 121212
#define CBRK_ENGINEFAIL_KEY 284953
#define CBRK_GPSFAIL_KEY 240024
#include <stdbool.h>
src/modules/systemlib/mixer/mixer.h
+6
View File
@@ -130,6 +130,9 @@
#include <nuttx/config.h>
#include "drivers/drv_mixer.h"
#include <uORB/topics/multirotor_motor_limits.h>
#include "mixer_load.h"
/**
@@ -531,6 +534,9 @@ private:
float _yaw_scale;
float _idle_speed;
orb_advert_t _limits_pub;
multirotor_motor_limits_s _limits;
unsigned _rotor_count;
const Rotor *_rotors;
src/modules/systemlib/mixer/mixer_multirotor.cpp
+21 -1
View File
@@ -36,7 +36,8 @@
*
* Multi-rotor mixers.
*/
#include <uORB/uORB.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <nuttx/config.h>
#include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float min_out = 0.0f;
float max_out = 0.0f;
_limits.roll_pitch = false;
_limits.yaw = false;
_limits.throttle_upper = false;
_limits.throttle_lower = false;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
_limits.yaw = true;
}
/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
}
_limits.roll_pitch = true;
} else {
/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
_limits.throttle_upper = true;
} else {
scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* scale outputs to range _idle_speed..1, and do final limiting */
for (unsigned i = 0; i < _rotor_count; i++) {
if (outputs[i] < _idle_speed) {
_limits.throttle_lower = true;
}
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
/* publish/advertise motor limits if running on FMU */
if (_limits_pub > 0) {
orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
} else {
_limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
}
#endif
return _rotor_count;
}
src/modules/uORB/objects_common.cpp
+6
View File
@@ -192,12 +192,18 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
#include "topics/multirotor_motor_limits.h"
ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
#include "topics/telemetry_status.h"
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
#include "topics/test_motor.h"
ORB_DEFINE(test_motor, struct test_motor_s);
#include "topics/debug_key_value.h"
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
src/modules/uORB/topics/esc_status.h
+10 -9
View File
@@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE {
/**
* Electronic speed controller status.
* Unsupported float fields should be assigned NaN.
*/
struct esc_status_s {
/* use of a counter and timestamp recommended (but not necessary) */
@@ -89,17 +90,17 @@ struct esc_status_s {
enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */
struct {
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */
uint16_t esc_version; /**< Version of current ESC - if supported */
uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */
uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */
uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */
uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */
float esc_setpoint; /**< setpoint of current ESC */
uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */
float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */
float esc_current; /**< Current measured from current ESC [A] - if supported */
float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */
float esc_setpoint; /**< setpoint of current ESC */
uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */
uint16_t esc_state; /**< State of ESC - depend on Vendor */
uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
uint16_t esc_version; /**< Version of current ESC - if supported */
uint16_t esc_state; /**< State of ESC - depend on Vendor */
} esc[CONNECTED_ESC_MAX];
};
src/modules/uORB/topics/mission_result.h
+5 -2
View File
@@ -55,8 +55,11 @@ struct mission_result_s
{
unsigned seq_reached; /**< Sequence of the mission item which has been reached */
unsigned seq_current; /**< Sequence of the current mission item */
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
bool geofence_violated; /**< true if the geofence is violated */
bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */
};
/**
src/modules/uORB/topics/offboard_control_setpoint.h
+192 -9
View File
@@ -53,11 +53,42 @@ enum OFFBOARD_CONTROL_MODE {
OFFBOARD_CONTROL_MODE_DIRECT = 0,
OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
};
enum OFFBOARD_CONTROL_FRAME {
OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
OFFBOARD_CONTROL_FRAME_GLOBAL = 4
};
/* mappings for the ignore bitmask */
enum {OFB_IGN_BIT_POS_X,
OFB_IGN_BIT_POS_Y,
OFB_IGN_BIT_POS_Z,
OFB_IGN_BIT_VEL_X,
OFB_IGN_BIT_VEL_Y,
OFB_IGN_BIT_VEL_Z,
OFB_IGN_BIT_ACC_X,
OFB_IGN_BIT_ACC_Y,
OFB_IGN_BIT_ACC_Z,
OFB_IGN_BIT_BODYRATE_X,
OFB_IGN_BIT_BODYRATE_Y,
OFB_IGN_BIT_BODYRATE_Z,
OFB_IGN_BIT_ATT,
OFB_IGN_BIT_THRUST,
OFB_IGN_BIT_YAW,
OFB_IGN_BIT_YAWRATE,
};
/**
@@ -70,10 +101,21 @@ struct offboard_control_setpoint_s {
enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */
float p1; /**< ailerons roll / roll rate input */
float p2; /**< elevator / pitch / pitch rate */
float p3; /**< rudder / yaw rate / yaw */
float p4; /**< throttle / collective thrust / altitude */
double position[3]; /**< lat, lon, alt / x, y, z */
float velocity[3]; /**< x vel, y vel, z vel */
float acceleration[3]; /**< x acc, y acc, z acc */
float attitude[4]; /**< attitude of vehicle (quaternion) */
float attitude_rate[3]; /**< body angular rates (x, y, z) */
float thrust; /**< thrust */
float yaw; /**< yaw: this is the yaw from the position_target message
(not from the full attitude_target message) */
float yaw_rate; /**< yaw rate: this is the yaw from the position_target message
(not from the full attitude_target message) */
uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
for mapping */
bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
float override_mode_switch;
@@ -87,6 +129,147 @@ struct offboard_control_setpoint_s {
* @}
*/
/**
* Returns true if the position setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index)));
}
/**
* Returns true if all position setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) {
return false;
}
}
return true;
}
/**
* Returns true if some position setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_position(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the velocity setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
}
/**
* Returns true if all velocity setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) {
return false;
}
}
return true;
}
/**
* Returns true if some velocity setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the acceleration setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
}
/**
* Returns true if all acceleration setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) {
return false;
}
}
return true;
}
/**
* Returns true if some acceleration setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the bodyrate setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
}
/**
* Returns true if some of the bodyrate setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the attitude setpoint should be ignored
*/
inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
}
/**
* Returns true if the thrust setpoint should be ignored
*/
inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
}
/**
* Returns true if the yaw setpoint should be ignored
*/
inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
}
/**
* Returns true if the yaw rate setpoint should be ignored
*/
inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
}
/* register this as object request broker structure */
ORB_DECLARE(offboard_control_setpoint);
src/modules/uORB/topics/position_setpoint_triplet.h
+7
View File
@@ -79,10 +79,17 @@ struct position_setpoint_s
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
bool yaw_valid; /**< true if yaw setpoint valid */
float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */
bool yawspeed_valid; /**< true if yawspeed setpoint valid */
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
float a_x; //**< acceleration x setpoint */
float a_y; //**< acceleration y setpoint */
float a_z; //**< acceleration z setpoint */
bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */
bool acceleration_is_force; //*< interprete acceleration as force */
};
/**
src/modules/uORB/topics/rc_channels.h
+7 -3
View File
@@ -34,6 +34,8 @@
/**
* @file rc_channels.h
* Definition of the rc_channels uORB topic.
*
* @deprecated DO NOT USE FOR NEW CODE
*/
#ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
AUX_2,
AUX_3,
AUX_4,
AUX_5,
RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
AUX_5
};
// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
#define RC_CHANNELS_FUNCTION_MAX 18
/**
* @addtogroup topics
* @{
@@ -76,7 +81,6 @@ struct rc_channels_s {
uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */
float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */
uint8_t channel_count; /**< Number of valid channels */
char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */
int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */
uint8_t rssi; /**< Receive signal strength index */
bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */
src/modules/uORB/topics/telemetry_status.h
+2
View File
@@ -67,6 +67,8 @@ struct telemetry_status_s {
uint8_t noise; /**< background noise level */
uint8_t remote_noise; /**< remote background noise level */
uint8_t txbuf; /**< how full the tx buffer is as a percentage */
uint8_t system_id; /**< system id of the remote system */
uint8_t component_id; /**< component id of the remote system */
};
/**
src/modules/navigator/offboard.h → src/modules/uORB/topics/test_motor.h
+28 -30
View File
@@ -1,6 +1,6 @@
/***************************************************************************
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -30,43 +30,41 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file offboard.h
* @file test_motor.h
*
* Helper class for offboard commands
* Test a single drive motor while the vehicle is disarmed
*
* Publishing values to this topic causes a single motor to spin, e.g. for
* ground test purposes. This topic will be ignored while the vehicle is armed.
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_OFFBOARD_H
#define NAVIGATOR_OFFBOARD_H
#ifndef TOPIC_TEST_MOTOR_H
#define TOPIC_TEST_MOTOR_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <stdint.h>
#include "../uORB.h"
#include <uORB/uORB.h>
#include <uORB/topics/offboard_control_setpoint.h>
#define NUM_MOTOR_OUTPUTS 8
#include "navigator_mode.h"
/**
* @addtogroup topics
* @{
*/
class Navigator;
class Offboard : public NavigatorMode
{
public:
Offboard(Navigator *navigator, const char *name);
~Offboard();
virtual void on_inactive();
virtual void on_activation();
virtual void on_active();
private:
void update_offboard_control_setpoint();
struct offboard_control_setpoint_s _offboard_control_sp;
struct test_motor_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
unsigned motor_number; /**< number of motor to spin */
float value; /**< output data, in natural output units */
};
/**
* @}
*/
/* actuator output sets; this list can be expanded as more drivers emerge */
ORB_DECLARE(test_motor);
#endif
src/modules/uORB/topics/vehicle_command.h
+3
View File
@@ -65,6 +65,9 @@ enum VEHICLE_CMD {
VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */
VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */
VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */
VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */
VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */
src/modules/uORB/topics/vehicle_control_mode.h
+1
View File
@@ -79,6 +79,7 @@ struct vehicle_control_mode_s {
bool flag_control_offboard_enabled; /**< true if offboard control should be used */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_force_enabled; /**< true if force control is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
src/modules/uORB/topics/vehicle_gps_position.h
+1 -1
View File
@@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
uint64_t timestamp_variance;
float s_variance_m_s; /**< speed accuracy estimate m/s */
float c_variance_rad; /**< course accuracy estimate rad */
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
float eph; /**< GPS HDOP horizontal dilution of position in m */
float epv; /**< GPS VDOP horizontal dilution of position in m */
src/modules/uORB/topics/vehicle_status.h
+17 -1
View File
@@ -104,7 +104,10 @@ typedef enum {
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
@@ -200,14 +203,25 @@ struct vehicle_status_s {
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool data_link_lost; /**< datalink to GCS lost */
bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */
uint8_t data_link_lost_counter; /**< counts unique data link lost events */
bool engine_failure; /** Set to true if an engine failure is detected */
bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */
bool gps_failure; /** Set to true if a gps failure is detected */
bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */
bool barometer_failure; /** Set to true if a barometer failure is detected */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
and should not be overridden by RC */
/* see SYS_STATUS mavlink message for the following */
uint32_t onboard_control_sensors_present;
@@ -229,6 +243,8 @@ struct vehicle_status_s {
bool circuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
bool circuit_breaker_engaged_enginefailure_check;
bool circuit_breaker_engaged_gpsfailure_check;
};
/**
src/modules/uavcan/actuators/esc.cpp
+32 -12
View File
@@ -73,7 +73,7 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -93,25 +93,24 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
*/
uavcan::equipment::esc::RawCommand msg;
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
if (_armed) {
for (unsigned i = 0; i < num_outputs; i++) {
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
}
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
perf_count(_perfcnt_scaling_error);
} else {
; // Correct value
}
msg.cmd.push_back(static_cast<unsigned>(scaled));
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
}
}
@@ -129,10 +128,31 @@ void UavcanEscController::arm_esc(bool arm)
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
if (msg.esc_index < CONNECTED_ESC_MAX) {
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
auto &ref = _esc_status.esc[msg.esc_index];
ref.esc_address = msg.getSrcNodeID().get();
ref.esc_voltage = msg.voltage;
ref.esc_current = msg.current;
ref.esc_temperature = msg.temperature;
ref.esc_setpoint = msg.power_rating_pct;
ref.esc_rpm = msg.rpm;
ref.esc_errorcount = msg.error_count;
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
{
// TODO publish to ORB
_esc_status.counter += 1;
_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub > 0) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
}
src/modules/uavcan/actuators/esc.hpp
+8 -9
View File
@@ -48,6 +48,8 @@
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <systemlib/perf_counter.h>
#include <uORB/topics/esc_status.h>
class UavcanEscController
{
@@ -73,9 +75,8 @@ private:
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
@@ -84,6 +85,10 @@ private:
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};
orb_advert_t _esc_status_pub = -1;
/*
* libuavcan related things
*/
@@ -93,12 +98,6 @@ private:
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
bool _armed = false;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters
*/
src/modules/unit_test/unit_test.cpp
+15 -9
View File
@@ -36,7 +36,11 @@
#include <systemlib/err.h>
UnitTest::UnitTest()
UnitTest::UnitTest() :
_tests_run(0),
_tests_failed(0),
_tests_passed(0),
_assertions(0)
{
}
@@ -44,20 +48,22 @@ UnitTest::~UnitTest()
{
}
void UnitTest::printResults(void)
void UnitTest::print_results(void)
{
warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
warnx(" Tests passed : %d", mu_tests_passed());
warnx(" Tests failed : %d", mu_tests_failed());
warnx(" Assertions : %d", mu_assertion());
warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
warnx(" Tests passed : %d", _tests_passed);
warnx(" Tests failed : %d", _tests_failed);
warnx(" Assertions : %d", _assertions);
}
void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
/// @brief Used internally to the ut_assert macro to print assert failures.
void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line)
{
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
}
void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
/// @brief Used internally to the ut_compare macro to print assert failures.
void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
{
warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
}
src/modules/unit_test/unit_test.h
+71 -54
View File
@@ -37,68 +37,85 @@
#include <systemlib/err.h>
/// @brief Base class to be used for unit tests.
class __EXPORT UnitTest
{
public:
#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
INLINE_GLOBAL(int, mu_tests_run)
INLINE_GLOBAL(int, mu_tests_failed)
INLINE_GLOBAL(int, mu_tests_passed)
INLINE_GLOBAL(int, mu_assertion)
INLINE_GLOBAL(int, mu_line)
INLINE_GLOBAL(const char*, mu_last_test)
UnitTest();
virtual ~UnitTest();
virtual ~UnitTest();
virtual void init(void) { };
virtual void cleanup(void) { };
virtual void runTests(void) = 0;
void printResults(void);
void printAssert(const char* msg, const char* test, const char* file, int line);
void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
#define ut_assert(message, test) \
do { \
if (!(test)) { \
printAssert(message, #test, __FILE__, __LINE__); \
return false; \
} else { \
mu_assertion()++; \
} \
} while (0)
#define ut_compare(message, v1, v2) \
do { \
int _v1 = v1; \
int _v2 = v2; \
if (_v1 != _v2) { \
printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
return false; \
} else { \
mu_assertion()++; \
} \
/// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro.
/// @return true: all unit tests succeeded, false: one or more unit tests failed
virtual bool run_tests(void) = 0;
/// @brief Prints results from running of unit tests.
void print_results(void);
/// @brief Macro to create a function which will run a unit test class and print results.
#define ut_declare_test(test_function, test_class) \
bool test_function(void) \
{ \
test_class* test = new test_class(); \
bool success = test->run_tests(); \
test->print_results(); \
return success; \
}
protected:
/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit
/// test should return true if it succeeded, false for fail.
#define ut_run_test(test) \
do { \
warnx("RUNNING TEST: %s", #test); \
_tests_run++; \
_init(); \
if (!test()) { \
warnx("TEST FAILED: %s", #test); \
_tests_failed++; \
} else { \
warnx("TEST PASSED: %s", #test); \
_tests_passed++; \
} \
_cleanup(); \
} while (0)
#define ut_run_test(test) \
do { \
warnx("RUNNING TEST: %s", #test); \
mu_tests_run()++; \
init(); \
if (!test()) { \
warnx("TEST FAILED: %s", #test); \
mu_tests_failed()++; \
} else { \
warnx("TEST PASSED: %s", #test); \
mu_tests_passed()++; \
} \
cleanup(); \
} while (0)
/// @brief Used to assert a value within a unit test.
#define ut_assert(message, test) \
do { \
if (!(test)) { \
_print_assert(message, #test, __FILE__, __LINE__); \
return false; \
} else { \
_assertions++; \
} \
} while (0)
/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
/// since it will give you better error reporting of the actual values being compared.
#define ut_compare(message, v1, v2) \
do { \
int _v1 = v1; \
int _v2 = v2; \
if (_v1 != _v2) { \
_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
return false; \
} else { \
_assertions++; \
} \
} while (0)
virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior.
virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior.
void _print_assert(const char* msg, const char* test, const char* file, int line);
void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
int _tests_run; ///< The number of individual unit tests run
int _tests_failed; ///< The number of unit tests which failed
int _tests_passed; ///< The number of unit tests which passed
int _assertions; ///< Total number of assertions tested by all unit tests
};
#endif /* UNIT_TEST_H_ */