mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-26 11:30:35 +08:00
Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages
Conflicts: src/modules/navigator/navigator.h src/modules/navigator/navigator_main.cpp
This commit is contained in:
Thomas Gubler
@@ -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
|
||||
|
||||
@@ -555,10 +553,35 @@ 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;
|
||||
}
|
||||
@@ -637,6 +660,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
|
||||
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
|
||||
case VEHICLE_CMD_PREFLIGHT_STORAGE:
|
||||
case VEHICLE_CMD_CUSTOM_0:
|
||||
case VEHICLE_CMD_CUSTOM_1:
|
||||
case VEHICLE_CMD_CUSTOM_2:
|
||||
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
|
||||
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
|
||||
/* ignore commands that handled in low prio loop */
|
||||
break;
|
||||
|
||||
@@ -676,6 +704,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");
|
||||
@@ -763,6 +797,8 @@ 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);
|
||||
@@ -868,11 +904,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;
|
||||
}
|
||||
|
||||
@@ -958,6 +996,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;
|
||||
@@ -1005,8 +1052,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;
|
||||
|
||||
@@ -1018,6 +1071,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
param_get(_param_takeoff_alt, &takeoff_alt);
|
||||
param_get(_param_enable_parachute, ¶chute_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);
|
||||
@@ -1058,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
if (mavlink_fd &&
|
||||
telemetry_last_heartbeat[i] == 0 &&
|
||||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
@@ -1071,6 +1130,25 @@ 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);
|
||||
@@ -1157,8 +1235,6 @@ 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 */
|
||||
|
||||
/* update home position */
|
||||
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
|
||||
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
|
||||
@@ -1370,15 +1446,51 @@ int commander_thread_main(int argc, char *argv[])
|
||||
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 */
|
||||
orb_check(mission_result_sub, &updated);
|
||||
|
||||
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;
|
||||
@@ -1489,15 +1601,25 @@ 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;
|
||||
@@ -1516,10 +1638,40 @@ 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(×tamp_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);
|
||||
|
||||
@@ -1533,6 +1685,53 @@ 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 */
|
||||
@@ -1574,7 +1773,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) {
|
||||
@@ -2024,6 +2224,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;
|
||||
@@ -2035,6 +2236,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;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
@@ -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_ */
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -58,6 +58,7 @@
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#ifdef SENSOR_COMBINED_SUB
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#endif
|
||||
@@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
|
||||
|
||||
__EXPORT uint32_t millis();
|
||||
|
||||
__EXPORT uint64_t getMicros();
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
|
||||
uint32_t millis()
|
||||
@@ -104,6 +108,11 @@ uint32_t millis()
|
||||
return IMUmsec;
|
||||
}
|
||||
|
||||
uint64_t getMicros()
|
||||
{
|
||||
return IMUusec;
|
||||
}
|
||||
|
||||
class FixedwingEstimator
|
||||
{
|
||||
public:
|
||||
@@ -171,6 +180,7 @@ private:
|
||||
#else
|
||||
int _sensor_combined_sub;
|
||||
#endif
|
||||
int _distance_sub; /**< distance measurement */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _baro_sub; /**< barometer subscription */
|
||||
int _gps_sub; /**< GPS subscription */
|
||||
@@ -196,7 +206,8 @@ private:
|
||||
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
|
||||
struct vehicle_gps_position_s _gps; /**< GPS position */
|
||||
struct wind_estimate_s _wind; /**< Wind estimate */
|
||||
struct wind_estimate_s _wind; /**< wind estimate */
|
||||
struct range_finder_report _distance; /**< distance estimate */
|
||||
|
||||
struct gyro_scale _gyro_offsets;
|
||||
struct accel_scale _accel_offsets;
|
||||
@@ -226,6 +237,7 @@ private:
|
||||
hrt_abstime _filter_start_time;
|
||||
hrt_abstime _last_sensor_timestamp;
|
||||
hrt_abstime _last_run;
|
||||
hrt_abstime _distance_last_valid;
|
||||
bool _gyro_valid;
|
||||
bool _accel_valid;
|
||||
bool _mag_valid;
|
||||
@@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
#else
|
||||
_sensor_combined_sub(-1),
|
||||
#endif
|
||||
_distance_sub(-1),
|
||||
_airspeed_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_gps_sub(-1),
|
||||
@@ -399,6 +412,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_filter_start_time(0),
|
||||
_last_sensor_timestamp(0),
|
||||
_last_run(0),
|
||||
_distance_last_valid(0),
|
||||
_gyro_valid(false),
|
||||
_accel_valid(false),
|
||||
_mag_valid(false),
|
||||
@@ -549,6 +563,7 @@ FixedwingEstimator::parameters_update()
|
||||
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
|
||||
_ekf->accelProcessNoise = _parameters.acc_pnoise;
|
||||
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
|
||||
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
|
||||
}
|
||||
|
||||
return OK;
|
||||
@@ -704,6 +719,7 @@ FixedwingEstimator::task_main()
|
||||
/*
|
||||
* do subscriptions
|
||||
*/
|
||||
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
@@ -753,6 +769,7 @@ FixedwingEstimator::task_main()
|
||||
bool newHgtData = false;
|
||||
bool newAdsData = false;
|
||||
bool newDataMag = false;
|
||||
bool newRangeData = false;
|
||||
|
||||
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
|
||||
|
||||
@@ -850,7 +867,8 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
_last_sensor_timestamp = _gyro.timestamp;
|
||||
IMUmsec = _gyro.timestamp / 1e3f;
|
||||
IMUmsec = _gyro.timestamp / 1e3;
|
||||
IMUusec = _gyro.timestamp;
|
||||
|
||||
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
|
||||
_last_run = _gyro.timestamp;
|
||||
@@ -914,7 +932,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
// Copy gyro and accel
|
||||
_last_sensor_timestamp = _sensor_combined.timestamp;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3f;
|
||||
IMUmsec = _sensor_combined.timestamp / 1e3;
|
||||
IMUusec = _sensor_combined.timestamp;
|
||||
|
||||
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
|
||||
|
||||
@@ -994,8 +1013,6 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (gps_updated) {
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
|
||||
perf_count(_perf_gps);
|
||||
|
||||
@@ -1008,11 +1025,17 @@ FixedwingEstimator::task_main()
|
||||
_gps_start_time = hrt_absolute_time();
|
||||
|
||||
/* check if we had a GPS outage for a long time */
|
||||
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
|
||||
float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
|
||||
|
||||
const float pos_reset_threshold = 5.0f; // seconds
|
||||
|
||||
/* timeout of 5 seconds */
|
||||
if (gps_elapsed > pos_reset_threshold) {
|
||||
_ekf->ResetPosition();
|
||||
_ekf->ResetVelocity();
|
||||
_ekf->ResetStoredStates();
|
||||
}
|
||||
_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
|
||||
|
||||
/* fuse GPS updates */
|
||||
|
||||
@@ -1044,6 +1067,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
newDataGps = true;
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1052,8 +1077,15 @@ FixedwingEstimator::task_main()
|
||||
orb_check(_baro_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
hrt_abstime baro_last = _baro.timestamp;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
|
||||
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
|
||||
if (!_baro_init) {
|
||||
@@ -1114,6 +1146,19 @@ FixedwingEstimator::task_main()
|
||||
newDataMag = false;
|
||||
}
|
||||
|
||||
orb_check(_distance_sub, &newRangeData);
|
||||
|
||||
if (newRangeData) {
|
||||
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
|
||||
|
||||
if (_distance.valid) {
|
||||
_ekf->rngMea = _distance.distance;
|
||||
_distance_last_valid = _distance.timestamp;
|
||||
} else {
|
||||
newRangeData = false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
|
||||
*/
|
||||
@@ -1197,6 +1242,7 @@ FixedwingEstimator::task_main()
|
||||
} else if (_ekf->statesInitialised) {
|
||||
|
||||
// We're apparently initialized in this case now
|
||||
// check (and reset the filter as needed)
|
||||
int check = check_filter_state();
|
||||
|
||||
if (check) {
|
||||
@@ -1206,21 +1252,7 @@ FixedwingEstimator::task_main()
|
||||
|
||||
// Run the strapdown INS equations every IMU update
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
||||
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
|
||||
|
||||
quat2eul(eulerEst, tempQuat);
|
||||
|
||||
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
|
||||
|
||||
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
|
||||
|
||||
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
|
||||
|
||||
#endif
|
||||
// store the predicted states for subsequent use by measurement fusion
|
||||
_ekf->StoreStates(IMUmsec);
|
||||
// Check if on ground - status is used by covariance prediction
|
||||
@@ -1334,6 +1366,13 @@ FixedwingEstimator::task_main()
|
||||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
|
||||
_ekf->GroundEKF();
|
||||
}
|
||||
|
||||
|
||||
// Output results
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
@@ -1447,6 +1486,10 @@ FixedwingEstimator::task_main()
|
||||
_global_pos.vel_d = _local_pos.vz;
|
||||
}
|
||||
|
||||
/* terrain altitude */
|
||||
_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
|
||||
_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
|
||||
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
|
||||
|
||||
_global_pos.yaw = _local_pos.yaw;
|
||||
|
||||
@@ -1467,8 +1510,10 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
_wind.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
_wind.covariance_east = _ekf->P[15][15];
|
||||
|
||||
|
||||
@@ -2,6 +2,11 @@
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <math.h>
|
||||
|
||||
#ifndef M_PI_F
|
||||
#define M_PI_F ((float)M_PI)
|
||||
#endif
|
||||
|
||||
#define EKF_COVARIANCE_DIVERGED 1.0e8f
|
||||
|
||||
@@ -38,13 +43,14 @@ AttPosEKF::AttPosEKF() :
|
||||
resetStates{},
|
||||
storedStates{},
|
||||
statetimeStamp{},
|
||||
lastVelPosFusion(millis()),
|
||||
statesAtVelTime{},
|
||||
statesAtPosTime{},
|
||||
statesAtHgtTime{},
|
||||
statesAtMagMeasTime{},
|
||||
statesAtVtasMeasTime{},
|
||||
statesAtRngTime{},
|
||||
statesAtOptFlowTime{},
|
||||
statesAtFlowTime{},
|
||||
correctedDelAng(),
|
||||
correctedDelVel(),
|
||||
summedDelAng(),
|
||||
@@ -59,7 +65,16 @@ AttPosEKF::AttPosEKF() :
|
||||
accel(),
|
||||
dVelIMU(),
|
||||
dAngIMU(),
|
||||
dtIMU(0),
|
||||
dtIMU(0.005f),
|
||||
dtIMUfilt(0.005f),
|
||||
dtVelPos(0.01f),
|
||||
dtVelPosFilt(0.01f),
|
||||
dtHgtFilt(0.01f),
|
||||
dtGpsFilt(0.1f),
|
||||
windSpdFiltNorth(0.0f),
|
||||
windSpdFiltEast(0.0f),
|
||||
windSpdFiltAltitude(0.0f),
|
||||
windSpdFiltClimb(0.0f),
|
||||
fusionModeGPS(0),
|
||||
innovVelPos{},
|
||||
varInnovVelPos{},
|
||||
@@ -103,13 +118,13 @@ AttPosEKF::AttPosEKF() :
|
||||
|
||||
inhibitWindStates(true),
|
||||
inhibitMagStates(true),
|
||||
inhibitGndHgtState(true),
|
||||
inhibitGndState(true),
|
||||
|
||||
onGround(true),
|
||||
staticMode(true),
|
||||
useAirspeed(true),
|
||||
useCompass(true),
|
||||
useRangeFinder(false),
|
||||
useRangeFinder(true),
|
||||
useOpticalFlow(false),
|
||||
|
||||
ekfDiverged(false),
|
||||
@@ -117,7 +132,24 @@ AttPosEKF::AttPosEKF() :
|
||||
current_ekf_state{},
|
||||
last_ekf_error{},
|
||||
numericalProtection(true),
|
||||
storeIndex(0)
|
||||
storeIndex(0),
|
||||
storedOmega{},
|
||||
Popt{},
|
||||
flowStates{},
|
||||
prevPosN(0.0f),
|
||||
prevPosE(0.0f),
|
||||
auxFlowObsInnov{},
|
||||
auxFlowObsInnovVar{},
|
||||
fScaleFactorVar(0.0f),
|
||||
Tnb_flow{},
|
||||
R_LOS(0.0f),
|
||||
auxFlowTestRatio{},
|
||||
auxRngTestRatio(0.0f),
|
||||
flowInnovGate(0.0f),
|
||||
auxFlowInnovGate(0.0f),
|
||||
rngInnovGate(0.0f),
|
||||
minFlowRng(0.0f),
|
||||
moCompR_LOS(0.0f)
|
||||
{
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
memset(¤t_ekf_state, 0, sizeof(current_ekf_state));
|
||||
@@ -260,6 +292,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
|
||||
|
||||
// Constrain states (to protect against filter divergence)
|
||||
ConstrainStates();
|
||||
|
||||
// update filtered IMU time step length
|
||||
dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
|
||||
}
|
||||
|
||||
void AttPosEKF::CovariancePrediction(float dt)
|
||||
@@ -312,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
} else {
|
||||
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
|
||||
} else {
|
||||
processNoise[22] = 0;
|
||||
@@ -962,6 +997,21 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtGpsFilt(float dt)
|
||||
{
|
||||
dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtHgtFilt(float dt)
|
||||
{
|
||||
dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::updateDtVelPosFilt(float dt)
|
||||
{
|
||||
dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
|
||||
}
|
||||
|
||||
void AttPosEKF::FuseVelposNED()
|
||||
{
|
||||
|
||||
@@ -998,6 +1048,18 @@ void AttPosEKF::FuseVelposNED()
|
||||
// associated with sequential fusion
|
||||
if (fuseVelData || fusePosData || fuseHgtData)
|
||||
{
|
||||
uint64_t tNow = getMicros();
|
||||
updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
|
||||
lastVelPosFusion = tNow;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
|
||||
|
||||
// scaler according to the number of repetitions of the
|
||||
// same measurement in one fusion step
|
||||
float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
|
||||
|
||||
// set the GPS data timeout depending on whether airspeed data is present
|
||||
if (useAirspeed) horizRetryTime = gpsRetryTime;
|
||||
else horizRetryTime = gpsRetryTimeNoTAS;
|
||||
@@ -1010,12 +1072,12 @@ void AttPosEKF::FuseVelposNED()
|
||||
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
|
||||
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
|
||||
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
|
||||
R_OBS[0] = sq(vneSigma) + sq(velErr);
|
||||
R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
|
||||
R_OBS[1] = R_OBS[0];
|
||||
R_OBS[2] = sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
|
||||
R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
|
||||
R_OBS[4] = R_OBS[3];
|
||||
R_OBS[5] = sq(posDSigma) + sq(posErr);
|
||||
R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
|
||||
|
||||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||||
if (fuseVelData)
|
||||
@@ -1173,7 +1235,7 @@ void AttPosEKF::FuseVelposNED()
|
||||
}
|
||||
}
|
||||
// Don't update terrain state if inhibited
|
||||
if (inhibitGndHgtState) {
|
||||
if (inhibitGndState) {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
|
||||
@@ -1356,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
if (!inhibitGndState) {
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
@@ -1430,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[1] = 1.0f/SK_MY[0];
|
||||
innovMag[1] = MagPred[1] - magData.y;
|
||||
}
|
||||
@@ -1505,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[2] = 1.0f/SK_MZ[0];
|
||||
innovMag[2] = MagPred[2] - magData.z;
|
||||
|
||||
@@ -1616,6 +1668,32 @@ void AttPosEKF::FuseAirspeed()
|
||||
// Perform fusion of True Airspeed measurement
|
||||
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
|
||||
{
|
||||
|
||||
float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
|
||||
|
||||
if (isfinite(windSpdFiltClimb)) {
|
||||
windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
|
||||
} else {
|
||||
windSpdFiltClimb = states[6];
|
||||
}
|
||||
|
||||
if (altDiff < 20.0f) {
|
||||
// Lowpass the output of the wind estimate - we want a long-term
|
||||
// stable estimate, but not start to load into the overall dynamics
|
||||
// of the system (which adjusting covariances would do)
|
||||
|
||||
// Change filter coefficient based on altitude change rate
|
||||
float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
|
||||
|
||||
windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
|
||||
windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
|
||||
} else {
|
||||
windSpdFiltNorth = vwn;
|
||||
windSpdFiltEast = vwe;
|
||||
}
|
||||
|
||||
windSpdFiltAltitude = hgtMea;
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
|
||||
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
|
||||
@@ -1675,11 +1753,6 @@ void AttPosEKF::FuseAirspeed()
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovVtas = 1.0f/SK_TAS;
|
||||
|
||||
// Calculate the measurement innovation
|
||||
@@ -1811,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder()
|
||||
rngPred = (ptd - pd)/cosRngTilt;
|
||||
innovRng = rngPred - rngMea;
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovRng*innovRng*SK_RNG[0]) < 25)
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
states[22] = states[22] - Kfusion[22] * innovRng;
|
||||
@@ -1827,285 +1903,387 @@ void AttPosEKF::FuseRangeFinder()
|
||||
|
||||
void AttPosEKF::FuseOptFlow()
|
||||
{
|
||||
static uint8_t obsIndex;
|
||||
static float SH_LOS[13];
|
||||
static float SKK_LOS[15];
|
||||
static float SK_LOS[2];
|
||||
static float q0 = 0.0f;
|
||||
static float q1 = 0.0f;
|
||||
static float q2 = 0.0f;
|
||||
static float q3 = 1.0f;
|
||||
static float vn = 0.0f;
|
||||
static float ve = 0.0f;
|
||||
static float vd = 0.0f;
|
||||
static float pd = 0.0f;
|
||||
static float ptd = 0.0f;
|
||||
static float R_LOS = 0.01f;
|
||||
static float losPred[2];
|
||||
// static uint8_t obsIndex;
|
||||
// static float SH_LOS[13];
|
||||
// static float SKK_LOS[15];
|
||||
// static float SK_LOS[2];
|
||||
// static float q0 = 0.0f;
|
||||
// static float q1 = 0.0f;
|
||||
// static float q2 = 0.0f;
|
||||
// static float q3 = 1.0f;
|
||||
// static float vn = 0.0f;
|
||||
// static float ve = 0.0f;
|
||||
// static float vd = 0.0f;
|
||||
// static float pd = 0.0f;
|
||||
// static float ptd = 0.0f;
|
||||
// static float R_LOS = 0.01f;
|
||||
// static float losPred[2];
|
||||
|
||||
// Transformation matrix from nav to body axes
|
||||
Mat3f Tnb_local;
|
||||
// Transformation matrix from body to sensor axes
|
||||
// assume camera is aligned with Z body axis plus a misalignment
|
||||
// defined by 3 small angles about X, Y and Z body axis
|
||||
Mat3f Tbs;
|
||||
Tbs.x.y = a3;
|
||||
Tbs.y.x = -a3;
|
||||
Tbs.x.z = -a2;
|
||||
Tbs.z.x = a2;
|
||||
Tbs.y.z = a1;
|
||||
Tbs.z.y = -a1;
|
||||
// Transformation matrix from navigation to sensor axes
|
||||
Mat3f Tns;
|
||||
float H_LOS[n_states];
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_LOS[i] = 0.0f;
|
||||
// // Transformation matrix from nav to body axes
|
||||
// Mat3f Tnb_local;
|
||||
// // Transformation matrix from body to sensor axes
|
||||
// // assume camera is aligned with Z body axis plus a misalignment
|
||||
// // defined by 3 small angles about X, Y and Z body axis
|
||||
// Mat3f Tbs;
|
||||
// Tbs.x.y = a3;
|
||||
// Tbs.y.x = -a3;
|
||||
// Tbs.x.z = -a2;
|
||||
// Tbs.z.x = a2;
|
||||
// Tbs.y.z = a1;
|
||||
// Tbs.z.y = -a1;
|
||||
// // Transformation matrix from navigation to sensor axes
|
||||
// Mat3f Tns;
|
||||
// float H_LOS[n_states];
|
||||
// for (uint8_t i = 0; i < n_states; i++) {
|
||||
// H_LOS[i] = 0.0f;
|
||||
// }
|
||||
// Vector3f velNED_local;
|
||||
// Vector3f relVelSensor;
|
||||
|
||||
// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
// {
|
||||
// // Sequential fusion of XY components to spread processing load across
|
||||
// // two prediction time steps.
|
||||
|
||||
// // Calculate observation jacobians and Kalman gains
|
||||
// if (fuseOptFlowData)
|
||||
// {
|
||||
// // Copy required states to local variable names
|
||||
// q0 = statesAtOptFlowTime[0];
|
||||
// q1 = statesAtOptFlowTime[1];
|
||||
// q2 = statesAtOptFlowTime[2];
|
||||
// q3 = statesAtOptFlowTime[3];
|
||||
// vn = statesAtOptFlowTime[4];
|
||||
// ve = statesAtOptFlowTime[5];
|
||||
// vd = statesAtOptFlowTime[6];
|
||||
// pd = statesAtOptFlowTime[9];
|
||||
// ptd = statesAtOptFlowTime[22];
|
||||
// velNED_local.x = vn;
|
||||
// velNED_local.y = ve;
|
||||
// velNED_local.z = vd;
|
||||
|
||||
// // calculate rotation from NED to body axes
|
||||
// float q00 = sq(q0);
|
||||
// float q11 = sq(q1);
|
||||
// float q22 = sq(q2);
|
||||
// float q33 = sq(q3);
|
||||
// float q01 = q0 * q1;
|
||||
// float q02 = q0 * q2;
|
||||
// float q03 = q0 * q3;
|
||||
// float q12 = q1 * q2;
|
||||
// float q13 = q1 * q3;
|
||||
// float q23 = q2 * q3;
|
||||
// Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
// Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
// Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
// Tnb_local.y.x = 2*(q12 - q03);
|
||||
// Tnb_local.z.x = 2*(q13 + q02);
|
||||
// Tnb_local.x.y = 2*(q12 + q03);
|
||||
// Tnb_local.z.y = 2*(q23 - q01);
|
||||
// Tnb_local.x.z = 2*(q13 - q02);
|
||||
// Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// // calculate transformation from NED to sensor axes
|
||||
// Tns = Tbs*Tnb_local;
|
||||
|
||||
// // calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// // calculate relative velocity in sensor frame
|
||||
// relVelSensor = Tns*velNED_local;
|
||||
|
||||
// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
// losPred[0] = relVelSensor.y/range;
|
||||
// losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// // Calculate observation jacobians
|
||||
// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// SH_LOS[3] = 1/(pd - ptd);
|
||||
// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
// SH_LOS[7] = 1/sq(pd - ptd);
|
||||
// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gain
|
||||
// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
// SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
// innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
|
||||
// obsIndex = 1;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
// else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
// {
|
||||
// // Calculate observation jacobians
|
||||
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// // Calculate Kalman gains
|
||||
// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
// varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
// innovOptFlow[1] = losPred[1] - losData[1];
|
||||
|
||||
// // reset the observation index
|
||||
// obsIndex = 0;
|
||||
// fuseOptFlowData = false;
|
||||
// }
|
||||
|
||||
// // Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// {
|
||||
// // correct the state vector
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
// }
|
||||
// // normalise the quaternion states
|
||||
// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
// if (quatMag > 1e-12f)
|
||||
// {
|
||||
// for (uint8_t j= 0; j<=3; j++)
|
||||
// {
|
||||
// float quatMagInv = 1.0f/quatMag;
|
||||
// states[j] = states[j] * quatMagInv;
|
||||
// }
|
||||
// }
|
||||
// // correct the covariance P = (I - K*H)*P
|
||||
// // take advantage of the empty columns in KH to reduce the
|
||||
// // number of operations
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j <= 6; j++)
|
||||
// {
|
||||
// KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
// }
|
||||
// for (uint8_t j = 7; j <= 8; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
// for (uint8_t j = 10; j <= 21; j++)
|
||||
// {
|
||||
// KH[i][j] = 0.0f;
|
||||
// }
|
||||
// KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// KHP[i][j] = 0.0f;
|
||||
// for (uint8_t k = 0; k <= 6; k++)
|
||||
// {
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
// }
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// for (uint8_t i = 0; i < n_states; i++)
|
||||
// {
|
||||
// for (uint8_t j = 0; j < n_states; j++)
|
||||
// {
|
||||
// P[i][j] = P[i][j] - KHP[i][j];
|
||||
// }
|
||||
// }
|
||||
// ForceSymmetry();
|
||||
// ConstrainVariances();
|
||||
// }
|
||||
}
|
||||
|
||||
/*
|
||||
Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
|
||||
This fiter requires optical flow rates that are not motion compensated
|
||||
Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
|
||||
*/
|
||||
void AttPosEKF::GroundEKF()
|
||||
{
|
||||
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
|
||||
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
|
||||
if (!inhibitGndState) {
|
||||
float distanceTravelledSq;
|
||||
distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
|
||||
prevPosN = statesAtRngTime[7];
|
||||
prevPosE = statesAtRngTime[8];
|
||||
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
|
||||
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
|
||||
}
|
||||
Vector3f velNED_local;
|
||||
Vector3f relVelSensor;
|
||||
// we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
|
||||
Popt[0][0] = 0.0f;
|
||||
Popt[0][1] = 0.0f;
|
||||
Popt[1][0] = 0.0f;
|
||||
|
||||
// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
{
|
||||
// Sequential fusion of XY components to spread processing load across
|
||||
// two prediction time steps.
|
||||
// Fuse range finder data
|
||||
// Need to check that our range finder tilt angle is less than 30 degrees
|
||||
float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
|
||||
if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
|
||||
float range; // range from camera to centre of image
|
||||
float q0; // quaternion at optical flow measurement time
|
||||
float q1; // quaternion at optical flow measurement time
|
||||
float q2; // quaternion at optical flow measurement time
|
||||
float q3; // quaternion at optical flow measurement time
|
||||
float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseOptFlowData)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtOptFlowTime[0];
|
||||
q1 = statesAtOptFlowTime[1];
|
||||
q2 = statesAtOptFlowTime[2];
|
||||
q3 = statesAtOptFlowTime[3];
|
||||
vn = statesAtOptFlowTime[4];
|
||||
ve = statesAtOptFlowTime[5];
|
||||
vd = statesAtOptFlowTime[6];
|
||||
pd = statesAtOptFlowTime[9];
|
||||
ptd = statesAtOptFlowTime[22];
|
||||
velNED_local.x = vn;
|
||||
velNED_local.y = ve;
|
||||
velNED_local.z = vd;
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtRngTime[0];
|
||||
q1 = statesAtRngTime[1];
|
||||
q2 = statesAtRngTime[2];
|
||||
q3 = statesAtRngTime[3];
|
||||
|
||||
// calculate rotation from NED to body axes
|
||||
float q00 = sq(q0);
|
||||
float q11 = sq(q1);
|
||||
float q22 = sq(q2);
|
||||
float q33 = sq(q3);
|
||||
float q01 = q0 * q1;
|
||||
float q02 = q0 * q2;
|
||||
float q03 = q0 * q3;
|
||||
float q12 = q1 * q2;
|
||||
float q13 = q1 * q3;
|
||||
float q23 = q2 * q3;
|
||||
Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
Tnb_local.y.x = 2*(q12 - q03);
|
||||
Tnb_local.z.x = 2*(q13 + q02);
|
||||
Tnb_local.x.y = 2*(q12 + q03);
|
||||
Tnb_local.z.y = 2*(q23 - q01);
|
||||
Tnb_local.x.z = 2*(q13 - q02);
|
||||
Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// calculate transformation from NED to sensor axes
|
||||
Tns = Tbs*Tnb_local;
|
||||
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tns*velNED_local;
|
||||
|
||||
// divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
losPred[0] = relVelSensor.y/range;
|
||||
losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
//printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
//printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
//printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
SH_LOS[7] = 1/sq(pd - ptd);
|
||||
SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gain
|
||||
SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseOptFlowData = false;
|
||||
// calculate Kalman gains
|
||||
float SK_RNG[3];
|
||||
SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
|
||||
SK_RNG[2] = 1/SK_RNG[0];
|
||||
float K_RNG[2];
|
||||
if (!inhibitScaleState) {
|
||||
K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[0] = 0.0f;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// Calculate observation jacobians
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gains
|
||||
SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
innovOptFlow[1] = losPred[1] - losData[1];
|
||||
if (!inhibitGndState) {
|
||||
K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
|
||||
} else {
|
||||
K_RNG[1] = 0.0f;
|
||||
}
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = 1.0f/SK_RNG[1];
|
||||
|
||||
// constrain terrain height to be below the vehicle
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// estimate range to centre of image
|
||||
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
|
||||
|
||||
// Calculate the measurement innovation
|
||||
innovRng = range - rngMea;
|
||||
|
||||
// calculate the innovation consistency test ratio
|
||||
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if out of bounds
|
||||
if (auxRngTestRatio < 1.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
}
|
||||
// normalise the quaternion states
|
||||
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
if (quatMag > 1e-12f)
|
||||
{
|
||||
for (uint8_t j= 0; j<=3; j++)
|
||||
{
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
states[j] = states[j] * quatMagInv;
|
||||
}
|
||||
}
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 6; j++)
|
||||
{
|
||||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
}
|
||||
for (uint8_t j = 7; j <= 8; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
for (uint8_t j = 10; j <= 21; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 6; k++)
|
||||
{
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
// correct the state
|
||||
for (uint8_t i = 0; i < 2 ; i++) {
|
||||
flowStates[i] -= K_RNG[i] * innovRng;
|
||||
}
|
||||
// constrain the states
|
||||
|
||||
// constrain focal length to 0.1 to 10 mm
|
||||
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
|
||||
// constrain altitude
|
||||
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
|
||||
|
||||
// correct the covariance matrix
|
||||
float nextPopt[2][2];
|
||||
nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
|
||||
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
|
||||
// prevent the state variances from becoming negative and maintain symmetry
|
||||
Popt[0][0] = maxf(nextPopt[0][0],0.0f);
|
||||
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
|
||||
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
|
||||
Popt[1][0] = Popt[0][1];
|
||||
}
|
||||
}
|
||||
obsIndex = obsIndex + 1;
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
@@ -2126,6 +2304,24 @@ float AttPosEKF::sq(float valIn)
|
||||
return valIn*valIn;
|
||||
}
|
||||
|
||||
float AttPosEKF::maxf(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 >= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
float AttPosEKF::min(float valIn1, float valIn2)
|
||||
{
|
||||
if (valIn1 <= valIn2) {
|
||||
return valIn1;
|
||||
} else {
|
||||
return valIn2;
|
||||
}
|
||||
}
|
||||
|
||||
// Store states in a history array along with time stamp
|
||||
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
|
||||
{
|
||||
@@ -2322,9 +2518,9 @@ void AttPosEKF::OnGroundCheck()
|
||||
}
|
||||
// don't update terrain offset state if on ground
|
||||
if (onGround) {
|
||||
inhibitGndHgtState = true;
|
||||
inhibitGndState = true;
|
||||
} else {
|
||||
inhibitGndHgtState = false;
|
||||
inhibitGndState = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3006,9 +3202,14 @@ void AttPosEKF::ZeroVariables()
|
||||
{
|
||||
|
||||
// Initialize on-init initialized variables
|
||||
|
||||
dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
|
||||
dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
|
||||
dtGpsFilt = 1.0f / 5.0f;
|
||||
dtHgtFilt = 1.0f / 100.0f;
|
||||
storeIndex = 0;
|
||||
|
||||
lastVelPosFusion = millis();
|
||||
|
||||
// Do the data structure init
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
@@ -3028,6 +3229,13 @@ void AttPosEKF::ZeroVariables()
|
||||
dVelIMU.zero();
|
||||
lastGyroOffset.zero();
|
||||
|
||||
windSpdFiltNorth = 0.0f;
|
||||
windSpdFiltEast = 0.0f;
|
||||
// setting the altitude to zero will give us a higher
|
||||
// gain to adjust faster in the first step
|
||||
windSpdFiltAltitude = 0.0f;
|
||||
windSpdFiltClimb = 0.0f;
|
||||
|
||||
for (unsigned i = 0; i < data_buffer_size; i++) {
|
||||
|
||||
for (unsigned j = 0; j < n_states; j++) {
|
||||
|
||||
@@ -80,6 +80,14 @@ public:
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
@@ -116,13 +124,16 @@ public:
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
// Times
|
||||
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
@@ -140,7 +151,16 @@ public:
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
|
||||
float dtIMUfilt; // average time between IMU measurements (sec)
|
||||
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
@@ -192,7 +212,8 @@ public:
|
||||
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
@@ -211,6 +232,30 @@ public:
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
// Optical Flow error estimation
|
||||
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
|
||||
|
||||
// Two state EKF used to estimate focal length scale factor and terrain position
|
||||
float Popt[2][2]; // state covariance matrix
|
||||
float flowStates[2]; // flow states [scale factor, terrain position]
|
||||
float prevPosN; // north position at last measurement
|
||||
float prevPosE; // east position at last measurement
|
||||
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
|
||||
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
|
||||
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
|
||||
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
|
||||
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
|
||||
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
|
||||
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
|
||||
float flowInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
float rngInnovGate; // number of standard deviations used for the innovation consistency check
|
||||
float minFlowRng; // minimum range over which to fuse optical flow measurements
|
||||
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
void updateDtGpsFilt(float dt);
|
||||
|
||||
void updateDtHgtFilt(float dt);
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
@@ -226,6 +271,8 @@ void FuseRangeFinder();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
@@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
static float maxf(float valIn1, float valIn2);
|
||||
|
||||
static float min(float valIn1, float valIn2);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
@@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
void updateDtVelPosFilt(float dt);
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
bool GyroOffsetsDiverged();
|
||||
@@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
uint64_t getMicros();
|
||||
|
||||
|
||||
@@ -63,6 +63,7 @@
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
@@ -124,6 +125,7 @@ private:
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_sub; /**< notification of manual control updates */
|
||||
int _global_pos_sub; /**< global position subscription */
|
||||
int _vehicle_status_sub; /**< vehicle status subscription */
|
||||
|
||||
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
|
||||
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
|
||||
@@ -139,6 +141,7 @@ private:
|
||||
struct actuator_controls_s _actuators; /**< actuator control inputs */
|
||||
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
|
||||
struct vehicle_global_position_s _global_pos; /**< global position */
|
||||
struct vehicle_status_s _vehicle_status; /**< vehicle status */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
|
||||
@@ -275,6 +278,11 @@ private:
|
||||
*/
|
||||
void global_pos_poll();
|
||||
|
||||
/**
|
||||
* Check for vehicle status updates.
|
||||
*/
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
@@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
_params_sub(-1),
|
||||
_manual_sub(-1),
|
||||
_global_pos_sub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
|
||||
/* publications */
|
||||
_rate_sp_pub(-1),
|
||||
@@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
_actuators = {};
|
||||
_actuators_airframe = {};
|
||||
_global_pos = {};
|
||||
_vehicle_status = {};
|
||||
|
||||
|
||||
_parameter_handles.tconst = param_find("FW_ATT_TC");
|
||||
@@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
bool vehicle_status_updated;
|
||||
orb_check(_vehicle_status_sub, &vehicle_status_updated);
|
||||
|
||||
if (vehicle_status_updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
@@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main()
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vcontrol_mode_sub, 200);
|
||||
@@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main()
|
||||
vehicle_accel_poll();
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
vehicle_status_poll();
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[2];
|
||||
@@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main()
|
||||
|
||||
global_pos_poll();
|
||||
|
||||
vehicle_status_poll();
|
||||
|
||||
/* lock integrator until control is started */
|
||||
bool lock_integrator;
|
||||
|
||||
@@ -720,7 +746,14 @@ 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 + _parameters.pitchsp_offset_rad;
|
||||
@@ -779,6 +812,13 @@ FixedwingAttitudeControl::task_main()
|
||||
}
|
||||
}
|
||||
|
||||
/* If the aircraft is on ground reset the integrators */
|
||||
if (_vehicle_status.condition_landed) {
|
||||
_roll_ctrl.reset_integrator();
|
||||
_pitch_ctrl.reset_integrator();
|
||||
_yaw_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
/* Prepare speed_body_u and speed_body_w */
|
||||
float speed_body_u = 0.0f;
|
||||
float speed_body_v = 0.0f;
|
||||
@@ -853,8 +893,12 @@ 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);
|
||||
|
||||
@@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */
|
||||
*/
|
||||
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
|
||||
|
||||
using namespace launchdetection;
|
||||
|
||||
class FixedwingPositionControl
|
||||
{
|
||||
public:
|
||||
@@ -139,7 +141,8 @@ 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 */
|
||||
@@ -154,7 +157,8 @@ 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 */
|
||||
@@ -171,8 +175,7 @@ private:
|
||||
bool land_onslope;
|
||||
|
||||
/* takeoff/launch states */
|
||||
bool launch_detected;
|
||||
bool usePreTakeoffThrust;
|
||||
LaunchDetectionResult launch_detection_state;
|
||||
|
||||
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
|
||||
|
||||
@@ -210,6 +213,7 @@ private:
|
||||
float max_climb_rate;
|
||||
float climbout_diff;
|
||||
float heightrate_p;
|
||||
float heightrate_ff;
|
||||
float speedrate_p;
|
||||
float throttle_damp;
|
||||
float integrator_gain;
|
||||
@@ -255,6 +259,7 @@ private:
|
||||
param_t max_climb_rate;
|
||||
param_t climbout_diff;
|
||||
param_t heightrate_p;
|
||||
param_t heightrate_ff;
|
||||
param_t speedrate_p;
|
||||
param_t throttle_damp;
|
||||
param_t integrator_gain;
|
||||
@@ -301,10 +306,15 @@ 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.
|
||||
*/
|
||||
@@ -380,7 +390,8 @@ private:
|
||||
bool climbout_mode, float climbout_pitch_min_rad,
|
||||
float altitude,
|
||||
const math::Vector<3> &ground_speed,
|
||||
tecs_mode mode = TECS_MODE_NORMAL);
|
||||
tecs_mode mode = TECS_MODE_NORMAL,
|
||||
bool pitch_max_special = false);
|
||||
|
||||
};
|
||||
|
||||
@@ -408,6 +419,7 @@ 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),
|
||||
@@ -425,6 +437,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_manual(),
|
||||
_airspeed(),
|
||||
_control_mode(),
|
||||
_vehicle_status(),
|
||||
_global_pos(),
|
||||
_pos_sp_triplet(),
|
||||
_sensor_combined(),
|
||||
@@ -438,8 +451,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
land_stayonground(false),
|
||||
land_motor_lim(false),
|
||||
land_onslope(false),
|
||||
launch_detected(false),
|
||||
usePreTakeoffThrust(false),
|
||||
launch_detection_state(LAUNCHDETECTION_RES_NONE),
|
||||
last_manual(false),
|
||||
landingslope(),
|
||||
flare_curve_alt_rel_last(0.0f),
|
||||
@@ -494,6 +506,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
|
||||
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
|
||||
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
|
||||
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
|
||||
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
@@ -563,12 +576,19 @@ FixedwingPositionControl::parameters_update()
|
||||
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
||||
|
||||
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
|
||||
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
|
||||
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
|
||||
|
||||
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
|
||||
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
|
||||
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
|
||||
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
|
||||
|
||||
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
|
||||
if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
|
||||
_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
|
||||
}
|
||||
|
||||
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));
|
||||
@@ -594,6 +614,7 @@ FixedwingPositionControl::parameters_update()
|
||||
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
|
||||
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
|
||||
_tecs.set_heightrate_p(_parameters.heightrate_p);
|
||||
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
|
||||
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
||||
|
||||
/* sanity check parameters */
|
||||
@@ -627,16 +648,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()
|
||||
{
|
||||
@@ -823,7 +855,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
|
||||
* 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 || rel_alt_estimated > 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;
|
||||
}
|
||||
|
||||
@@ -1076,41 +1108,46 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
|
||||
|
||||
/* Perform launch detection */
|
||||
if(!launch_detected) { //do not do further checks once a launch was detected
|
||||
if (launchDetector.launchDetectionEnabled()) {
|
||||
static hrt_abstime last_sent = 0;
|
||||
if(hrt_absolute_time() - last_sent > 4e6) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
|
||||
last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* Tell the attitude controller to stop integrating while we are waiting
|
||||
* for the launch */
|
||||
_att_sp.roll_reset_integral = true;
|
||||
_att_sp.pitch_reset_integral = true;
|
||||
_att_sp.yaw_reset_integral = true;
|
||||
|
||||
/* Detect launch */
|
||||
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
|
||||
if (launchDetector.getLaunchDetected()) {
|
||||
launch_detected = true;
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
|
||||
}
|
||||
} else {
|
||||
/* no takeoff detection --> fly */
|
||||
launch_detected = true;
|
||||
warnx("launchdetection off");
|
||||
if (launchDetector.launchDetectionEnabled() &&
|
||||
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
|
||||
/* Inform user that launchdetection is running */
|
||||
static hrt_abstime last_sent = 0;
|
||||
if(hrt_absolute_time() - last_sent > 4e6) {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
|
||||
last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* Detect launch */
|
||||
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
|
||||
|
||||
/* update our copy of the laucn detection state */
|
||||
launch_detection_state = launchDetector.getLaunchDetected();
|
||||
} else {
|
||||
/* no takeoff detection --> fly */
|
||||
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
|
||||
}
|
||||
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
/* Set control values depending on the detection state */
|
||||
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
|
||||
/* Launch has been detected, hence we have to control the plane. */
|
||||
|
||||
if (launch_detected) {
|
||||
usePreTakeoffThrust = false;
|
||||
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
_att_sp.yaw_body = _l1_control.nav_bearing();
|
||||
|
||||
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
|
||||
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
|
||||
* full throttle, otherwise we use the preTakeOff Throttle */
|
||||
float takeoff_throttle = launch_detection_state !=
|
||||
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
|
||||
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
|
||||
|
||||
/* select maximum pitch: the launchdetector may impose another limit for the pitch
|
||||
* depending on the state of the launch */
|
||||
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
|
||||
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
|
||||
|
||||
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
|
||||
* meters */
|
||||
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
|
||||
|
||||
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
||||
@@ -1118,18 +1155,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
|
||||
eas2tas,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
math::radians(_parameters.pitch_limit_max),
|
||||
_parameters.throttle_min, _parameters.throttle_max,
|
||||
takeoff_pitch_max_rad,
|
||||
_parameters.throttle_min, takeoff_throttle,
|
||||
_parameters.throttle_cruise,
|
||||
true,
|
||||
math::max(math::radians(pos_sp_triplet.current.pitch_min),
|
||||
math::radians(10.0f)),
|
||||
_global_pos.alt,
|
||||
ground_speed,
|
||||
TECS_MODE_TAKEOFF);
|
||||
TECS_MODE_TAKEOFF,
|
||||
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
|
||||
|
||||
/* limit roll motion to ensure enough lift */
|
||||
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
|
||||
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
|
||||
math::radians(15.0f));
|
||||
|
||||
} else {
|
||||
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
|
||||
@@ -1138,17 +1177,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
math::radians(_parameters.pitch_limit_max),
|
||||
_parameters.throttle_min,
|
||||
_parameters.throttle_max,
|
||||
takeoff_throttle,
|
||||
_parameters.throttle_cruise,
|
||||
false,
|
||||
math::radians(_parameters.pitch_limit_min),
|
||||
_global_pos.alt,
|
||||
ground_speed);
|
||||
}
|
||||
|
||||
} else {
|
||||
usePreTakeoffThrust = true;
|
||||
/* Tell the attitude controller to stop integrating while we are waiting
|
||||
* for the launch */
|
||||
_att_sp.roll_reset_integral = true;
|
||||
_att_sp.pitch_reset_integral = true;
|
||||
_att_sp.yaw_reset_integral = true;
|
||||
|
||||
/* Set default roll and pitch setpoints during detection phase */
|
||||
_att_sp.roll_body = 0.0f;
|
||||
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
|
||||
math::radians(10.0f));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* reset landing state */
|
||||
@@ -1182,13 +1230,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
}
|
||||
}
|
||||
|
||||
if (usePreTakeoffThrust) {
|
||||
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) {
|
||||
/* Copy thrust and pitch values from tecs
|
||||
* 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 &&
|
||||
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
|
||||
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
|
||||
}
|
||||
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
last_manual = false;
|
||||
@@ -1212,13 +1274,16 @@ 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);
|
||||
|
||||
@@ -1257,9 +1322,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 */
|
||||
@@ -1342,8 +1410,7 @@ FixedwingPositionControl::task_main()
|
||||
|
||||
void FixedwingPositionControl::reset_takeoff_state()
|
||||
{
|
||||
launch_detected = false;
|
||||
usePreTakeoffThrust = false;
|
||||
launch_detection_state = LAUNCHDETECTION_RES_NONE;
|
||||
launchDetector.reset();
|
||||
}
|
||||
|
||||
@@ -1362,7 +1429,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
||||
bool climbout_mode, float climbout_pitch_min_rad,
|
||||
float altitude,
|
||||
const math::Vector<3> &ground_speed,
|
||||
tecs_mode mode)
|
||||
tecs_mode mode, bool pitch_max_special)
|
||||
{
|
||||
if (_mTecs.getEnabled()) {
|
||||
/* Using mtecs library: prepare arguments for mtecs call */
|
||||
@@ -1372,14 +1439,36 @@ 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();
|
||||
}
|
||||
|
||||
if (pitch_max_special) {
|
||||
/* Use the maximum pitch from the argument */
|
||||
limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
|
||||
} else {
|
||||
/* use pitch max set by MT param */
|
||||
limitOverride.disablePitchMaxOverride();
|
||||
}
|
||||
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
|
||||
limitOverride);
|
||||
} else {
|
||||
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 */
|
||||
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
|
||||
_airspeed.indicated_airspeed_m_s, eas2tas,
|
||||
|
||||
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
|
||||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For overpowered aircraft, this should be reduced to a value that
|
||||
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
|
||||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* For electric aircraft this will normally be set to zero, but can be set
|
||||
* to a small non-zero value if a folding prop is fitted to prevent the
|
||||
* prop from folding and unfolding repeatedly in-flight or to provide
|
||||
* some aerodynamic drag from a turning prop to improve the descent rate.
|
||||
*
|
||||
* For aircraft with internal combustion engine this parameter should be set
|
||||
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
|
||||
/**
|
||||
* Throttle limit value before flare
|
||||
*
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* This throttle value will be set as throttle limit at FW_LND_TLALT,
|
||||
* before arcraft will flare.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
|
||||
/**
|
||||
* Maximum climb rate
|
||||
*
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* This is the best climb rate that the aircraft can achieve with
|
||||
* the throttle set to THR_MAX and the airspeed set to the
|
||||
* default value. For electric aircraft make sure this number can be
|
||||
* achieved towards the end of flight when the battery voltage has reduced.
|
||||
* The setting of this parameter can be checked by commanding a positive
|
||||
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
|
||||
* required to climb is close to THR_MAX and the aircraft is maintaining
|
||||
* airspeed, then this parameter is set correctly. If the airspeed starts
|
||||
* to reduce, then the parameter is set to high, and if the throttle
|
||||
* demand required to climb and maintain speed is noticeably less than
|
||||
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
|
||||
* FW_THR_MAX reduced.
|
||||
*
|
||||
* @group L1 Control
|
||||
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
|
||||
/**
|
||||
* Minimum descent rate
|
||||
*
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* This is the sink rate of the aircraft with the throttle
|
||||
* set to THR_MIN and flown at the same airspeed as used
|
||||
* to measure FW_T_CLMB_MAX.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
|
||||
/**
|
||||
* Maximum descent rate
|
||||
*
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* This sets the maximum descent rate that the controller will use.
|
||||
* If this value is too large, the aircraft can over-speed on descent.
|
||||
* This should be set to a value that can be achieved without
|
||||
* exceeding the lower pitch angle limit and without over-speeding
|
||||
* the aircraft.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
|
||||
/**
|
||||
* TECS time constant
|
||||
*
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* This is the time constant of the TECS control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
|
||||
/**
|
||||
* TECS Throttle time constant
|
||||
*
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* This is the time constant of the TECS throttle control algorithm (in seconds).
|
||||
* Smaller values make it faster to respond, larger values make it slower
|
||||
* to respond.
|
||||
*
|
||||
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
|
||||
/**
|
||||
* Throttle damping factor
|
||||
*
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* This is the damping gain for the throttle demand loop.
|
||||
* Increase to add damping to correct for oscillations in speed and height.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
|
||||
/**
|
||||
* Integrator gain
|
||||
*
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* This is the integrator gain on the control loop.
|
||||
* Increasing this gain increases the speed at which speed
|
||||
* and height offsets are trimmed out, but reduces damping and
|
||||
* increases overshoot.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
|
||||
* Maximum vertical acceleration
|
||||
*
|
||||
* This is the maximum vertical acceleration (in metres/second square)
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* either up or down that the controller will use to correct speed
|
||||
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
|
||||
* allows for reasonably aggressive pitch changes if required to recover
|
||||
* from under-speed conditions.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
|
||||
/**
|
||||
* Complementary filter "omega" parameter for height
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse vertical acceleration and barometric height to obtain
|
||||
* an estimate of height rate and height. Increasing this frequency weights
|
||||
* the solution more towards use of the barometer, whilst reducing it weights
|
||||
* the solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
|
||||
/**
|
||||
* Complementary filter "omega" parameter for speed
|
||||
*
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* This is the cross-over frequency (in radians/second) of the complementary
|
||||
* filter used to fuse longitudinal acceleration and airspeed to obtain an
|
||||
* improved airspeed estimate. Increasing this frequency weights the solution
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* more towards use of the arispeed sensor, whilst reducing it weights the
|
||||
* solution more towards use of the accelerometer data.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
|
||||
/**
|
||||
* Roll -> Throttle feedforward
|
||||
*
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* Increasing this gain turn increases the amount of throttle that will
|
||||
* be used to compensate for the additional drag created by turning.
|
||||
* Ideally this should be set to approximately 10 x the extra sink rate
|
||||
* in m/s created by a 45 degree bank turn. Increase this gain if
|
||||
* the aircraft initially loses energy in turns and reduce if the
|
||||
* aircraft initially gains energy in turns. Efficient high aspect-ratio
|
||||
* aircraft (eg powered sailplanes) can use a lower value, whereas
|
||||
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
|
||||
/**
|
||||
* Speed <--> Altitude priority
|
||||
*
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* This parameter adjusts the amount of weighting that the pitch control
|
||||
* applies to speed vs height errors. Setting it to 0.0 will cause the
|
||||
* pitch control to control height and ignore speed errors. This will
|
||||
* normally improve height accuracy but give larger airspeed errors.
|
||||
* Setting it to 2.0 will cause the pitch control loop to control speed
|
||||
* and ignore height errors. This will normally reduce airspeed errors,
|
||||
* but give larger height errors. The default value of 1.0 allows the pitch
|
||||
* control to simultaneously control height and speed.
|
||||
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
|
||||
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
|
||||
/**
|
||||
* Pitch damping factor
|
||||
*
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* This is the damping gain for the pitch demand loop. Increase to add
|
||||
* damping to correct for oscillations in height. The default value of 0.0
|
||||
* will work well provided the pitch to servo controller has been tuned
|
||||
* properly.
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
@@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Height rate FF factor
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Speed rate P factor
|
||||
*
|
||||
@@ -379,18 +386,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
|
||||
|
||||
/**
|
||||
* Landing flare altitude (relative)
|
||||
* Landing flare altitude (relative to landing altitude)
|
||||
*
|
||||
* @unit meter
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
|
||||
|
||||
/**
|
||||
* Landing throttle limit altitude (relative)
|
||||
* Landing throttle limit altitude (relative landing altitude)
|
||||
*
|
||||
* Default of -1.0f lets the system default to applying throttle
|
||||
* limiting at 2/3 of the flare altitude.
|
||||
*
|
||||
* @unit meter
|
||||
* @group L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
|
||||
|
||||
/**
|
||||
* Landing heading hold horizontal distance
|
||||
|
||||
+547
-173
@@ -31,38 +31,39 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file mavlink_ftp.cpp
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <crc32.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_tests/mavlink_ftp_test.h"
|
||||
|
||||
// Uncomment the line below to get better debug output. Never commit with this left on.
|
||||
//#define MAVLINK_FTP_DEBUG
|
||||
|
||||
MavlinkFTP *MavlinkFTP::_server;
|
||||
|
||||
MavlinkFTP *
|
||||
MavlinkFTP::getServer()
|
||||
MavlinkFTP::get_server(void)
|
||||
{
|
||||
// XXX this really cries out for some locking...
|
||||
if (_server == nullptr) {
|
||||
_server = new MavlinkFTP;
|
||||
}
|
||||
return _server;
|
||||
static MavlinkFTP server;
|
||||
return &server;
|
||||
}
|
||||
|
||||
MavlinkFTP::MavlinkFTP() :
|
||||
_session_fds{},
|
||||
_workBufs{},
|
||||
_workFree{},
|
||||
_lock{}
|
||||
_request_bufs{},
|
||||
_request_queue{},
|
||||
_request_queue_sem{},
|
||||
_utRcvMsgFunc{},
|
||||
_ftp_test{}
|
||||
{
|
||||
// initialise the request freelist
|
||||
dq_init(&_workFree);
|
||||
sem_init(&_lock, 0, 1);
|
||||
dq_init(&_request_queue);
|
||||
sem_init(&_request_queue_sem, 0, 1);
|
||||
|
||||
// initialize session list
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
@@ -71,161 +72,258 @@ MavlinkFTP::MavlinkFTP() :
|
||||
|
||||
// drop work entries onto the free list
|
||||
for (unsigned i = 0; i < kRequestQueueSize; i++) {
|
||||
_qFree(&_workBufs[i]);
|
||||
_return_request(&_request_bufs[i]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
void
|
||||
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
_utRcvMsgFunc = rcvMsgFunc;
|
||||
_ftp_test = ftp_test;
|
||||
}
|
||||
#endif
|
||||
|
||||
void
|
||||
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
|
||||
{
|
||||
// get a free request
|
||||
auto req = _dqFree();
|
||||
struct Request* req = _get_request();
|
||||
|
||||
// if we couldn't get a request slot, just drop it
|
||||
if (req != nullptr) {
|
||||
if (req == nullptr) {
|
||||
warnx("Dropping FTP request: queue full\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// decode the request
|
||||
if (req->decode(mavlink, msg)) {
|
||||
|
||||
// and queue it for the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
|
||||
} else {
|
||||
_qFree(req);
|
||||
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
if (!_utRcvMsgFunc) {
|
||||
warnx("Incorrectly written unit test\n");
|
||||
return;
|
||||
}
|
||||
// We use fake ids when unit testing
|
||||
req->serverSystemId = MavlinkFtpTest::serverSystemId;
|
||||
req->serverComponentId = MavlinkFtpTest::serverComponentId;
|
||||
req->serverChannel = MavlinkFtpTest::serverChannel;
|
||||
#else
|
||||
// Not unit testing, use the real thing
|
||||
req->serverSystemId = mavlink->get_system_id();
|
||||
req->serverComponentId = mavlink->get_component_id();
|
||||
req->serverChannel = mavlink->get_channel();
|
||||
#endif
|
||||
|
||||
// This is the system id we want to target when sending
|
||||
req->targetSystemId = msg->sysid;
|
||||
|
||||
if (req->message.target_system == req->serverSystemId) {
|
||||
req->mavlink = mavlink;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// We are running in Unit Test mode. Don't queue, just call _worket directly.
|
||||
_process_request(req);
|
||||
#else
|
||||
// We are running in normal mode. Queue the request to the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Queued static work queue routine to handle mavlink messages
|
||||
void
|
||||
MavlinkFTP::_workerTrampoline(void *arg)
|
||||
MavlinkFTP::_worker_trampoline(void *arg)
|
||||
{
|
||||
auto req = reinterpret_cast<Request *>(arg);
|
||||
auto server = MavlinkFTP::getServer();
|
||||
Request* req = reinterpret_cast<Request *>(arg);
|
||||
MavlinkFTP* server = MavlinkFTP::get_server();
|
||||
|
||||
// call the server worker with the work item
|
||||
server->_worker(req);
|
||||
server->_process_request(req);
|
||||
}
|
||||
|
||||
/// @brief Processes an FTP message
|
||||
void
|
||||
MavlinkFTP::_worker(Request *req)
|
||||
MavlinkFTP::_process_request(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
uint32_t messageCRC;
|
||||
|
||||
// basic sanity checks; must validate length before use
|
||||
if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
|
||||
errorCode = kErrNoRequest;
|
||||
if (payload->size > kMaxDataLength) {
|
||||
errorCode = kErrInvalidDataSize;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// check request CRC to make sure this is one of ours
|
||||
messageCRC = hdr->crc32;
|
||||
hdr->crc32 = 0;
|
||||
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
|
||||
errorCode = kErrNoRequest;
|
||||
goto out;
|
||||
warnx("ftp: bad crc");
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
|
||||
#endif
|
||||
|
||||
switch (hdr->opcode) {
|
||||
switch (payload->opcode) {
|
||||
case kCmdNone:
|
||||
break;
|
||||
|
||||
case kCmdTerminate:
|
||||
errorCode = _workTerminate(req);
|
||||
case kCmdTerminateSession:
|
||||
errorCode = _workTerminate(payload);
|
||||
break;
|
||||
|
||||
case kCmdReset:
|
||||
errorCode = _workReset();
|
||||
case kCmdResetSessions:
|
||||
errorCode = _workReset(payload);
|
||||
break;
|
||||
|
||||
case kCmdList:
|
||||
errorCode = _workList(req);
|
||||
case kCmdListDirectory:
|
||||
errorCode = _workList(payload);
|
||||
break;
|
||||
|
||||
case kCmdOpen:
|
||||
errorCode = _workOpen(req, false);
|
||||
case kCmdOpenFileRO:
|
||||
errorCode = _workOpen(payload, O_RDONLY);
|
||||
break;
|
||||
|
||||
case kCmdCreate:
|
||||
errorCode = _workOpen(req, true);
|
||||
case kCmdCreateFile:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdRead:
|
||||
errorCode = _workRead(req);
|
||||
case kCmdOpenFileWO:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdWrite:
|
||||
errorCode = _workWrite(req);
|
||||
case kCmdReadFile:
|
||||
errorCode = _workRead(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemove:
|
||||
errorCode = _workRemove(req);
|
||||
case kCmdWriteFile:
|
||||
errorCode = _workWrite(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveFile:
|
||||
errorCode = _workRemoveFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdRename:
|
||||
errorCode = _workRename(payload);
|
||||
break;
|
||||
|
||||
case kCmdTruncateFile:
|
||||
errorCode = _workTruncateFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdCreateDirectory:
|
||||
errorCode = _workCreateDirectory(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveDirectory:
|
||||
errorCode = _workRemoveDirectory(payload);
|
||||
break;
|
||||
|
||||
|
||||
case kCmdCalcFileCRC32:
|
||||
errorCode = _workCalcFileCRC32(payload);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = kErrNoRequest;
|
||||
errorCode = kErrUnknownCommand;
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
// handle success vs. error
|
||||
if (errorCode == kErrNone) {
|
||||
hdr->opcode = kRspAck;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspAck;
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: ack\n");
|
||||
#endif
|
||||
} else {
|
||||
int r_errno = errno;
|
||||
warnx("FTP: nak %u", errorCode);
|
||||
hdr->opcode = kRspNak;
|
||||
hdr->size = 1;
|
||||
hdr->data[0] = errorCode;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspNak;
|
||||
payload->size = 1;
|
||||
payload->data[0] = errorCode;
|
||||
if (errorCode == kErrFailErrno) {
|
||||
payload->size = 2;
|
||||
payload->data[1] = r_errno;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// respond to the request
|
||||
_reply(req);
|
||||
|
||||
// free the request buffer back to the freelist
|
||||
_qFree(req);
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP reponse message out through mavlink
|
||||
void
|
||||
MavlinkFTP::_reply(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
hdr->magic = kProtocolMagic;
|
||||
payload->seqNumber = payload->seqNumber + 1;
|
||||
|
||||
// message is assumed to be already constructed in the request buffer, so generate the CRC
|
||||
hdr->crc32 = 0;
|
||||
hdr->crc32 = crc32(req->rawData(), req->dataSize());
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
uint16_t len =
|
||||
#endif
|
||||
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
|
||||
req->serverComponentId, // Sender component id
|
||||
req->serverChannel, // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
req->targetSystemId, // Target system id
|
||||
0, // Target component id
|
||||
(const uint8_t*)payload); // Payload to pack into message
|
||||
|
||||
bool success = true;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// Unit test hook is set, call that instead
|
||||
_utRcvMsgFunc(&msg, _ftp_test);
|
||||
#else
|
||||
Mavlink *mavlink = req->mavlink;
|
||||
|
||||
mavlink->lockMessageBufferMutex();
|
||||
success = mavlink->message_buffer_write(&msg, len);
|
||||
mavlink->unlockMessageBufferMutex();
|
||||
|
||||
#endif
|
||||
|
||||
// then pack and send the reply back to the request source
|
||||
req->reply();
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
|
||||
req->serverSystemId,
|
||||
req->serverComponentId,
|
||||
req->serverChannel,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @brief Responds to a List command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workList(Request *req)
|
||||
MavlinkFTP::_workList(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
char dirPath[kMaxDataLength];
|
||||
strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
|
||||
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
DIR *dp = opendir(dirPath);
|
||||
|
||||
if (dp == nullptr) {
|
||||
warnx("FTP: can't open path '%s'", dirPath);
|
||||
return kErrNotDir;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
|
||||
warnx("FTP: list %s offset %d", dirPath, payload->offset);
|
||||
#endif
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
@@ -233,19 +331,19 @@ MavlinkFTP::_workList(Request *req)
|
||||
unsigned offset = 0;
|
||||
|
||||
// move to the requested offset
|
||||
seekdir(dp, hdr->offset);
|
||||
seekdir(dp, payload->offset);
|
||||
|
||||
for (;;) {
|
||||
// read the directory entry
|
||||
if (readdir_r(dp, &entry, &result)) {
|
||||
warnx("FTP: list %s readdir_r failure\n", dirPath);
|
||||
errorCode = kErrIO;
|
||||
errorCode = kErrFailErrno;
|
||||
break;
|
||||
}
|
||||
|
||||
// no more entries?
|
||||
if (result == nullptr) {
|
||||
if (hdr->offset != 0 && offset == 0) {
|
||||
if (payload->offset != 0 && offset == 0) {
|
||||
// User is requesting subsequent dir entries but there were none. This means the user asked
|
||||
// to seek past EOF.
|
||||
errorCode = kErrEOF;
|
||||
@@ -270,14 +368,22 @@ MavlinkFTP::_workList(Request *req)
|
||||
}
|
||||
break;
|
||||
case DTYPE_DIRECTORY:
|
||||
direntType = kDirentDir;
|
||||
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
|
||||
// Don't bother sending these back
|
||||
direntType = kDirentSkip;
|
||||
} else {
|
||||
direntType = kDirentDir;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
direntType = kDirentUnknown;
|
||||
break;
|
||||
// We only send back file and diretory entries, skip everything else
|
||||
direntType = kDirentSkip;
|
||||
}
|
||||
|
||||
if (entry.d_type == DTYPE_FILE) {
|
||||
if (direntType == kDirentSkip) {
|
||||
// Skip send only dirent identifier
|
||||
buf[0] = '\0';
|
||||
} else if (direntType == kDirentFile) {
|
||||
// Files send filename and file length
|
||||
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
|
||||
} else {
|
||||
@@ -293,145 +399,230 @@ MavlinkFTP::_workList(Request *req)
|
||||
}
|
||||
|
||||
// Move the data into the buffer
|
||||
hdr->data[offset++] = direntType;
|
||||
strcpy((char *)&hdr->data[offset], buf);
|
||||
payload->data[offset++] = direntType;
|
||||
strcpy((char *)&payload->data[offset], buf);
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
|
||||
#endif
|
||||
offset += nameLen + 1;
|
||||
}
|
||||
|
||||
closedir(dp);
|
||||
hdr->size = offset;
|
||||
payload->size = offset;
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
/// @brief Responds to an Open command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workOpen(Request *req, bool create)
|
||||
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
int session_index = _findUnusedSession();
|
||||
int session_index = _find_unused_session();
|
||||
if (session_index < 0) {
|
||||
return kErrNoSession;
|
||||
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(req->dataAsCString(), &st) != 0) {
|
||||
return kErrNotFile;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
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 oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
|
||||
|
||||
int fd = ::open(req->dataAsCString(), oflag);
|
||||
int fd = ::open(filename, oflag);
|
||||
if (fd < 0) {
|
||||
return create ? kErrPerm : kErrNotFile;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
_session_fds[session_index] = fd;
|
||||
|
||||
hdr->session = session_index;
|
||||
if (create) {
|
||||
hdr->size = 0;
|
||||
} else {
|
||||
hdr->size = sizeof(uint32_t);
|
||||
*((uint32_t*)hdr->data) = fileSize;
|
||||
}
|
||||
payload->session = session_index;
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = fileSize;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Read command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRead(Request *req)
|
||||
MavlinkFTP::_workRead(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
int session_index = payload->session;
|
||||
|
||||
int session_index = hdr->session;
|
||||
|
||||
if (!_validSession(session_index)) {
|
||||
return kErrNoSession;
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", hdr->offset);
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrEOF;
|
||||
}
|
||||
|
||||
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
|
||||
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
|
||||
if (bytes_read < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("read fail %d", bytes_read);
|
||||
return kErrIO;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
hdr->size = bytes_read;
|
||||
payload->size = bytes_read;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Write command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workWrite(Request *req)
|
||||
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 kErrPerm;
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
{
|
||||
//auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTerminate(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
if (!_validSession(hdr->session)) {
|
||||
return kErrNoSession;
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
::close(_session_fds[hdr->session]);
|
||||
|
||||
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
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(void)
|
||||
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
|
||||
{
|
||||
char file[kMaxDataLength];
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (unlink(file) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @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)
|
||||
{
|
||||
if (!_valid_session(payload->session)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
::close(_session_fds[payload->session]);
|
||||
_session_fds[payload->session] = -1;
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Reset command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(PayloadHeader* payload)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] != -1) {
|
||||
@@ -440,11 +631,104 @@ MavlinkFTP::_workReset(void)
|
||||
}
|
||||
}
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
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)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (rmdir(dir) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CreateDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @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::_validSession(unsigned index)
|
||||
MavlinkFTP::_valid_session(unsigned index)
|
||||
{
|
||||
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
|
||||
return false;
|
||||
@@ -452,8 +736,9 @@ MavlinkFTP::_validSession(unsigned index)
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Returns an unused session index
|
||||
int
|
||||
MavlinkFTP::_findUnusedSession(void)
|
||||
MavlinkFTP::_find_unused_session(void)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] == -1) {
|
||||
@@ -464,16 +749,105 @@ MavlinkFTP::_findUnusedSession(void)
|
||||
return -1;
|
||||
}
|
||||
|
||||
/// @brief Guarantees that the payload data is null terminated.
|
||||
/// @return Returns a pointer to the payload data as a char *
|
||||
char *
|
||||
MavlinkFTP::Request::dataAsCString()
|
||||
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
|
||||
{
|
||||
// guarantee nul termination
|
||||
if (header()->size < kMaxDataLength) {
|
||||
requestData()[header()->size] = '\0';
|
||||
if (payload->size < kMaxDataLength) {
|
||||
payload->data[payload->size] = '\0';
|
||||
} else {
|
||||
requestData()[kMaxDataLength - 1] = '\0';
|
||||
payload->data[kMaxDataLength - 1] = '\0';
|
||||
}
|
||||
|
||||
// and return data
|
||||
return (char *)&(header()->data[0]);
|
||||
return (char *)&(payload->data[0]);
|
||||
}
|
||||
|
||||
/// @brief Returns a unused Request entry. NULL if none available.
|
||||
MavlinkFTP::Request *
|
||||
MavlinkFTP::_get_request(void)
|
||||
{
|
||||
_lock_request_queue();
|
||||
Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
|
||||
_unlock_request_queue();
|
||||
return req;
|
||||
}
|
||||
|
||||
/// @brief Locks a semaphore to provide exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_lock_request_queue(void)
|
||||
{
|
||||
do {}
|
||||
while (sem_wait(&_request_queue_sem) != 0);
|
||||
}
|
||||
|
||||
/// @brief Unlocks the semaphore providing exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_unlock_request_queue(void)
|
||||
{
|
||||
sem_post(&_request_queue_sem);
|
||||
}
|
||||
|
||||
/// @brief Returns a no longer needed request to the queue
|
||||
void
|
||||
MavlinkFTP::_return_request(Request *req)
|
||||
{
|
||||
_lock_request_queue();
|
||||
dq_addlast(&req->work.dq, &_request_queue);
|
||||
_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;
|
||||
}
|
||||
|
||||
+119
-169
@@ -33,20 +33,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file mavlink_ftp.h
|
||||
*
|
||||
* MAVLink remote file server.
|
||||
*
|
||||
* Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
|
||||
* a session ID and sequence number.
|
||||
*
|
||||
* A limited number of requests (currently 2) may be outstanding at a time.
|
||||
* Additional messages will be discarded.
|
||||
*
|
||||
* Messages consist of a fixed header, followed by a data area.
|
||||
*
|
||||
*/
|
||||
/// @file mavlink_ftp.h
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <dirent.h>
|
||||
#include <queue.h>
|
||||
@@ -57,174 +45,136 @@
|
||||
#include "mavlink_messages.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
class MavlinkFtpTest;
|
||||
|
||||
/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
|
||||
/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
|
||||
class MavlinkFTP
|
||||
{
|
||||
public:
|
||||
/// @brief Returns the one Mavlink FTP server in the system.
|
||||
static MavlinkFTP* get_server(void);
|
||||
|
||||
/// @brief Contructor is only public so unit test code can new objects.
|
||||
MavlinkFTP();
|
||||
|
||||
/// @brief Adds the specified message to the work queue.
|
||||
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
|
||||
|
||||
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
/// @brief Sets up the server to run in unit test mode.
|
||||
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
|
||||
/// @param ftp_test MavlinkFtpTest object which the function is associated with
|
||||
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
|
||||
|
||||
static MavlinkFTP *getServer();
|
||||
|
||||
// static interface
|
||||
void handle_message(Mavlink* mavlink,
|
||||
mavlink_message_t *msg);
|
||||
|
||||
private:
|
||||
|
||||
static const unsigned kRequestQueueSize = 2;
|
||||
|
||||
static MavlinkFTP *_server;
|
||||
|
||||
struct RequestHeader
|
||||
{
|
||||
uint8_t magic;
|
||||
uint8_t session;
|
||||
uint8_t opcode;
|
||||
uint8_t size;
|
||||
uint32_t crc32;
|
||||
uint32_t offset;
|
||||
uint8_t data[];
|
||||
};
|
||||
|
||||
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
|
||||
/// 32 bit alignment to avoid usage of any pack pragmas.
|
||||
struct PayloadHeader
|
||||
{
|
||||
uint16_t seqNumber; ///< sequence number for message
|
||||
uint8_t session; ///< Session id for read and write commands
|
||||
uint8_t opcode; ///< Command opcode
|
||||
uint8_t size; ///< Size of data
|
||||
uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message
|
||||
uint8_t padding[2]; ///< 32 bit aligment padding
|
||||
uint32_t offset; ///< Offsets for List and Read commands
|
||||
uint8_t data[]; ///< command data, varies by Opcode
|
||||
};
|
||||
|
||||
/// @brief Command opcodes
|
||||
enum Opcode : uint8_t
|
||||
{
|
||||
kCmdNone, // ignored, always acked
|
||||
kCmdTerminate, // releases sessionID, closes file
|
||||
kCmdReset, // terminates all sessions
|
||||
kCmdList, // list files in <path> from <offset>
|
||||
kCmdOpen, // opens <path> for reading, returns <session>
|
||||
kCmdRead, // reads <size> bytes from <offset> in <session>
|
||||
kCmdCreate, // creates <path> for writing, returns <session>
|
||||
kCmdWrite, // appends <size> bytes at <offset> in <session>
|
||||
kCmdRemove, // remove file (only if created by server?)
|
||||
|
||||
kRspAck,
|
||||
kRspNak
|
||||
kCmdNone, ///< ignored, always acked
|
||||
kCmdTerminateSession, ///< Terminates open Read session
|
||||
kCmdResetSessions, ///< Terminates all open Read sessions
|
||||
kCmdListDirectory, ///< List files in <path> from <offset>
|
||||
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, ///< 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
|
||||
};
|
||||
|
||||
|
||||
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
|
||||
enum ErrorCode : uint8_t
|
||||
{
|
||||
{
|
||||
kErrNone,
|
||||
kErrNoRequest,
|
||||
kErrNoSession,
|
||||
kErrSequence,
|
||||
kErrNotDir,
|
||||
kErrNotFile,
|
||||
kErrEOF,
|
||||
kErrNotAppend,
|
||||
kErrTooBig,
|
||||
kErrIO,
|
||||
kErrPerm
|
||||
};
|
||||
|
||||
int _findUnusedSession(void);
|
||||
bool _validSession(unsigned index);
|
||||
|
||||
static const unsigned kMaxSession = 2;
|
||||
int _session_fds[kMaxSession];
|
||||
|
||||
class Request
|
||||
kErrFail, ///< Unknown failure
|
||||
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
|
||||
kErrInvalidDataSize, ///< PayloadHeader.size is invalid
|
||||
kErrInvalidSession, ///< Session is not currently open
|
||||
kErrNoSessionsAvailable, ///< All available Sessions in use
|
||||
kErrEOF, ///< Offset past end of file for List and Read commands
|
||||
kErrUnknownCommand ///< Unknown command opcode
|
||||
};
|
||||
|
||||
private:
|
||||
/// @brief Unit of work which is queued to work_queue
|
||||
struct Request
|
||||
{
|
||||
public:
|
||||
union {
|
||||
dq_entry_t entry;
|
||||
work_s work;
|
||||
};
|
||||
|
||||
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
|
||||
if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
|
||||
_mavlink = mavlink;
|
||||
mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void reply() {
|
||||
|
||||
// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
|
||||
// flat memory architecture, as we're operating between threads here.
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
|
||||
_mavlink->get_channel(), &msg, sequence()+1, rawData());
|
||||
|
||||
_mavlink->lockMessageBufferMutex();
|
||||
bool success = _mavlink->message_buffer_write(&msg, len);
|
||||
_mavlink->unlockMessageBufferMutex();
|
||||
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
|
||||
_mavlink->get_system_id(),
|
||||
_mavlink->get_component_id(),
|
||||
_mavlink->get_channel(),
|
||||
len,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t *rawData() { return &_message.data[0]; }
|
||||
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
|
||||
uint8_t *requestData() { return &(header()->data[0]); }
|
||||
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
|
||||
uint16_t sequence() const { return _message.seqnr; }
|
||||
mavlink_channel_t channel() { return _mavlink->get_channel(); }
|
||||
|
||||
char *dataAsCString();
|
||||
|
||||
private:
|
||||
Mavlink *_mavlink;
|
||||
mavlink_encapsulated_data_t _message;
|
||||
work_s work; ///< work queue entry
|
||||
Mavlink *mavlink; ///< Mavlink to reply to
|
||||
uint8_t serverSystemId; ///< System ID to send from
|
||||
uint8_t serverComponentId; ///< Component ID to send from
|
||||
uint8_t serverChannel; ///< Channel to send to
|
||||
uint8_t targetSystemId; ///< System ID to target reply to
|
||||
|
||||
mavlink_file_transfer_protocol_t message; ///< Protocol message
|
||||
};
|
||||
|
||||
Request *_get_request(void);
|
||||
void _return_request(Request *req);
|
||||
void _lock_request_queue(void);
|
||||
void _unlock_request_queue(void);
|
||||
|
||||
char *_data_as_cstring(PayloadHeader* payload);
|
||||
|
||||
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);
|
||||
|
||||
static const uint8_t kProtocolMagic = 'f';
|
||||
static const char kDirentFile = 'F';
|
||||
static const char kDirentDir = 'D';
|
||||
static const char kDirentUnknown = 'U';
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
|
||||
ErrorCode _workList(PayloadHeader *payload);
|
||||
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
|
||||
ErrorCode _workRead(PayloadHeader *payload);
|
||||
ErrorCode _workWrite(PayloadHeader *payload);
|
||||
ErrorCode _workTerminate(PayloadHeader *payload);
|
||||
ErrorCode _workReset(PayloadHeader* payload);
|
||||
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workCreateDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workRemoveFile(PayloadHeader *payload);
|
||||
ErrorCode _workTruncateFile(PayloadHeader *payload);
|
||||
ErrorCode _workRename(PayloadHeader *payload);
|
||||
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
|
||||
|
||||
/// Request worker; runs on the low-priority work queue to service
|
||||
/// remote requests.
|
||||
///
|
||||
static void _workerTrampoline(void *arg);
|
||||
void _worker(Request *req);
|
||||
|
||||
/// Reply to a request (XXX should be a Request method)
|
||||
///
|
||||
void _reply(Request *req);
|
||||
|
||||
ErrorCode _workList(Request *req);
|
||||
ErrorCode _workOpen(Request *req, bool create);
|
||||
ErrorCode _workRead(Request *req);
|
||||
ErrorCode _workWrite(Request *req);
|
||||
ErrorCode _workRemove(Request *req);
|
||||
ErrorCode _workTerminate(Request *req);
|
||||
ErrorCode _workReset();
|
||||
|
||||
// work freelist
|
||||
Request _workBufs[kRequestQueueSize];
|
||||
dq_queue_t _workFree;
|
||||
sem_t _lock;
|
||||
|
||||
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
|
||||
void _qUnlock() { sem_post(&_lock); }
|
||||
|
||||
void _qFree(Request *req) {
|
||||
_qLock();
|
||||
dq_addlast(&req->entry, &_workFree);
|
||||
_qUnlock();
|
||||
}
|
||||
|
||||
Request *_dqFree() {
|
||||
_qLock();
|
||||
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
|
||||
_qUnlock();
|
||||
return req;
|
||||
}
|
||||
|
||||
static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests
|
||||
Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work
|
||||
dq_queue_t _request_queue; ///< Queue of available Request buffers
|
||||
sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue
|
||||
|
||||
int _find_unused_session(void);
|
||||
bool _valid_session(unsigned index);
|
||||
|
||||
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
|
||||
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
|
||||
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
|
||||
|
||||
/// @brief Maximum data size in RequestHeader::data
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
|
||||
|
||||
static const unsigned kMaxSession = 2; ///< Max number of active sessions
|
||||
int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot
|
||||
|
||||
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
|
||||
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
|
||||
};
|
||||
|
||||
@@ -1403,14 +1403,14 @@ 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", 0.5f);
|
||||
configure_stream("OPTICAL_FLOW", 20.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
configure_stream("ATTITUDE", 15.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 15.0f);
|
||||
configure_stream("CAMERA_CAPTURE", 1.0f);
|
||||
configure_stream("ATTITUDE", 50.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 50.0f);
|
||||
configure_stream("CAMERA_CAPTURE", 2.0f);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
@@ -886,8 +886,8 @@ protected:
|
||||
msg.eph = cm_uint16_from_m_float(gps.eph);
|
||||
msg.epv = cm_uint16_from_m_float(gps.epv);
|
||||
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
|
||||
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
|
||||
msg.satellites_visible = gps.satellites_used;
|
||||
msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
|
||||
msg.satellites_visible = gps.satellites_used;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
|
||||
}
|
||||
@@ -957,11 +957,11 @@ protected:
|
||||
msg.lat = pos.lat * 1e7;
|
||||
msg.lon = pos.lon * 1e7;
|
||||
msg.alt = pos.alt * 1000.0f;
|
||||
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
|
||||
msg.vx = pos.vel_n * 100.0f;
|
||||
msg.vy = pos.vel_e * 100.0f;
|
||||
msg.vz = pos.vel_d * 100.0f;
|
||||
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
|
||||
msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
|
||||
msg.vx = pos.vel_n * 100.0f;
|
||||
msg.vy = pos.vel_e * 100.0f;
|
||||
msg.vz = pos.vel_d * 100.0f;
|
||||
msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
|
||||
}
|
||||
@@ -1022,9 +1022,9 @@ protected:
|
||||
msg.x = pos.x;
|
||||
msg.y = pos.y;
|
||||
msg.z = pos.z;
|
||||
msg.vx = pos.vx;
|
||||
msg.vy = pos.vy;
|
||||
msg.vz = pos.vz;
|
||||
msg.vx = pos.vx;
|
||||
msg.vy = pos.vy;
|
||||
msg.vz = pos.vz;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
|
||||
}
|
||||
@@ -1085,9 +1085,9 @@ protected:
|
||||
msg.x = pos.x;
|
||||
msg.y = pos.y;
|
||||
msg.z = pos.z;
|
||||
msg.roll = pos.roll;
|
||||
msg.pitch = pos.pitch;
|
||||
msg.yaw = pos.yaw;
|
||||
msg.roll = pos.roll;
|
||||
msg.pitch = pos.pitch;
|
||||
msg.yaw = pos.yaw;
|
||||
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
|
||||
}
|
||||
|
||||
@@ -798,10 +798,10 @@ int
|
||||
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||
{
|
||||
if (mission_item->altitude_is_relative) {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
|
||||
} else {
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
|
||||
}
|
||||
|
||||
switch (mission_item->nav_cmd) {
|
||||
|
||||
@@ -55,6 +55,7 @@
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <time.h>
|
||||
#include <float.h>
|
||||
#include <unistd.h>
|
||||
@@ -103,6 +104,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_battery_pub(-1),
|
||||
_cmd_pub(-1),
|
||||
_flow_pub(-1),
|
||||
_range_pub(-1),
|
||||
_offboard_control_sp_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
@@ -123,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
{
|
||||
|
||||
// make sure the FTP server is started
|
||||
(void)MavlinkFTP::getServer();
|
||||
(void)MavlinkFTP::get_server();
|
||||
}
|
||||
|
||||
MavlinkReceiver::~MavlinkReceiver()
|
||||
@@ -182,8 +184,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
handle_message_request_data_stream(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
|
||||
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
|
||||
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
|
||||
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
@@ -210,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
||||
handle_message_hil_state_quaternion(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
handle_message_hil_optical_flow(msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -373,6 +379,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
/* optical flow */
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
struct optical_flow_s f;
|
||||
memset(&f, 0, sizeof(f));
|
||||
|
||||
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.quality = flow.quality;
|
||||
f.sensor_id = flow.sensor_id;
|
||||
|
||||
if (_flow_pub < 0) {
|
||||
_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
|
||||
}
|
||||
|
||||
/* Use distance value for range finder report */
|
||||
struct range_finder_report r;
|
||||
memset(&r, 0, sizeof(f));
|
||||
|
||||
r.timestamp = hrt_absolute_time();
|
||||
r.error_count = 0;
|
||||
r.type = RANGE_FINDER_TYPE_LASER;
|
||||
r.distance = flow.ground_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);
|
||||
|
||||
if (_range_pub < 0) {
|
||||
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
|
||||
} else {
|
||||
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
|
||||
{
|
||||
@@ -772,6 +824,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);
|
||||
|
||||
@@ -113,6 +113,7 @@ private:
|
||||
void handle_message_command_long(mavlink_message_t *msg);
|
||||
void handle_message_command_int(mavlink_message_t *msg);
|
||||
void handle_message_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_set_mode(mavlink_message_t *msg);
|
||||
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate(mavlink_message_t *msg);
|
||||
@@ -145,6 +146,7 @@ private:
|
||||
orb_advert_t _battery_pub;
|
||||
orb_advert_t _cmd_pub;
|
||||
orb_advert_t _flow_pub;
|
||||
orb_advert_t _range_pub;
|
||||
orb_advert_t _offboard_control_sp_pub;
|
||||
orb_advert_t _global_vel_sp_pub;
|
||||
orb_advert_t _att_sp_pub;
|
||||
|
||||
@@ -0,0 +1,761 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_test.cpp
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <crc32.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
|
||||
const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets
|
||||
};
|
||||
|
||||
const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
|
||||
const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
|
||||
|
||||
MavlinkFtpTest::MavlinkFtpTest() :
|
||||
_ftp_server{},
|
||||
_reply_msg{},
|
||||
_lastOutgoingSeqNumber{}
|
||||
{
|
||||
}
|
||||
|
||||
MavlinkFtpTest::~MavlinkFtpTest()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/// @brief Called before every test to initialize the FTP Server.
|
||||
void MavlinkFtpTest::_init(void)
|
||||
{
|
||||
_ftp_server = new MavlinkFTP;;
|
||||
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Called after every test to take down the FTP Server.
|
||||
void MavlinkFtpTest::_cleanup(void)
|
||||
{
|
||||
delete _ftp_server;
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Tests for correct behavior of an Ack response.
|
||||
bool MavlinkFtpTest::_ack_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdNone;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct response to an invalid opcpde.
|
||||
bool MavlinkFtpTest::_bad_opcode_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = 0xFF; // bogus opcode
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a payload which an invalid data size field.
|
||||
bool MavlinkFtpTest::_bad_datasize_test(void)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
|
||||
_setup_ftp_msg(&payload, 0, nullptr, &msg);
|
||||
|
||||
// Set the data size to be one larger than is legal
|
||||
((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
|
||||
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
|
||||
if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_list_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
|
||||
char response2[] = "Ddev|Detc|Dfs|Dobj";
|
||||
|
||||
struct _testCase {
|
||||
const char *dir; ///< Directory to run List command on
|
||||
char *response; ///< Expected response entries from List command
|
||||
int response_count; ///< Number of directories that should be returned
|
||||
bool success; ///< true: List command should succeed, false: List command should fail
|
||||
};
|
||||
struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", nullptr, 0, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests", response1, 4, true },
|
||||
{ "/", response2, 4, true },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
|
||||
|
||||
// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
|
||||
// to a hardcoded return result string.
|
||||
|
||||
// Convert null terminators to seperator char so we can use strok to parse returned data
|
||||
for (uint8_t j=0; j<reply->size-1; j++) {
|
||||
if (reply->data[j] == 0) {
|
||||
reply->data[j] = '|';
|
||||
}
|
||||
}
|
||||
|
||||
// Loop over returned directory entries trying to find then in the response list
|
||||
char *dir;
|
||||
int response_count = 0;
|
||||
dir = strtok((char *)&reply->data[0], "|");
|
||||
while (dir != nullptr) {
|
||||
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
|
||||
response_count++;
|
||||
dir = strtok(nullptr, "|");
|
||||
}
|
||||
|
||||
ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
|
||||
/// is beyond the last directory entry.
|
||||
bool MavlinkFtpTest::_list_eof_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/";
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 4; // offset past top level dirs
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file which does not exist.
|
||||
bool MavlinkFtpTest::_open_badfile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/foo"; // non-existent file
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate
|
||||
bool MavlinkFtpTest::_open_terminate_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
|
||||
ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Terminate command on an invalid session.
|
||||
bool MavlinkFtpTest::_terminate_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session + 1;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an open session.
|
||||
bool MavlinkFtpTest::_read_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
// Read in the file so we can compare it to what we get back
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
uint8_t *bytes = new uint8_t[st.st_size];
|
||||
ut_assert("new failed", bytes != nullptr);
|
||||
int fd = ::open(test->file, O_RDONLY);
|
||||
ut_assert("open failed", fd != -1);
|
||||
int bytes_read = ::read(fd, bytes, st.st_size);
|
||||
ut_compare("read failed", bytes_read, st.st_size);
|
||||
::close(fd);
|
||||
|
||||
// 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.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session;
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Offset incorrect", reply->offset, 0);
|
||||
|
||||
if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
|
||||
ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
|
||||
ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an invalid session.
|
||||
bool MavlinkFtpTest::_read_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session + 1; // Invalid session
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removedirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
bool deleteFile;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests/empty_dir", false, false },
|
||||
{ _unittest_microsd_dir, false, false },
|
||||
{ _unittest_microsd_file, false, false },
|
||||
{ _unittest_microsd_dir, true, true },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
if (test->deleteFile) {
|
||||
ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_createdirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/etc/bogus", false },
|
||||
{ _unittest_microsd_dir, true },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ "/fs/microsd/bogus/bogus", false },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdCreateDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removefile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *file;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false },
|
||||
{ _rgReadTestCases[0].file, false },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ _unittest_microsd_file, true },
|
||||
{ _unittest_microsd_file, false },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
|
||||
/// it needs to send a message out on Mavlink.
|
||||
void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
ftp_test->_receive_message(msg);
|
||||
}
|
||||
|
||||
/// @brief Non-Static version of receive_message
|
||||
void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
|
||||
{
|
||||
// Move the message into our own member variable
|
||||
memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
|
||||
}
|
||||
|
||||
/// @brief Decode and validate the incoming message
|
||||
bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode
|
||||
mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message
|
||||
MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
|
||||
|
||||
// Make sure the targets are correct
|
||||
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
|
||||
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
|
||||
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
|
||||
|
||||
*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
|
||||
|
||||
// Make sure we have a good sequence number
|
||||
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
|
||||
|
||||
// Bump sequence number for next outgoing message
|
||||
_lastOutgoingSeqNumber++;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Initializes an FTP message into a mavlink message
|
||||
void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_message_t *msg) ///< Returned mavlink message
|
||||
{
|
||||
uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
|
||||
MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
|
||||
|
||||
memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
|
||||
|
||||
payload->seqNumber = _lastOutgoingSeqNumber;
|
||||
payload->size = size;
|
||||
if (size != 0) {
|
||||
memcpy(payload->data, data, size);
|
||||
}
|
||||
|
||||
payload->padding[0] = 0;
|
||||
payload->padding[1] = 0;
|
||||
|
||||
msg->checksum = 0;
|
||||
mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id
|
||||
clientComponentId, // Sender component id
|
||||
msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
serverSystemId, // Target system id
|
||||
serverComponentId, // Target component id
|
||||
payload_bytes); // Payload to pack into message
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP message to the server and returns response
|
||||
bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server
|
||||
MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
|
||||
_setup_ftp_msg(payload_header, size, data, &msg);
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
|
||||
}
|
||||
|
||||
/// @brief Cleans up an files created on microsd during testing
|
||||
void MavlinkFtpTest::_cleanup_microsd(void)
|
||||
{
|
||||
::unlink(_unittest_microsd_file);
|
||||
::rmdir(_unittest_microsd_dir);
|
||||
}
|
||||
|
||||
/// @brief Runs all the unit tests
|
||||
bool MavlinkFtpTest::run_tests(void)
|
||||
{
|
||||
ut_run_test(_ack_test);
|
||||
ut_run_test(_bad_opcode_test);
|
||||
ut_run_test(_bad_datasize_test);
|
||||
ut_run_test(_list_test);
|
||||
ut_run_test(_list_eof_test);
|
||||
ut_run_test(_open_badfile_test);
|
||||
ut_run_test(_open_terminate_test);
|
||||
ut_run_test(_terminate_badsession_test);
|
||||
ut_run_test(_read_test);
|
||||
ut_run_test(_read_badsession_test);
|
||||
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)
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_test.h
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <unit_test/unit_test.h>
|
||||
#include "../mavlink_bridge_header.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
class MavlinkFtpTest : public UnitTest
|
||||
{
|
||||
public:
|
||||
MavlinkFtpTest();
|
||||
virtual ~MavlinkFtpTest();
|
||||
|
||||
virtual bool run_tests(void);
|
||||
|
||||
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
static const uint8_t serverSystemId = 50; ///< System ID for server
|
||||
static const uint8_t serverComponentId = 1; ///< Component ID for server
|
||||
static const uint8_t serverChannel = 0; ///< Channel to send to
|
||||
|
||||
static const uint8_t clientSystemId = 1; ///< System ID for client
|
||||
static const uint8_t clientComponentId = 0; ///< Component ID for client
|
||||
|
||||
// We don't want any of these
|
||||
MavlinkFtpTest(const MavlinkFtpTest&);
|
||||
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);
|
||||
bool _list_test(void);
|
||||
bool _list_eof_test(void);
|
||||
bool _open_badfile_test(void);
|
||||
bool _open_terminate_test(void);
|
||||
bool _terminate_badsession_test(void);
|
||||
bool _read_test(void);
|
||||
bool _read_badsession_test(void);
|
||||
bool _removedirectory_test(void);
|
||||
bool _createdirectory_test(void);
|
||||
bool _removefile_test(void);
|
||||
|
||||
void _receive_message(const mavlink_message_t *msg);
|
||||
void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
|
||||
bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
|
||||
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
|
||||
uint8_t size,
|
||||
const uint8_t *data,
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply,
|
||||
MavlinkFTP::PayloadHeader **payload_reply);
|
||||
void _cleanup_microsd(void);
|
||||
|
||||
MavlinkFTP *_ftp_server;
|
||||
|
||||
mavlink_message_t _reply_msg;
|
||||
|
||||
uint16_t _lastOutgoingSeqNumber;
|
||||
|
||||
struct ReadTestCase {
|
||||
const char *file;
|
||||
const uint16_t length;
|
||||
};
|
||||
static const ReadTestCase _rgReadTestCases[];
|
||||
|
||||
static const char _unittest_microsd_dir[];
|
||||
static const char _unittest_microsd_file[];
|
||||
};
|
||||
|
||||
bool mavlink_ftp_test(void);
|
||||
@@ -0,0 +1,7 @@
|
||||
import sys
|
||||
print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
|
||||
file = open(sys.argv[1], 'w')
|
||||
for i in range(int(sys.argv[2])):
|
||||
b = bytearray([i & 0xFF])
|
||||
file.write(b)
|
||||
file.close()
|
||||
@@ -0,0 +1,47 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 mavlink_ftp_tests.cpp
|
||||
*/
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
|
||||
extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
|
||||
|
||||
int mavlink_tests_main(int argc, char *argv[])
|
||||
{
|
||||
return mavlink_ftp_test() ? 0 : -1;
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# System state machine tests.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink_tests
|
||||
SRCS = mavlink_tests.cpp \
|
||||
mavlink_ftp_test.cpp \
|
||||
../mavlink_ftp.cpp \
|
||||
../mavlink.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
MODULE_STACKSIZE = 5000
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST
|
||||
@@ -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)
|
||||
{
|
||||
@@ -651,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()
|
||||
{
|
||||
@@ -754,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 */
|
||||
@@ -850,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);
|
||||
@@ -1136,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 */
|
||||
|
||||
@@ -0,0 +1,227 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 datalinkloss.cpp
|
||||
* Helper class for Data Link Loss Mode according to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "datalinkloss.h"
|
||||
|
||||
#define DELAY_SIGMA 0.01f
|
||||
|
||||
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_param_commsholdwaittime(this, "CH_T"),
|
||||
_param_commsholdlat(this, "CH_LAT"),
|
||||
_param_commsholdlon(this, "CH_LON"),
|
||||
_param_commsholdalt(this, "CH_ALT"),
|
||||
_param_airfieldhomelat(this, "NAV_AH_LAT", false),
|
||||
_param_airfieldhomelon(this, "NAV_AH_LON", false),
|
||||
_param_airfieldhomealt(this, "NAV_AH_ALT", false),
|
||||
_param_airfieldhomewaittime(this, "AH_T"),
|
||||
_param_numberdatalinklosses(this, "N"),
|
||||
_param_skipcommshold(this, "CHSK"),
|
||||
_dll_state(DLL_STATE_NONE)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
DataLinkLoss::~DataLinkLoss()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::on_inactive()
|
||||
{
|
||||
/* reset DLL state only if setpoint moved */
|
||||
if (!_navigator->get_can_loiter_at_sp()) {
|
||||
_dll_state = DLL_STATE_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::on_activation()
|
||||
{
|
||||
_dll_state = DLL_STATE_NONE;
|
||||
updateParams();
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
updateParams();
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::set_dll_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
set_previous_pos_setpoint();
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_dll_state) {
|
||||
case DLL_STATE_FLYTOCOMMSHOLDWP: {
|
||||
_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
|
||||
_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _param_commsholdalt.get();
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
break;
|
||||
}
|
||||
case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
|
||||
_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
|
||||
_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _param_airfieldhomealt.get();
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
break;
|
||||
}
|
||||
case DLL_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
warnx("not switched to manual: request flight termination");
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = false;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::advance_dll()
|
||||
{
|
||||
switch (_dll_state) {
|
||||
case DLL_STATE_NONE:
|
||||
/* Check the number of data link losses. If above home fly home directly */
|
||||
/* if number of data link losses limit is not reached fly to comms hold wp */
|
||||
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
|
||||
warnx("%d data link losses, limit is %d, fly to airfield home",
|
||||
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
} else {
|
||||
if (!_param_skipcommshold.get()) {
|
||||
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
|
||||
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
|
||||
} else {
|
||||
/* comms hold wp not active, fly to airfield home directly */
|
||||
warnx("Skipping comms hold wp. Flying directly to airfield home");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case DLL_STATE_FLYTOCOMMSHOLDWP:
|
||||
warnx("fly to airfield home");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
break;
|
||||
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
|
||||
_dll_state = DLL_STATE_TERMINATE;
|
||||
warnx("time is up, state should have been changed manually by now");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
break;
|
||||
case DLL_STATE_TERMINATE:
|
||||
warnx("dll end");
|
||||
_dll_state = DLL_STATE_END;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 datalinkloss.h
|
||||
* Helper class for Data Link Loss Mode acording to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_DATALINKLOSS_H
|
||||
#define NAVIGATOR_DATALINKLOSS_H
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class DataLinkLoss : public MissionBlock
|
||||
{
|
||||
public:
|
||||
DataLinkLoss(Navigator *navigator, const char *name);
|
||||
|
||||
~DataLinkLoss();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
/* Params */
|
||||
control::BlockParamFloat _param_commsholdwaittime;
|
||||
control::BlockParamInt _param_commsholdlat; // * 1e7
|
||||
control::BlockParamInt _param_commsholdlon; // * 1e7
|
||||
control::BlockParamFloat _param_commsholdalt;
|
||||
control::BlockParamInt _param_airfieldhomelat; // * 1e7
|
||||
control::BlockParamInt _param_airfieldhomelon; // * 1e7
|
||||
control::BlockParamFloat _param_airfieldhomealt;
|
||||
control::BlockParamFloat _param_airfieldhomewaittime;
|
||||
control::BlockParamInt _param_numberdatalinklosses;
|
||||
control::BlockParamInt _param_skipcommshold;
|
||||
|
||||
enum DLLState {
|
||||
DLL_STATE_NONE = 0,
|
||||
DLL_STATE_FLYTOCOMMSHOLDWP = 1,
|
||||
DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
|
||||
DLL_STATE_TERMINATE = 3,
|
||||
DLL_STATE_END = 4
|
||||
} _dll_state;
|
||||
|
||||
/**
|
||||
* Set the DLL item
|
||||
*/
|
||||
void set_dll_item();
|
||||
|
||||
/**
|
||||
* Move to next DLL item
|
||||
*/
|
||||
void advance_dll();
|
||||
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,126 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 datalinkloss_params.c
|
||||
*
|
||||
* Parameters for DLL
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* Data Link Loss parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Comms hold wait time
|
||||
*
|
||||
* The amount of time in seconds the system should wait at the comms hold waypoint
|
||||
*
|
||||
* @unit seconds
|
||||
* @min 0.0
|
||||
* @group DLL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
|
||||
|
||||
/**
|
||||
* Comms hold Lat
|
||||
*
|
||||
* Latitude of comms hold waypoint
|
||||
*
|
||||
* @unit degrees * 1e7
|
||||
* @min 0.0
|
||||
* @group DLL
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
|
||||
|
||||
/**
|
||||
* Comms hold Lon
|
||||
*
|
||||
* Longitude of comms hold waypoint
|
||||
*
|
||||
* @unit degrees * 1e7
|
||||
* @min 0.0
|
||||
* @group DLL
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
|
||||
|
||||
/**
|
||||
* Comms hold alt
|
||||
*
|
||||
* Altitude of comms hold waypoint
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @group DLL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
|
||||
|
||||
/**
|
||||
* Aifield hole wait time
|
||||
*
|
||||
* The amount of time in seconds the system should wait at the airfield home waypoint
|
||||
*
|
||||
* @unit seconds
|
||||
* @min 0.0
|
||||
* @group DLL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
|
||||
|
||||
/**
|
||||
* Number of allowed Datalink timeouts
|
||||
*
|
||||
* After more than this number of data link timeouts the aircraft returns home directly
|
||||
*
|
||||
* @group DLL
|
||||
* @min 0
|
||||
* @max 1000
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_N, 2);
|
||||
|
||||
/**
|
||||
* Skip comms hold wp
|
||||
*
|
||||
* If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
|
||||
* airfield home
|
||||
*
|
||||
* @group DLL
|
||||
* @min 0
|
||||
* @max 1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);
|
||||
@@ -0,0 +1,149 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 enginefailure.cpp
|
||||
* Helper class for a fixedwing engine failure mode
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "enginefailure.h"
|
||||
|
||||
#define DELAY_SIGMA 0.01f
|
||||
|
||||
EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_ef_state(EF_STATE_NONE)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
EngineFailure::~EngineFailure()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
EngineFailure::on_inactive()
|
||||
{
|
||||
_ef_state = EF_STATE_NONE;
|
||||
}
|
||||
|
||||
void
|
||||
EngineFailure::on_activation()
|
||||
{
|
||||
_ef_state = EF_STATE_NONE;
|
||||
advance_ef();
|
||||
set_ef_item();
|
||||
}
|
||||
|
||||
void
|
||||
EngineFailure::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
advance_ef();
|
||||
set_ef_item();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
EngineFailure::set_ef_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* make sure we have the latest params */
|
||||
updateParams();
|
||||
|
||||
set_previous_pos_setpoint();
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_ef_state) {
|
||||
case EF_STATE_LOITERDOWN: {
|
||||
//XXX create mission item at ground (below?) here
|
||||
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
//XXX setting altitude to a very low value, evaluate other options
|
||||
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
EngineFailure::advance_ef()
|
||||
{
|
||||
switch (_ef_state) {
|
||||
case EF_STATE_NONE:
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
|
||||
_ef_state = EF_STATE_LOITERDOWN;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 enginefailure.h
|
||||
* Helper class for a fixedwing engine failure mode
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_ENGINEFAILURE_H
|
||||
#define NAVIGATOR_ENGINEFAILURE_H
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class EngineFailure : public MissionBlock
|
||||
{
|
||||
public:
|
||||
EngineFailure(Navigator *navigator, const char *name);
|
||||
|
||||
~EngineFailure();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
enum EFState {
|
||||
EF_STATE_NONE = 0,
|
||||
EF_STATE_LOITERDOWN = 1,
|
||||
} _ef_state;
|
||||
|
||||
/**
|
||||
* Set the DLL item
|
||||
*/
|
||||
void set_ef_item();
|
||||
|
||||
/**
|
||||
* Move to next EF item
|
||||
*/
|
||||
void advance_ef();
|
||||
|
||||
};
|
||||
#endif
|
||||
@@ -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,27 +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)
|
||||
{
|
||||
double lat = vehicle->lat / 1e7d;
|
||||
double lon = vehicle->lon / 1e7d;
|
||||
//float alt = vehicle->alt;
|
||||
return inside(global_position.lat, global_position.lon, global_position.alt);
|
||||
}
|
||||
|
||||
return inside(lat, lon, vehicle->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
|
||||
@@ -284,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;
|
||||
|
||||
@@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -0,0 +1,178 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 gpsfailure.cpp
|
||||
* Helper class for gpsfailure mode according to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "gpsfailure.h"
|
||||
|
||||
#define DELAY_SIGMA 0.01f
|
||||
|
||||
GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_param_loitertime(this, "LT"),
|
||||
_param_openlooploiter_roll(this, "R"),
|
||||
_param_openlooploiter_pitch(this, "P"),
|
||||
_param_openlooploiter_thrust(this, "TR"),
|
||||
_gpsf_state(GPSF_STATE_NONE),
|
||||
_timestamp_activation(0)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
GpsFailure::~GpsFailure()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
GpsFailure::on_inactive()
|
||||
{
|
||||
/* reset GPSF state only if setpoint moved */
|
||||
if (!_navigator->get_can_loiter_at_sp()) {
|
||||
_gpsf_state = GPSF_STATE_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
GpsFailure::on_activation()
|
||||
{
|
||||
_gpsf_state = GPSF_STATE_NONE;
|
||||
_timestamp_activation = hrt_absolute_time();
|
||||
updateParams();
|
||||
advance_gpsf();
|
||||
set_gpsf_item();
|
||||
}
|
||||
|
||||
void
|
||||
GpsFailure::on_active()
|
||||
{
|
||||
|
||||
switch (_gpsf_state) {
|
||||
case GPSF_STATE_LOITER: {
|
||||
/* Position controller does not run in this mode:
|
||||
* navigator has to publish an attitude setpoint */
|
||||
_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
|
||||
_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
|
||||
_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
|
||||
_navigator->publish_att_sp();
|
||||
|
||||
/* Measure time */
|
||||
hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
|
||||
|
||||
//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
|
||||
//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
|
||||
if (elapsed > _param_loitertime.get() * 1e6f) {
|
||||
/* no recovery, adavance the state machine */
|
||||
warnx("gps not recovered, switch to next state");
|
||||
advance_gpsf();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case GPSF_STATE_TERMINATE:
|
||||
set_gpsf_item();
|
||||
advance_gpsf();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
GpsFailure::set_gpsf_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* Set pos sp triplet to invalid to stop pos controller */
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = false;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
switch (_gpsf_state) {
|
||||
case GPSF_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
warnx("gps fail: request flight termination");
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
reset_mission_item_reached();
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
GpsFailure::advance_gpsf()
|
||||
{
|
||||
updateParams();
|
||||
|
||||
switch (_gpsf_state) {
|
||||
case GPSF_STATE_NONE:
|
||||
_gpsf_state = GPSF_STATE_LOITER;
|
||||
warnx("gpsf loiter");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
|
||||
break;
|
||||
case GPSF_STATE_LOITER:
|
||||
_gpsf_state = GPSF_STATE_TERMINATE;
|
||||
warnx("gpsf terminate");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
|
||||
warnx("mavlink sent");
|
||||
break;
|
||||
case GPSF_STATE_TERMINATE:
|
||||
warnx("gpsf end");
|
||||
_gpsf_state = GPSF_STATE_END;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 gpsfailure.h
|
||||
* Helper class for Data Link Loss Mode according to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_GPSFAILURE_H
|
||||
#define NAVIGATOR_GPSFAILURE_H
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class GpsFailure : public MissionBlock
|
||||
{
|
||||
public:
|
||||
GpsFailure(Navigator *navigator, const char *name);
|
||||
|
||||
~GpsFailure();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
/* Params */
|
||||
control::BlockParamFloat _param_loitertime;
|
||||
control::BlockParamFloat _param_openlooploiter_roll;
|
||||
control::BlockParamFloat _param_openlooploiter_pitch;
|
||||
control::BlockParamFloat _param_openlooploiter_thrust;
|
||||
|
||||
enum GPSFState {
|
||||
GPSF_STATE_NONE = 0,
|
||||
GPSF_STATE_LOITER = 1,
|
||||
GPSF_STATE_TERMINATE = 2,
|
||||
GPSF_STATE_END = 3,
|
||||
} _gpsf_state;
|
||||
|
||||
hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
|
||||
|
||||
/**
|
||||
* Set the GPSF item
|
||||
*/
|
||||
void set_gpsf_item();
|
||||
|
||||
/**
|
||||
* Move to next GPSF item
|
||||
*/
|
||||
void advance_gpsf();
|
||||
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,97 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 gpsfailure_params.c
|
||||
*
|
||||
* Parameters for GPSF navigation mode
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* GPS Failure Navigation Mode parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Loiter time
|
||||
*
|
||||
* The amount of time in seconds the system should do open loop loiter and wait for gps recovery
|
||||
* before it goes into flight termination.
|
||||
*
|
||||
* @unit seconds
|
||||
* @min 0.0
|
||||
* @group GPSF
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
|
||||
|
||||
/**
|
||||
* Open loop loiter roll
|
||||
*
|
||||
* Roll in degrees during the open loop loiter
|
||||
*
|
||||
* @unit deg
|
||||
* @min 0.0
|
||||
* @max 30.0
|
||||
* @group GPSF
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
|
||||
|
||||
/**
|
||||
* Open loop loiter pitch
|
||||
*
|
||||
* Pitch in degrees during the open loop loiter
|
||||
*
|
||||
* @unit deg
|
||||
* @min -30.0
|
||||
* @max 30.0
|
||||
* @group GPSF
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
|
||||
|
||||
/**
|
||||
* Open loop loiter thrust
|
||||
*
|
||||
* Thrust value which is set during the open loop loiter
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group GPSF
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
|
||||
|
||||
|
||||
@@ -36,12 +36,15 @@
|
||||
* Helper class to access missions
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <float.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
@@ -49,6 +52,7 @@
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
@@ -59,20 +63,23 @@
|
||||
|
||||
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_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)
|
||||
_dist_1wp_ok(false),
|
||||
_missionFeasiblityChecker(),
|
||||
_min_current_sp_distance_xy(FLT_MAX),
|
||||
_mission_item_previous_alt(NAN),
|
||||
_distance_current_previous(0.0f)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
@@ -107,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;
|
||||
}
|
||||
@@ -141,9 +149,22 @@ 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();
|
||||
} else {
|
||||
/* if waypoint position reached allow loiter on the setpoint */
|
||||
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
|
||||
@@ -202,7 +223,7 @@ Mission::update_offboard_mission()
|
||||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
|
||||
|
||||
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
|
||||
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
|
||||
@@ -313,11 +334,19 @@ Mission::set_mission_items()
|
||||
/* make sure param is up to date */
|
||||
updateParams();
|
||||
|
||||
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
|
||||
altitude_sp_foh_reset();
|
||||
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* set previous position setpoint to current */
|
||||
set_previous_pos_setpoint();
|
||||
|
||||
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
|
||||
if (pos_sp_triplet->previous.valid) {
|
||||
_mission_item_previous_alt = _mission_item.altitude;
|
||||
}
|
||||
|
||||
/* get home distance state */
|
||||
bool home_dist_ok = check_dist_1wp();
|
||||
/* the home dist check provides user feedback, so we initialize it to this */
|
||||
@@ -343,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
|
||||
@@ -359,6 +392,7 @@ 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();
|
||||
@@ -399,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());
|
||||
|
||||
@@ -407,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) {
|
||||
report_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;
|
||||
|
||||
@@ -444,11 +492,81 @@ 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 */
|
||||
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
|
||||
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
|
||||
pos_sp_triplet->current.lon,
|
||||
pos_sp_triplet->previous.lat,
|
||||
pos_sp_triplet->previous.lon);
|
||||
}
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::altitude_sp_foh_update()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
/* Don't change setpoint if last and current waypoint are not valid */
|
||||
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
|
||||
!isfinite(_mission_item_previous_alt)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
|
||||
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
|
||||
* and the FW controller has a custom landing logic */
|
||||
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
/* Calculate distance to current waypoint */
|
||||
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
|
||||
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
|
||||
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
|
||||
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
|
||||
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
|
||||
_distance_current_previous);
|
||||
|
||||
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
|
||||
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
|
||||
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
|
||||
pos_sp_triplet->current.alt = _mission_item.altitude;
|
||||
} else {
|
||||
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
|
||||
* of the mission item as it is used to check if the mission item is reached
|
||||
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
|
||||
* radius around the current waypoint
|
||||
**/
|
||||
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
|
||||
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
|
||||
float a = _mission_item_previous_alt - grad * _distance_current_previous;
|
||||
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
|
||||
|
||||
}
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::altitude_sp_foh_reset()
|
||||
{
|
||||
_min_current_sp_distance_xy = FLT_MAX;
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
|
||||
{
|
||||
@@ -584,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();
|
||||
}
|
||||
|
||||
@@ -36,6 +36,8 @@
|
||||
* Navigator mode to access missions
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
@@ -75,6 +77,11 @@ public:
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
enum mission_altitude_mode {
|
||||
MISSION_ALTMODE_ZOH = 0,
|
||||
MISSION_ALTMODE_FOH = 1
|
||||
};
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update onboard mission topic
|
||||
@@ -102,6 +109,16 @@ private:
|
||||
*/
|
||||
void set_mission_items();
|
||||
|
||||
/**
|
||||
* Updates the altitude sp to follow a foh
|
||||
*/
|
||||
void altitude_sp_foh_update();
|
||||
|
||||
/**
|
||||
* Resets the altitude sp foh logic
|
||||
*/
|
||||
void altitude_sp_foh_reset();
|
||||
|
||||
/**
|
||||
* Read current or next mission item from the dataman and watch out for DO_JUMPS
|
||||
* @return true if successful
|
||||
@@ -114,39 +131,32 @@ 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;
|
||||
control::BlockParamFloat _param_dist_1wp;
|
||||
control::BlockParamInt _param_altmode;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
struct mission_s _offboard_mission;
|
||||
|
||||
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,
|
||||
@@ -157,7 +167,13 @@ private:
|
||||
bool _inited;
|
||||
bool _dist_1wp_ok;
|
||||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
|
||||
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
|
||||
float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
|
||||
can be replaced by a full copy of the previous mission item if needed*/
|
||||
float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
|
||||
only use if current and previous are valid */
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
|
||||
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
|
||||
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
|
||||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
|
||||
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
|
||||
* coordinates with a radius equal to the loiter_radius field. It is not flying
|
||||
* through the waypoint center.
|
||||
* Therefore the item is marked as reached once the system reaches the loiter
|
||||
* radius (+ some margin). Time inside and turn count is handled elsewhere.
|
||||
*/
|
||||
if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else {
|
||||
/* for normal mission items used their acceptance radius */
|
||||
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
|
||||
|
||||
@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
{
|
||||
|
||||
return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
|
||||
updateNavigationCapabilities();
|
||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||
|
||||
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resLanding && resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
@@ -109,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;
|
||||
}
|
||||
@@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
||||
return false;
|
||||
/* No landing waypoints or no waypoints */
|
||||
return true;
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||
|
||||
@@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
|
||||
|
||||
/**
|
||||
* Altitude setpoint mode
|
||||
*
|
||||
* 0: the system will follow a zero order hold altitude setpoint
|
||||
* 1: the system will follow a first order hold altitude setpoint
|
||||
* values follow the definition in enum mission_altitude_mode
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
|
||||
|
||||
@@ -48,7 +48,14 @@ SRCS = navigator_main.cpp \
|
||||
rtl_params.c \
|
||||
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
|
||||
|
||||
|
||||
@@ -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,19 +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 "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 3
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 7
|
||||
|
||||
class Navigator : public control::SuperBlock
|
||||
{
|
||||
@@ -99,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
|
||||
*/
|
||||
@@ -111,8 +129,13 @@ 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; }
|
||||
Geofence& get_geofence() { return _geofence; }
|
||||
@@ -129,6 +152,8 @@ 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 */
|
||||
@@ -138,15 +163,24 @@ private:
|
||||
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 */
|
||||
@@ -154,13 +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 */
|
||||
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 */
|
||||
|
||||
@@ -169,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
|
||||
*/
|
||||
|
||||
@@ -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,6 +68,7 @@
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
@@ -84,6 +86,7 @@
|
||||
*/
|
||||
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
|
||||
|
||||
#define GEOFENCE_CHECK_INTERVAL 200000
|
||||
|
||||
namespace navigator
|
||||
{
|
||||
@@ -97,6 +100,7 @@ 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),
|
||||
@@ -105,32 +109,47 @@ Navigator::Navigator() :
|
||||
_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"),
|
||||
_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] = &_dataLinkLoss;
|
||||
_navigation_mode_array[4] = &_engineFailure;
|
||||
_navigation_mode_array[5] = &_gpsFailure;
|
||||
_navigation_mode_array[6] = &_rcLoss;
|
||||
|
||||
updateParams();
|
||||
}
|
||||
@@ -166,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()
|
||||
{
|
||||
@@ -217,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
|
||||
@@ -228,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
|
||||
@@ -236,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));
|
||||
@@ -248,6 +283,8 @@ Navigator::task_main()
|
||||
vehicle_status_update();
|
||||
vehicle_control_mode_update();
|
||||
global_position_update();
|
||||
gps_position_update();
|
||||
sensor_combined_update();
|
||||
home_position_update();
|
||||
navigation_capabilities_update();
|
||||
params_update();
|
||||
@@ -259,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;
|
||||
@@ -274,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) {
|
||||
|
||||
@@ -298,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();
|
||||
@@ -327,9 +383,23 @@ Navigator::task_main()
|
||||
/* global position updated */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
global_position_update();
|
||||
static int gposcounter = 0;
|
||||
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
|
||||
have_geofence_position_data = true;
|
||||
}
|
||||
gposcounter++;
|
||||
}
|
||||
|
||||
/* 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) {
|
||||
@@ -337,6 +407,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;
|
||||
}
|
||||
@@ -360,11 +433,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_AUTO_LANDENGFAIL:
|
||||
_navigation_mode = &_engineFailure;
|
||||
break;
|
||||
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
|
||||
_navigation_mode = &_gpsFailure;
|
||||
break;
|
||||
default:
|
||||
_navigation_mode = nullptr;
|
||||
@@ -435,7 +527,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");
|
||||
@@ -443,7 +535,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");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -527,3 +619,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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -0,0 +1,182 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 rcloss.cpp
|
||||
* Helper class for RC Loss Mode according to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "datalinkloss.h"
|
||||
|
||||
#define DELAY_SIGMA 0.01f
|
||||
|
||||
RCLoss::RCLoss(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_param_loitertime(this, "LT"),
|
||||
_rcl_state(RCL_STATE_NONE)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
RCLoss::~RCLoss()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::on_inactive()
|
||||
{
|
||||
/* reset RCL state only if setpoint moved */
|
||||
if (!_navigator->get_can_loiter_at_sp()) {
|
||||
_rcl_state = RCL_STATE_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::on_activation()
|
||||
{
|
||||
_rcl_state = RCL_STATE_NONE;
|
||||
updateParams();
|
||||
advance_rcl();
|
||||
set_rcl_item();
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
updateParams();
|
||||
advance_rcl();
|
||||
set_rcl_item();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::set_rcl_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
set_previous_pos_setpoint();
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_rcl_state) {
|
||||
case RCL_STATE_LOITER: {
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
break;
|
||||
}
|
||||
case RCL_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
warnx("rc not recovered: request flight termination");
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = false;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::advance_rcl()
|
||||
{
|
||||
switch (_rcl_state) {
|
||||
case RCL_STATE_NONE:
|
||||
if (_param_loitertime.get() > 0.0f) {
|
||||
warnx("RC loss, OBC mode, loiter");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
|
||||
_rcl_state = RCL_STATE_LOITER;
|
||||
} else {
|
||||
warnx("RC loss, OBC mode, slip loiter, terminate");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
|
||||
_rcl_state = RCL_STATE_TERMINATE;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
}
|
||||
break;
|
||||
case RCL_STATE_LOITER:
|
||||
_rcl_state = RCL_STATE_TERMINATE;
|
||||
warnx("time is up, no RC regain, terminating");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
break;
|
||||
case RCL_STATE_TERMINATE:
|
||||
warnx("rcl end");
|
||||
_rcl_state = RCL_STATE_END;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-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 rcloss.h
|
||||
* Helper class for RC Loss Mode acording to the OBC rules
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_RCLOSS_H
|
||||
#define NAVIGATOR_RCLOSS_H
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class RCLoss : public MissionBlock
|
||||
{
|
||||
public:
|
||||
RCLoss(Navigator *navigator, const char *name);
|
||||
|
||||
~RCLoss();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
|
||||
private:
|
||||
/* Params */
|
||||
control::BlockParamFloat _param_loitertime;
|
||||
|
||||
enum RCLState {
|
||||
RCL_STATE_NONE = 0,
|
||||
RCL_STATE_LOITER = 1,
|
||||
RCL_STATE_TERMINATE = 2,
|
||||
RCL_STATE_END = 3
|
||||
} _rcl_state;
|
||||
|
||||
/**
|
||||
* Set the RCL item
|
||||
*/
|
||||
void set_rcl_item();
|
||||
|
||||
/**
|
||||
* Move to next RCL item
|
||||
*/
|
||||
void advance_rcl();
|
||||
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,60 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 rcloss_params.c
|
||||
*
|
||||
* Parameters for RC Loss (OBC)
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* OBC RC Loss mode parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Loiter Time
|
||||
*
|
||||
* The amount of time in seconds the system should loiter at current position before termination
|
||||
* Set to -1 to make the system skip loitering
|
||||
*
|
||||
* @unit seconds
|
||||
* @min -1.0
|
||||
* @group RCL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
|
||||
@@ -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();
|
||||
|
||||
@@ -58,7 +58,7 @@ extern "C" {
|
||||
/*
|
||||
* Maximum interval in us before FMU signal is considered lost
|
||||
*/
|
||||
#define FMU_INPUT_DROP_LIMIT_US 200000
|
||||
#define FMU_INPUT_DROP_LIMIT_US 500000
|
||||
|
||||
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
|
||||
#define ROLL 0
|
||||
@@ -98,7 +98,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 +110,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 +143,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 +160,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 +214,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.
|
||||
*/
|
||||
|
||||
@@ -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,7 @@
|
||||
#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_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 +181,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 */
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -285,7 +286,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
||||
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_servos[offset] = *values;
|
||||
if (*values != PWM_IGNORE_THIS_CHANNEL) {
|
||||
r_page_servos[offset] = *values;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
@@ -516,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;
|
||||
|
||||
@@ -76,6 +76,7 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/satellite_info.h>
|
||||
#include <uORB/topics/vehicle_vicon_position.h>
|
||||
#include <uORB/topics/vision_position_estimate.h>
|
||||
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
@@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct vehicle_global_position_s global_pos;
|
||||
struct position_setpoint_triplet_s triplet;
|
||||
struct vehicle_vicon_position_s vicon_pos;
|
||||
struct vision_position_estimate vision_pos;
|
||||
struct optical_flow_s flow;
|
||||
struct rc_channels_s rc;
|
||||
struct differential_pressure_s diff_pres;
|
||||
@@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_EST1_s log_EST1;
|
||||
struct log_PWR_s log_PWR;
|
||||
struct log_VICN_s log_VICN;
|
||||
struct log_VISN_s log_VISN;
|
||||
struct log_GS0A_s log_GS0A;
|
||||
struct log_GS0B_s log_GS0B;
|
||||
struct log_GS1A_s log_GS1A;
|
||||
@@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
int gps_pos_sub;
|
||||
int sat_info_sub;
|
||||
int vicon_pos_sub;
|
||||
int vision_pos_sub;
|
||||
int flow_sub;
|
||||
int rc_sub;
|
||||
int airspeed_sub;
|
||||
@@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
|
||||
subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
|
||||
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
|
||||
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
@@ -1427,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
|
||||
log_msg.body.log_GPOS.eph = buf.global_pos.eph;
|
||||
log_msg.body.log_GPOS.epv = buf.global_pos.epv;
|
||||
if (buf.global_pos.terrain_alt_valid) {
|
||||
log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt;
|
||||
} else {
|
||||
log_msg.body.log_GPOS.terrain_alt = -1.0f;
|
||||
}
|
||||
LOGBUFFER_WRITE_AND_COUNT(GPOS);
|
||||
}
|
||||
|
||||
@@ -1460,6 +1470,22 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
LOGBUFFER_WRITE_AND_COUNT(VICN);
|
||||
}
|
||||
|
||||
/* --- VISION POSITION --- */
|
||||
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
|
||||
log_msg.msg_type = LOG_VISN_MSG;
|
||||
log_msg.body.log_VISN.x = buf.vision_pos.x;
|
||||
log_msg.body.log_VISN.y = buf.vision_pos.y;
|
||||
log_msg.body.log_VISN.z = buf.vision_pos.z;
|
||||
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
|
||||
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
|
||||
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
|
||||
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
|
||||
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
|
||||
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
|
||||
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
|
||||
LOGBUFFER_WRITE_AND_COUNT(VISN);
|
||||
}
|
||||
|
||||
/* --- FLOW --- */
|
||||
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
|
||||
log_msg.msg_type = LOG_FLOW_MSG;
|
||||
|
||||
@@ -220,6 +220,7 @@ struct log_GPOS_s {
|
||||
float vel_d;
|
||||
float eph;
|
||||
float epv;
|
||||
float terrain_alt;
|
||||
};
|
||||
|
||||
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
|
||||
@@ -391,6 +392,20 @@ struct log_TEL_s {
|
||||
uint64_t heartbeat_time;
|
||||
};
|
||||
|
||||
/* --- VISN - VISION POSITION --- */
|
||||
#define LOG_VISN_MSG 38
|
||||
struct log_VISN_s {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float vx;
|
||||
float vy;
|
||||
float vz;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float qw;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
@@ -435,7 +450,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
|
||||
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(GVSP, "fff", "VX,VY,VZ"),
|
||||
@@ -449,6 +464,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
|
||||
LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),
|
||||
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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>
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
|
||||
v = ¶m_info_base[param].val;
|
||||
}
|
||||
|
||||
if (param_type(param) == PARAM_TYPE_STRUCT) {
|
||||
if (param_type(param) >= PARAM_TYPE_STRUCT
|
||||
&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
|
||||
result = v->p;
|
||||
|
||||
} else {
|
||||
|
||||
@@ -307,7 +307,7 @@ __EXPORT int param_load_default(void);
|
||||
struct param_info_s __param__##_name = { \
|
||||
#_name, \
|
||||
PARAM_TYPE_STRUCT + sizeof(_default), \
|
||||
.val.p = &_default; \
|
||||
.val.p = &_default \
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -192,6 +192,9 @@ 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);
|
||||
|
||||
@@ -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 */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 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 multirotor_motor_limits.h
|
||||
*
|
||||
* Definition of multirotor_motor_limits topic
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_MOTOR_LIMITS_H_
|
||||
#define MULTIROTOR_MOTOR_LIMITS_H_
|
||||
|
||||
#include "../uORB.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Motor limits
|
||||
*/
|
||||
struct multirotor_motor_limits_s {
|
||||
uint8_t roll_pitch : 1; // roll/pitch limit reached
|
||||
uint8_t yaw : 1; // yaw limit reached
|
||||
uint8_t throttle_lower : 1; // lower throttle limit reached
|
||||
uint8_t throttle_upper : 1; // upper throttle limit reached
|
||||
uint8_t reserved : 4;
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(multirotor_motor_limits);
|
||||
|
||||
#endif
|
||||
@@ -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 */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* 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
|
||||
@@ -37,6 +34,10 @@
|
||||
/**
|
||||
* @file vehicle_command.h
|
||||
* Definition of the vehicle command uORB topic.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_VEHICLE_COMMAND_H_
|
||||
@@ -52,6 +53,9 @@
|
||||
* but can contain additional ones.
|
||||
*/
|
||||
enum VEHICLE_CMD {
|
||||
VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
|
||||
VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
|
||||
VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
|
||||
VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */
|
||||
VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
|
||||
VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */
|
||||
@@ -90,7 +94,8 @@ enum VEHICLE_CMD {
|
||||
VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */
|
||||
VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */
|
||||
VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */
|
||||
VEHICLE_CMD_ENUM_END = 501, /* | */
|
||||
VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
|
||||
VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -118,8 +123,8 @@ struct vehicle_command_s {
|
||||
float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */
|
||||
enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
|
||||
uint8_t target_system; /**< System which should execute the command */
|
||||
|
||||
@@ -72,6 +72,8 @@ struct vehicle_global_position_s {
|
||||
float yaw; /**< Yaw in radians -PI..+PI. */
|
||||
float eph; /**< Standard deviation of position estimate horizontally */
|
||||
float epv; /**< Standard deviation of position vertically */
|
||||
float terrain_alt; /**< Terrain altitude in m, WGS84 */
|
||||
bool terrain_alt_valid; /**< Terrain altitude estimate is valid */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -102,7 +102,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) */
|
||||
@@ -198,9 +201,18 @@ 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;
|
||||
@@ -229,6 +241,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;
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -43,8 +43,6 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define MM_PER_CM 10 // Millimeters per centimeter
|
||||
|
||||
const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
@@ -70,6 +68,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
|
||||
return (_receiver_node_id < 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::print_status() const
|
||||
{
|
||||
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
|
||||
if (_receiver_node_id < 0) {
|
||||
printf("N/A\n");
|
||||
} else {
|
||||
printf("%d\n", _receiver_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
|
||||
{
|
||||
// This bridge does not support redundant GNSS receivers yet.
|
||||
@@ -85,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
|
||||
auto report = ::vehicle_gps_position_s();
|
||||
|
||||
report.timestamp_position = hrt_absolute_time();
|
||||
report.lat = msg.lat_1e7;
|
||||
report.lon = msg.lon_1e7;
|
||||
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
|
||||
report.lat = msg.latitude_deg_1e8 / 10;
|
||||
report.lon = msg.longitude_deg_1e8 / 10;
|
||||
report.alt = msg.height_msl_mm;
|
||||
|
||||
report.timestamp_variance = report.timestamp_position;
|
||||
|
||||
|
||||
@@ -66,6 +66,8 @@ public:
|
||||
|
||||
unsigned get_num_redundant_channels() const override;
|
||||
|
||||
void print_status() const override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* GNSS fix message will be reported via this callback.
|
||||
|
||||
@@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
|
||||
{
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].redunancy_channel_id >= 0) {
|
||||
if (_channels[i].node_id >= 0) {
|
||||
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
|
||||
}
|
||||
}
|
||||
@@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
|
||||
delete [] _channels;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report)
|
||||
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
{
|
||||
assert(report != nullptr);
|
||||
|
||||
Channel *channel = nullptr;
|
||||
|
||||
// Checking if such channel already exists
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].redunancy_channel_id == redundancy_channel_id) {
|
||||
if (_channels[i].node_id == node_id) {
|
||||
channel = _channels + i;
|
||||
break;
|
||||
}
|
||||
@@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
|
||||
return; // Give up immediately - saves some CPU time
|
||||
}
|
||||
|
||||
log("adding channel %d...", redundancy_channel_id);
|
||||
log("adding channel %d...", node_id);
|
||||
|
||||
// Search for the first free channel
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].redunancy_channel_id < 0) {
|
||||
if (_channels[i].node_id < 0) {
|
||||
channel = _channels + i;
|
||||
break;
|
||||
}
|
||||
@@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
|
||||
}
|
||||
|
||||
// Publish to the appropriate topic, abort on failure
|
||||
channel->orb_id = _orb_topics[class_instance];
|
||||
channel->redunancy_channel_id = redundancy_channel_id;
|
||||
channel->class_instance = class_instance;
|
||||
channel->orb_id = _orb_topics[class_instance];
|
||||
channel->node_id = node_id;
|
||||
channel->class_instance = class_instance;
|
||||
|
||||
channel->orb_advert = orb_advertise(channel->orb_id, report);
|
||||
if (channel->orb_advert < 0) {
|
||||
@@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const
|
||||
return;
|
||||
}
|
||||
|
||||
log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance);
|
||||
log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
|
||||
}
|
||||
assert(channel != nullptr);
|
||||
|
||||
@@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
|
||||
{
|
||||
unsigned out = 0;
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].redunancy_channel_id >= 0) {
|
||||
if (_channels[i].node_id >= 0) {
|
||||
out += 1;
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::print_status() const
|
||||
{
|
||||
printf("devname: %s\n", _class_devname);
|
||||
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].node_id >= 0) {
|
||||
printf("channel %d: node id %d --> class instance %d\n",
|
||||
i, _channels[i].node_id, _channels[i].class_instance);
|
||||
} else {
|
||||
printf("channel %d: empty\n", i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -68,6 +68,11 @@ public:
|
||||
*/
|
||||
virtual unsigned get_num_redundant_channels() const = 0;
|
||||
|
||||
/**
|
||||
* Prints current status in a human readable format to stdout.
|
||||
*/
|
||||
virtual void print_status() const = 0;
|
||||
|
||||
/**
|
||||
* Sensor bridge factory.
|
||||
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
|
||||
@@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
|
||||
{
|
||||
struct Channel
|
||||
{
|
||||
int redunancy_channel_id = -1;
|
||||
int node_id = -1;
|
||||
orb_id_t orb_id = nullptr;
|
||||
orb_advert_t orb_advert = -1;
|
||||
int class_instance = -1;
|
||||
@@ -112,13 +117,15 @@ protected:
|
||||
/**
|
||||
* Sends one measurement into appropriate ORB topic.
|
||||
* New redundancy channels will be registered automatically.
|
||||
* @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID)
|
||||
* @param report ORB message object
|
||||
* @param node_id Sensor's Node ID
|
||||
* @param report Pointer to ORB message object
|
||||
*/
|
||||
void publish(const int redundancy_channel_id, const void *report);
|
||||
void publish(const int node_id, const void *report);
|
||||
|
||||
public:
|
||||
virtual ~UavcanCDevSensorBridgeBase();
|
||||
|
||||
unsigned get_num_redundant_channels() const override;
|
||||
|
||||
void print_status() const override;
|
||||
};
|
||||
|
||||
@@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
(void)adjustment;
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt();
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return 0;
|
||||
|
||||
@@ -41,6 +41,7 @@
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/board_serial.h>
|
||||
#include <systemlib/scheduling_priorities.h>
|
||||
#include <version/version.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <arch/chip/chip.h>
|
||||
@@ -175,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
* Start the task. Normally it should never exit.
|
||||
*/
|
||||
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
|
||||
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
|
||||
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
|
||||
static_cast<main_t>(run_trampoline), nullptr);
|
||||
|
||||
if (_instance->_task < 0) {
|
||||
@@ -548,14 +549,16 @@ UavcanNode::print_info()
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
// ESC mixer status
|
||||
warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u",
|
||||
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
|
||||
warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
|
||||
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
|
||||
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
|
||||
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
|
||||
|
||||
// Sensor bridges
|
||||
auto br = _sensor_bridges.getHead();
|
||||
while (br != nullptr) {
|
||||
warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels());
|
||||
printf("Sensor '%s':\n", br->get_name());
|
||||
br->print_status();
|
||||
printf("\n");
|
||||
br = br->getSibling();
|
||||
}
|
||||
|
||||
|
||||
@@ -36,7 +36,11 @@
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
UnitTest::UnitTest()
|
||||
UnitTest::UnitTest() :
|
||||
_tests_run(0),
|
||||
_tests_failed(0),
|
||||
_tests_passed(0),
|
||||
_assertions(0)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -44,15 +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);
|
||||
}
|
||||
|
||||
/// @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);
|
||||
}
|
||||
|
||||
@@ -37,50 +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 void runTests(void) = 0;
|
||||
void printResults(void);
|
||||
|
||||
void printAssert(const char* msg, const char* test, 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_run_test(test) \
|
||||
do { \
|
||||
warnx("RUNNING TEST: %s", #test); \
|
||||
mu_tests_run()++; \
|
||||
if (!test()) { \
|
||||
warnx("TEST FAILED: %s", #test); \
|
||||
mu_tests_failed()++; \
|
||||
} else { \
|
||||
warnx("TEST PASSED: %s", #test); \
|
||||
mu_tests_passed()++; \
|
||||
} \
|
||||
} while (0)
|
||||
virtual ~UnitTest();
|
||||
|
||||
/// @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)
|
||||
|
||||
/// @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_ */
|
||||
|
||||
Reference in New Issue
Block a user