diff options
Diffstat (limited to 'src/modules')
47 files changed, 1978 insertions, 1444 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 83145ac72..668bac5d9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -314,7 +314,7 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.valid = true; + _pos.global_valid = true; _pos.lat = lat * M_RAD_TO_DEG; _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66ec20b95..620185fb7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 44f47b47c..4154e3db4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); -PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52a..c039b8573 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,11 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static struct vehicle_status_s status; +static struct actuator_armed_s armed; +static struct safety_s safety; +static struct vehicle_control_mode_s control_mode; + /* tasks waiting for low prio thread */ typedef enum { LOW_PRIO_TASK_NONE = 0, @@ -203,12 +208,19 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); -void print_reject_mode(const char *msg); +void set_control_mode(); + +void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); void print_status(); +int arm(); +int disarm(); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -274,6 +286,16 @@ int commander_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "arm")) { + arm(); + exit(0); + } + + if (!strcmp(argv[1], "disarm")) { + disarm(); + exit(0); + } + usage("unrecognized command"); exit(1); } @@ -340,6 +362,32 @@ void print_status() static orb_advert_t status_pub; +int arm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + + } else { + return 1; + } +} + +int disarm() +{ + int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); + return 0; + + } else { + return 1; + } +} + bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -554,13 +602,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } -static struct vehicle_status_s status; - -/* armed topic */ -static struct actuator_armed_s armed; - -static struct safety_s safety; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -613,16 +654,11 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - + // We want to accept RC inputs as default + status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +681,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -872,7 +914,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -886,10 +928,12 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + static bool published_condition_landed_fw = false; if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -898,6 +942,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { + if (!published_condition_landed_fw) { + status.condition_landed = false; // Fixedwing does not have a landing detector currently + published_condition_landed_fw = true; + status_changed = true; + } } /* update battery status */ @@ -1031,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { + && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; @@ -1056,7 +1106,7 @@ int commander_thread_main(int argc, char *argv[]) } /* start RC input check */ - if (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 + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1244,8 +1294,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1422,7 +1477,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { - warnx("mode sw not finite"); + /* default to manual if signal is invalid */ status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { @@ -1472,7 +1527,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1489,7 +1544,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this state // else fallback to SEATBELT - print_reject_mode("EASY"); + print_reject_mode(status, "EASY"); } res = main_state_transition(status, MAIN_STATE_SEATBELT); @@ -1498,7 +1553,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this mode if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + print_reject_mode(status, "SEATBELT"); // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); @@ -1512,7 +1567,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); + print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) @@ -1531,16 +1586,122 @@ set_main_state_rc(struct vehicle_status_s *status) } void -print_reject_mode(const char *msg) + +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + 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 = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + 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 = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + +void +print_reject_mode(struct vehicle_status_s *status, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "#audio: warning: reject %s", msg); + sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + + // only buzz if armed, because else we're driving people nuts indoors + // they really need to look at the leds as well. + if (status->arming_state == ARMING_STATE_ARMED) { + tune_negative(); + } else { + + // Always show the led indication + led_negative(); + } } } @@ -1688,7 +1849,15 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_rc_calibration(mavlink_fd); + /* disable RC control input completely */ + status.rc_input_blocked = true; + calib_ret = OK; + mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1699,6 +1868,18 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status.rc_input_blocked) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* enable RC control input */ + status.rc_input_blocked = false; + mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + } + + /* this always succeeds */ + calib_ret = OK; + } if (calib_ret == OK) @@ -1757,6 +1938,10 @@ void *commander_low_prio_loop(void *arg) break; } + case VEHICLE_CMD_START_RX_PAIR: + /* handled in the IO driver */ + break; + default: /* don't answer on unsupported commands, it will be done in main loop */ break; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 21a1c4c2c..033e7dc88 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -124,10 +124,15 @@ void tune_neutral() void tune_negative() { + led_negative(); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); +} + +void led_negative() +{ blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } int tune_arm() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d0393f45a..af25a5e97 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,9 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); + +void led_negative(); + int blink_msg_state(); int led_init(void); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 90ede499a..41f3ca0aa 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -53,17 +53,16 @@ #endif static const int ERROR = -1; -int do_rc_calibration(int mavlink_fd) +int do_trim_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "trim calibration starting"); - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + usleep(200000); struct manual_control_setpoint_s sp; bool changed; orb_check(sub_man, &changed); if (!changed) { - mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + mavlink_log_critical(mavlink_fd, "no inputs, aborting"); return ERROR; } @@ -82,12 +81,12 @@ int do_rc_calibration(int mavlink_fd) int save_ret = param_save_default(); if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL"); close(sub_man); return ERROR; } - mavlink_log_info(mavlink_fd, "trim calibration done"); + mavlink_log_info(mavlink_fd, "trim cal done"); close(sub_man); return OK; } diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 9aa6faafa..45efedf55 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include <stdint.h> -int do_rc_calibration(int mavlink_fd); +int do_trim_calibration(int mavlink_fd); #endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a..31955d3e5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include <unistd.h> #include <stdint.h> #include <stdbool.h> +#include <dirent.h> +#include <fcntl.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -50,6 +52,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" @@ -332,6 +335,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + int sensfd = ::open(direntry->d_name, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; @@ -382,16 +412,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dc2d6c312..fa88dfaff 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman"; /* The data manager work queues */ typedef struct { - sq_queue_t q; - sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; - unsigned max_size; + sq_queue_t q; /* Nuttx queue */ + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; /* Current size of queue */ + unsigned max_size; /* Maximum queue size reached */ } work_q_t; -static work_q_t g_free_q; -static work_q_t g_work_q; +static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ +static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ -sem_t g_work_queued_sema; +sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -#define DM_SECTOR_HDR_SIZE 4 -static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; +#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); - sem_init(&(q->mutex), 1, 1); - q->size = q->max_size = 0; + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + q->size = q->max_size = 0; /* Queue is initially empty */ } -static void destroy_q(work_q_t *q) +static inline void +destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void lock_queue(work_q_t *q) { - sem_wait(&(q->mutex)); + sem_wait(&(q->mutex)); /* Acquire the queue lock */ } static inline void unlock_queue(work_q_t *q) { - sem_post(&(q->mutex)); + sem_post(&(q->mutex)); /* Release the queue lock */ } static work_q_item_t * @@ -172,54 +173,47 @@ create_work_item(void) { work_q_item_t *item; + /* Try to reuse item from free item queue */ lock_queue(&g_free_q); if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) g_free_q.size--; unlock_queue(&g_free_q); + /* If we there weren't any free items then obtain memory for a new one */ if (item == NULL) item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we got one then lock the item*/ if (item) sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + /* return the item pointer, or NULL if all failed */ return item; } /* Work queue management functions */ -static void -enqueue_work_item(work_q_item_t *item) -{ - /* put the work item on the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - - if (++g_work_q.size > g_work_q.max_size) - g_work_q.max_size = g_work_q.size; - - unlock_queue(&g_work_q); - /* tell the work thread that work is available */ - sem_post(&g_work_queued_sema); -} - -static void +static inline void destroy_work_item(work_q_item_t *item) { - sem_destroy(&item->wait_sem); + sem_destroy(&item->wait_sem); /* Destroy the item lock */ + /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); + /* Update the queue size and potentially the maximum queue size */ if (++g_free_q.size > g_free_q.max_size) g_free_q.max_size = g_free_q.size; unlock_queue(&g_free_q); } -static work_q_item_t * +static inline work_q_item_t * dequeue_work_item(void) { work_q_item_t *work; + + /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) @@ -229,6 +223,32 @@ dequeue_work_item(void) return work; } +static int +enqueue_work_item_and_wait_for_result(work_q_item_t *item) +{ + /* put the work item at the end of the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + /* Adjust the queue size and potentially the maximum queue size */ + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); + + /* wait for the result */ + sem_wait(&item->wait_sem); + + int result = item->result; + + destroy_work_item(item); + + return result; +} + /* Calculate the offset in file of specific item */ static int calculate_offset(dm_item_t item, unsigned char index) @@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index) * * byte 0: Length of user data item * byte 1: Persistence of this data item + * byte 2: Unused (for future use) + * byte 3: Unused (for future use) * byte DM_SECTOR_HDR_SIZE... : data item value * * The total size must not exceed k_sector_size @@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v len = -1; + /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); + fsync(g_task_fd); /* Make sure data is written to physical media */ + /* Make sure the write succeeded */ if (len != count) return -1; @@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) if (lseek(g_task_fd, offset, SEEK_SET) == offset) len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); - /* Check for length issues */ + /* Check for read error */ if (len < 0) return -1; + /* A zero length entry is a empty entry */ if (len == 0) buffer[0] = 0; + /* See if we got data */ if (buffer[0] > 0) { + /* We got more than requested!!! */ if (buffer[0] > count) return -1; @@ -340,11 +369,14 @@ _clear(dm_item_t item) { int i, result = 0; + /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); + /* Check for item type out of range */ if (offset < 0) return -1; + /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { char buf[1]; @@ -353,9 +385,11 @@ _clear(dm_item_t item) break; } + /* Avoid SD flash wear by only doing writes where necessary */ if (read(g_task_fd, buf, 1) < 1) break; + /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { if (lseek(g_task_fd, offset, SEEK_SET) != offset) { result = -1; @@ -373,6 +407,7 @@ _clear(dm_item_t item) offset += k_sector_size; } + /* Make sure data is actually written to physical media */ fsync(g_task_fd); return result; } @@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a write request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_write_func; work->write_params.item = item; @@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work->write_params.persistence = persistence; work->write_params.buf = buf; work->write_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } /* Retrieve from the data manager file */ @@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a read request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_read_func; work->read_params.item = item; work->read_params.index = index; work->read_params.buf = buf; work->read_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } __EXPORT int @@ -504,21 +535,19 @@ dm_clear(dm_item_t item) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a clear request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_clear_func; work->clear_params.item = item; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } /* Tell the data manager about the type of the last reset */ @@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a restart request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_restart_func; work->restart_params.reason = reason; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } static int @@ -570,24 +597,29 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); + /* We use two file descriptors, one for the caller context and one for the worker thread */ + /* They are actually the same but we need to some way to reject caller request while the */ + /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ @@ -595,7 +627,7 @@ task_main(int argc, char *argv[]) /* do we need to exit ??? */ if ((g_task_should_exit) && (g_fd >= 0)) { - /* Close the file handle to stop further queueing */ + /* Close the file handle to stop further queuing */ g_fd = -1; } @@ -607,6 +639,7 @@ task_main(int argc, char *argv[]) /* Empty the work queue */ while ((work = dequeue_work_item())) { + /* handle each work item with the appropriate handler */ switch (work->func) { case dm_write_func: g_func_counts[dm_write_func]++; @@ -647,7 +680,7 @@ task_main(int argc, char *argv[]) close(g_task_fd); g_task_fd = -1; - /* Empty the work queue */ + /* The work queue is now empty, empty the free queue */ for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; @@ -669,7 +702,7 @@ start(void) sem_init(&g_init_sema, 1, 0); - /* start the task */ + /* start the worker thread */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { warn("task start failed"); return -1; @@ -704,7 +737,7 @@ stop(void) static void usage(void) { - errx(1, "usage: dataman {start|stop|status}"); + errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); } int @@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[]) exit(0); } + /* Worker thread should be running for all other commands */ if (g_fd < 0) errx(1, "not running"); @@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[]) stop(); else if (!strcmp(argv[1], "status")) status(); + else if (!strcmp(argv[1], "poweronrestart")) + dm_restart(DM_INIT_REASON_POWER_ON); + else if (!strcmp(argv[1], "inflightrestart")) + dm_restart(DM_INIT_REASON_IN_FLIGHT); else usage(); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a62b53221..45fdaa355 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -236,7 +236,7 @@ private: float land_slope_length; float land_H1_virt; float land_flare_alt_relative; - float land_thrust_lim_horizontal_distance; + float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -281,7 +281,7 @@ private: param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; - param_t land_thrust_lim_horizontal_distance; + param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); - _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); 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_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -785,26 +785,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; if (pos_sp_triplet.previous.valid) { - prev_wp(0) = pos_sp_triplet.previous.lat; - prev_wp(1) = pos_sp_triplet.previous.lon; + prev_wp(0) = (float)pos_sp_triplet.previous.lat; + prev_wp(1) = (float)pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = pos_sp_triplet.current.lat; - prev_wp(1) = pos_sp_triplet.current.lon; + prev_wp(0) = (float)pos_sp_triplet.current.lat; + prev_wp(1) = (float)pos_sp_triplet.current.lon; } @@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } @@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - - + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out @@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); + float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) @@ -1263,7 +1264,7 @@ FixedwingPositionControl::task_main() // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); - math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* * Attempt to control position, on success (= sensors present and not in manual mode), diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 7954d75c2..512ca7b8a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index b139a6397..e5f7023ae 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -48,13 +48,13 @@ void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt) { _landing_slope_angle_rad = landing_slope_angle_rad; _flare_relative_alt = flare_relative_alt; - _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _motor_lim_relative_alt = motor_lim_relative_alt; _H1_virt = H1_virt; calculateSlopeValues(); @@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) { - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + else + return wp_altitude; +} +float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + else + return wp_landing_altitude; } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 1a149fc7c..76d65a55f 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -49,7 +49,7 @@ private: /* see Documentation/fw_landing.png for a plot of the landing slope */ float _landing_slope_angle_rad; /**< phi in the plot */ float _flare_relative_alt; /**< h_flare,rel in the plot */ - float _motor_lim_horizontal_distance; + float _motor_lim_relative_alt; float _H1_virt; /**< H1 in the plot */ float _H0; /**< h_flare,rel + H1 in the plot */ float _d1; /**< d1 in the plot */ @@ -63,7 +63,18 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); /** * @@ -85,17 +96,17 @@ public: } - float getFlareCurveAltitude(float wp_distance, float wp_altitude); + float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} inline float flare_relative_alt() {return _flare_relative_alt;} - inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;} inline float H1_virt() {return _H1_virt;} inline float H0() {return _H0;} inline float d1() {return _d1;} diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index b143e62f0..2c7ca7525 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -56,7 +56,7 @@ // #include <stdlib.h> // #include <poll.h> -// +// // #include <systemlib/systemlib.h> // #include <systemlib/err.h> // #include <mavlink/mavlink_log.h> diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4f4d94947..2585593ea 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -61,7 +61,7 @@ #include <uORB/topics/home_position.h> #include <uORB/topics/mission_result.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> #include <systemlib/param/param.h> @@ -502,28 +502,36 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; union px4_custom_mode custom_mode; custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { + if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + if (pos_sp_triplet.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9b289c965..ce2d2b34a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -57,7 +57,7 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -233,7 +233,7 @@ public: int get_sub; int airspeed_sub; int navigation_capabilities_sub; - int control_mode_sub; + int position_setpoint_triplet_sub; int rc_sub; int status_sub; int home_sub; @@ -252,12 +252,12 @@ public: struct navigation_capabilities_s nav_cap; /** Vehicle status */ struct vehicle_status_s v_status; - /** Vehicle control mode */ - struct vehicle_control_mode_s control_mode; /** RC channels */ struct rc_channels_s rc; /** Actuator armed state */ struct actuator_armed_s armed; + /** Position setpoint triplet */ + struct position_setpoint_triplet_s pos_sp_triplet; protected: /** diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 418e1c121..cd9408f2f 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -94,7 +94,6 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); - add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0); add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); @@ -277,6 +276,7 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) /* immediately communicate state _mavlink->get_chan()ges back to user */ orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); + orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet)); /* enable or disable HIL */ if (l->listener->v_status.hil_state == HIL_STATE_ON) @@ -317,10 +317,10 @@ MavlinkOrbListener::l_input_rc(const struct listener *l) const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), - l->listener->rc_raw.timestamp / 1000, + l->listener->rc_raw.timestamp_publication / 1000, i, (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, @@ -644,26 +644,6 @@ MavlinkOrbListener::l_nav_cap(const struct listener *l) } -void -MavlinkOrbListener::l_control_mode(const struct listener *l) -{ - orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(l->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - void * MavlinkOrbListener::uorb_receive_thread(void *arg) { @@ -759,9 +739,9 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ - /* --- CONTROL MODE --- */ - mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- POSITION SETPOINT TRIPLET --- */ + mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */ /* --- RC CHANNELS VALUE --- */ mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 349a11f2e..560b47423 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -57,7 +57,6 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -158,13 +157,11 @@ private: static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); - static void l_control_mode(const struct listener *l); struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; - struct vehicle_control_mode_s control_mode; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 94d97fc12..89dcb1d7b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,7 +330,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub == 0) { + if (telemetry_status_pub <= 0) { telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -613,7 +613,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet hil_global_pos.timestamp = timestamp; - hil_global_pos.valid = true; + hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index be32ce0f7..d8d3b5452 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -57,7 +57,6 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> @@ -122,6 +121,7 @@ private: struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; struct battery_status_s hil_battery_status; + struct position_setpoint_triplet_s pos_sp_triplet; orb_advert_t pub_hil_global_pos; orb_advert_t pub_hil_local_pos; orb_advert_t pub_hil_attitude; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 245ac024b..db5e2e9bb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -85,7 +85,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f -#define YAW_DEADZONE 0.01f +#define YAW_DEADZONE 0.05f #define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl @@ -113,45 +113,66 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ + math::Matrix<3, 3> _R; /**< rotation matrix for current state */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { - param_t att_p; + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; param_t yaw_p; - param_t att_rate_p; - param_t att_rate_i; - param_t att_rate_d; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { - math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; } _params; /** @@ -160,9 +181,9 @@ private: int parameters_update(); /** - * Update control outputs + * Check for parameter update and handle it. */ - void control_update(); + void parameter_update_poll(); /** * Check for changes in vehicle control mode. @@ -175,9 +196,14 @@ private: void vehicle_manual_poll(); /** - * Check for set triplet updates. + * Check for attitude setpoint updates. */ - void vehicle_setpoint_poll(); + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); /** * Check for arming status updates. @@ -185,6 +211,16 @@ private: void arming_status_poll(); /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -195,7 +231,7 @@ private: void task_main() __attribute__((noreturn)); }; -namespace att_control +namespace mc_att_control { /* oddly, ERROR is not defined for c++ */ @@ -213,43 +249,58 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), - _att_sp_sub(-1), - _control_mode_sub(-1), + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), _params_sub(-1), - _manual_sub(-1), - _arming_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), /* publications */ _att_sp_pub(-1), - _rates_sp_pub(-1), + _v_rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) { - memset(&_att, 0, sizeof(_att)); - memset(&_att_sp, 0, sizeof(_att_sp)); - memset(&_manual, 0, sizeof(_manual)); - memset(&_control_mode, 0, sizeof(_control_mode)); - memset(&_arming, 0, sizeof(_arming)); + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); - _params.p.zero(); + _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _R_sp.identity(); + _R.identity(); _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); - _params_handles.att_p = param_find("MC_ATT_P"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); - _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + I.identity(); + + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -276,7 +327,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - att_control::g_control = nullptr; + mc_att_control::g_control = nullptr; } int @@ -284,421 +335,460 @@ MulticopterAttitudeControl::parameters_update() { float v; - param_get(_params_handles.att_p, &v); - _params.p(0) = v; - _params.p(1) = v; - param_get(_params_handles.yaw_p, &v); - _params.p(2) = v; - - param_get(_params_handles.att_rate_p, &v); + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; - - param_get(_params_handles.att_rate_i, &v); - _params.rate_i(0) = v; - _params.rate_i(1) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; - - param_get(_params_handles.att_rate_d, &v); - _params.rate_d(0) = v; - _params.rate_d(1) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + return OK; } void -MulticopterAttitudeControl::vehicle_control_mode_poll() +MulticopterAttitudeControl::parameter_update_poll() { - bool control_mode_updated; + bool updated; /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &control_mode_updated); + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; - if (control_mode_updated) { + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); } } void MulticopterAttitudeControl::vehicle_manual_poll() { - bool manual_updated; + bool updated; /* get pilots inputs */ - orb_check(_manual_sub, &manual_updated); + orb_check(_manual_control_sp_sub, &updated); - if (manual_updated) { + if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } void -MulticopterAttitudeControl::vehicle_setpoint_poll() +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated; + orb_check(_v_att_sp_sub, &updated); - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); } } void -MulticopterAttitudeControl::arming_status_poll() +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() { /* check if there is a new setpoint */ - bool arming_updated; - orb_check(_arming_sub, &arming_updated); + bool updated; + orb_check(_v_rates_sp_sub, &updated); - if (arming_updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); } } void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +MulticopterAttitudeControl::arming_status_poll() { - att_control::g_control->task_main(); + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } } +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ void -MulticopterAttitudeControl::task_main() +MulticopterAttitudeControl::control_attitude(float dt) { - /* inform about start */ - warnx("started"); - fflush(stdout); + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; - /* - * do subscriptions - */ - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ - /* rate limit attitude updates to 100Hz */ - orb_set_interval(_att_sub, 10); + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } - parameters_update(); + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } - /* initialize values of critical structs until first regular update */ - _arming.armed = false; + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - arming_status_poll(); + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - /* setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - R_sp.identity(); + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.identity(); + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; - /* current angular rates */ - math::Vector<3> rates; - rates.zero(); + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } - /* angular rates integral error */ - math::Vector<3> rates_int; - rates_int.zero(); + yaw_sp_move_rate *= _params.rc_scale_yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } - /* identity matrix */ - math::Matrix<3, 3> I; - I.identity(); + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } - math::Quaternion q; + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } - bool reset_yaw_sp = true; + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); - /* wakeup source(s) */ - struct pollfd fds[2]; + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; - fds[1].fd = _att_sub; - fds[1].events = POLLIN; + _thrust_sp = _v_att_sp.thrust; - while (!_task_should_exit) { + /* construct attitude setpoint rotation matrix */ + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + _R_sp.set(&_v_att_sp.R_body[0][0]); - /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); - perf_begin(_loop_perf); + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* copy the topic to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - parameters_update(); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } + } - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; - float dt = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + /* rotation matrix for current state */ + _R.set(_v_att.R); - /* guard against too large dt's */ - if (dt > 0.02f) - dt = 0.02f; + /* all input data is ready, run controller itself */ - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* calculate weight for yaw control */ + float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } + e_R = e_R_z_axis * e_R_z_angle; - } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); - if (_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_att_sp.R_body[0][0]); + /* rotation matrix for roll/pitch only rotation */ + R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + } else { + /* zero roll/pitch rotation */ + R_rp = _R; + } - /* copy rotation matrix back to setpoint struct */ - memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - } + /* R_rp and _R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(_R.transposed() * _R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } - if (publish_att_sp) { - /* publish the attitude setpoint */ - _att_sp.timestamp = hrt_absolute_time(); + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +MulticopterAttitudeControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; } } + } + } +} - /* rotation matrix for current state */ - R.set(_att.R); - - /* current body angular rates */ - rates(0) = _att.rollspeed; - rates(1) = _att.pitchspeed; - rates(2) = _att.yawspeed; +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + mc_att_control::g_control->task_main(); +} - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); +void +MulticopterAttitudeControl::task_main() +{ + warnx("started"); + fflush(stdout); - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + /* + * do subscriptions + */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; + /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ + orb_set_interval(_v_att_sub, 5); - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); + /* initialize parameters cache */ + parameters_update(); - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + fds[0].fd = _v_att_sub; + fds[0].events = POLLIN; - e_R = e_R_z_axis * e_R_z_angle; + while (!_task_should_exit) { - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } + perf_begin(_loop_perf); - /* angular rates setpoint*/ - math::Vector<3> rates_sp = _params.p.emult(e_R); + /* run controller on attitude changes */ + if (fds[0].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - /* feed forward yaw setpoint rate */ - rates_sp(2) += yaw_sp_move_rate * yaw_w; + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; - /* reset integral if disarmed */ - // TODO add LANDED flag here - if (!_arming.armed) { - rates_int.zero(); + } else if (dt > 0.02f) { + dt = 0.02f; } - /* rate controller */ - math::Vector<3> rates_err = rates_sp - rates; - math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; - _rates_prev = rates; + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - /* update integral */ - for (int i = 0; i < 3; i++) { - float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); - if (isfinite(rate_i)) { - if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { - rates_int(i) = rate_i; - } - } - } + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish the attitude rates setpoint */ - _rates_sp.roll = rates_sp(0); - _rates_sp.pitch = rates_sp(1); - _rates_sp.yaw = rates_sp(2); - _rates_sp.thrust = _att_sp.thrust; - _rates_sp.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } } else { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; } - /* publish the attitude controls */ - if (_control_mode.flag_control_rates_enabled) { - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); - } else { - /* controller disabled, publish zero attitude controls */ - _actuators.control[0] = 0.0f; - _actuators.control[1] = 0.0f; - _actuators.control[2] = 0.0f; - _actuators.control[3] = 0.0f; + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - } - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } @@ -739,17 +829,17 @@ int mc_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) errx(1, "already running"); - att_control::g_control = new MulticopterAttitudeControl; + mc_att_control::g_control = new MulticopterAttitudeControl; - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "alloc failed"); - if (OK != att_control::g_control->start()) { - delete att_control::g_control; - att_control::g_control = nullptr; + if (OK != mc_att_control::g_control->start()) { + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; err(1, "start failed"); } @@ -757,16 +847,16 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "not running"); - delete att_control::g_control; - att_control::g_control = nullptr; + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; exit(0); } if (!strcmp(argv[1], "status")) { - if (att_control::g_control) { + if (mc_att_control::g_control) { errx(0, "running"); } else { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a170365ee..27a45b6bb 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,13 +41,16 @@ #include <systemlib/param/param.h> -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d3e39e3a0..25d34c872 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -63,8 +63,7 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/param/param.h> @@ -109,6 +108,7 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -116,11 +116,11 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _local_pos_sub; /**< vehicle local position */ + int _global_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -128,8 +128,7 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ - struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct vehicle_global_position_s _global_pos; /**< vehicle global position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ @@ -154,7 +153,6 @@ private: param_t rc_scale_pitch; param_t rc_scale_roll; - param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -166,7 +164,6 @@ private: float rc_scale_pitch; float rc_scale_roll; - float rc_scale_yaw; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -177,9 +174,15 @@ private: math::Vector<3> sp_offs_max; } _params; - math::Vector<3> _pos; + double _lat_sp; + double _lon_sp; + float _alt_sp; + + bool _reset_lat_lon_sp; + bool _reset_alt_sp; + bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ + math::Vector<3> _vel; - math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -201,6 +204,21 @@ private: static float scale_control(float ctl, float end, float dz); /** + * Reset lat/lon to current position + */ + void reset_lat_lon_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -227,6 +245,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _task_should_exit(false), _control_task(-1), + _mavlink_fd(-1), /* subscriptions */ _att_sub(-1), @@ -235,21 +254,28 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _local_pos_sub(-1), + _global_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ - _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1) + _pos_sp_triplet_pub(-1), + _global_vel_sp_pub(-1), + + _lat_sp(0.0), + _lon_sp(0.0), + _alt_sp(0.0f), + + _reset_lat_lon_sp(true), + _reset_alt_sp(true), + _use_global_alt(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_global_pos, 0, sizeof(_global_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); @@ -261,9 +287,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); - _pos.zero(); _vel.zero(); - _pos_sp.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -286,7 +310,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(true); @@ -335,7 +358,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); float v; param_get(_params_handles.xy_p, &v); @@ -405,10 +427,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_pos_sp_triplet_sub, &updated); + orb_check(_global_pos_sub, &updated); if (updated) - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } float @@ -432,13 +454,50 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void +MulticopterPositionControl::reset_lat_lon_sp() +{ + if (_reset_lat_lon_sp) { + _reset_lat_lon_sp = false; + _lat_sp = _global_pos.lat; + _lon_sp = _global_pos.lon; + mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + } +} + +void +MulticopterPositionControl::select_alt(bool global) +{ + if (global != _use_global_alt) { + _use_global_alt = global; + + if (global) { + /* switch from barometric to global altitude */ + _alt_sp += _global_pos.alt - _global_pos.baro_alt; + + } else { + /* switch from global to barometric altitude */ + _alt_sp += _global_pos.baro_alt - _global_pos.alt; + } + } +} + +void MulticopterPositionControl::task_main() { warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions @@ -449,7 +508,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -460,8 +519,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_sp_z = true; - bool reset_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; @@ -472,11 +529,6 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - hrt_abstime ref_timestamp = 0; - int32_t ref_lat = 0.0f; - int32_t ref_lon = 0.0f; - float ref_alt = 0.0f; - math::Vector<3> sp_move_rate; sp_move_rate.zero(); math::Vector<3> thrust_int; @@ -488,7 +540,7 @@ MulticopterPositionControl::task_main() struct pollfd fds[1]; /* Setup of loop */ - fds[0].fd = _local_pos_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -505,8 +557,6 @@ MulticopterPositionControl::task_main() continue; } - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - poll_subscriptions(); parameters_update(false); @@ -516,8 +566,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -529,60 +579,35 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; + _vel(0) = _global_pos.vel_n; + _vel(1) = _global_pos.vel_e; + _vel(2) = _global_pos.vel_d; sp_move_rate.zero(); - if (_local_pos.ref_timestamp != ref_timestamp) { - /* initialize local projection with new reference */ - double lat_home = _local_pos.ref_lat * 1e-7; - double lon_home = _local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - - if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { - /* correct setpoint in manual mode to stay in the same point */ - float ref_change_x = 0.0f; - float ref_change_y = 0.0f; - map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); - _pos_sp(0) += ref_change_x; - _pos_sp(1) += ref_change_y; - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_timestamp = _local_pos.ref_timestamp; - ref_lat = _local_pos.ref_lat; - ref_lon = _local_pos.ref_lon; - ref_alt = _local_pos.ref_alt; - } + float alt = _global_pos.alt; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { + /* select altitude source and update setpoint */ + select_alt(_global_pos.global_valid); + + if (!_use_global_alt) { + alt = _global_pos.baro_alt; + } + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { - /* reset setpoint Z to current altitude if needed */ - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); - } + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); /* move altitude setpoint with throttle stick */ sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { - /* reset setpoint XY to current position if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); - } + /* reset lat/lon setpoint to current position if needed */ + reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); @@ -602,352 +627,406 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - _pos_sp += sp_move_rate * dt; + add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); + _alt_sp -= sp_move_rate(2) * dt; /* check if position setpoint is too far from actual position */ 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); + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); + pos_sp_offs(0) /= _params.sp_offs_max(0); + pos_sp_offs(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); + pos_sp_offs(2) = -(_alt_sp - alt) / _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); + add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); + } + + /* fill position setpoint triplet */ + _pos_sp_triplet.previous.valid = true; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = true; + + _pos_sp_triplet.nav_state = NAV_STATE_NONE; + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.lat = _lat_sp; + _pos_sp_triplet.current.lon = _lon_sp; + _pos_sp_triplet.current.alt = _alt_sp; + _pos_sp_triplet.current.yaw = _att_sp.yaw_body; + _pos_sp_triplet.current.loiter_radius = 0.0f; + _pos_sp_triplet.current.loiter_direction = 1.0f; + _pos_sp_triplet.current.pitch_min = 0.0f; + + /* publish position setpoint triplet */ + if (_pos_sp_triplet_pub > 0) { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); + + } else { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } else { + /* always use AMSL altitude for AUTO */ + select_alt(true); + /* AUTO */ - if (_pos_sp_triplet.current.valid) { - struct position_setpoint_s current_sp = _pos_sp_triplet.current; + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); - _pos_sp(2) = -(current_sp.alt - ref_alt); + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); + if (_pos_sp_triplet.current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_lat_lon_sp = true; + _reset_alt_sp = true; - if (isfinite(current_sp.yaw)) { - _att_sp.yaw_body = current_sp.yaw; - } + /* update position setpoint */ + _lat_sp = _pos_sp_triplet.current.lat; + _lon_sp = _pos_sp_triplet.current.lon; + _alt_sp = _pos_sp_triplet.current.alt; - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + /* 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 */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - } + reset_lat_lon_sp(); + reset_alt_sp(); } } - /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ - _local_pos_sp.yaw = _att_sp.yaw_body; - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + R.identity(); + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = _att.yaw; + _att_sp.thrust = 0.0f; - /* publish local position setpoint */ - if (_local_pos_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); - } + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err; + float err_x, err_y; + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); + pos_err(2) = -(_alt_sp - alt); - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); - if (!_control_mode.flag_control_altitude_enabled) { - reset_sp_z = true; - _vel_sp(2) = 0.0f; - } + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } - if (!_control_mode.flag_control_position_enabled) { - reset_sp_xy = true; - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } + if (!_control_mode.flag_control_position_enabled) { + _reset_lat_lon_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } - if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } - /* limit 3D speed only in AUTO mode */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + 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; + 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); + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); - /* publish velocity setpoint */ - if (_global_vel_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - } else { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); - } + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - /* reset integrals if needed */ - if (_control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = _params.thr_min; + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; - if (reset_int_z_manual) { - i = _manual.throttle; + if (reset_int_z_manual) { + i = _manual.throttle; - if (i < _params.thr_min) { - i = _params.thr_min; + if (i < _params.thr_min) { + i = _params.thr_min; - } else if (i > _params.thr_max) { - i = _params.thr_max; + } else if (i > _params.thr_max) { + i = _params.thr_max; + } } + + thrust_int(2) = -i; + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - thrust_int(2) = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } else { + reset_int_z = true; } - } else { - reset_int_z = true; - } + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); + } - if (_control_mode.flag_control_velocity_enabled) { - if (reset_int_xy) { - reset_int_xy = false; - thrust_int(0) = 0.0f; - thrust_int(1) = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } else { + reset_int_xy = true; } - } else { - reset_int_xy = true; - } + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; - /* velocity error */ - math::Vector<3> vel_err = _vel_sp - _vel; + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; - /* thrust vector in NED frame */ - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; - - if (!_control_mode.flag_control_velocity_enabled) { - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - } + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } - if (!_control_mode.flag_control_climb_rate_enabled) { - thrust_sp(2) = 0.0f; - } + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; - /* limit min lift */ - float thr_min = _params.thr_min; + /* limit min lift */ + float thr_min = _params.thr_min; - if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } - if (-thrust_sp(2) < thr_min) { - thrust_sp(2) = -thr_min; - saturation_z = true; - } + float tilt_max = _params.tilt_max; - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* adjust limits for landing mode */ + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) thr_min = 0.0f; } - } - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + /* limit min lift */ + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; } - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; - saturation_z = true; + if (_control_mode.flag_control_velocity_enabled) { + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } + } + } else { - att_comp = 1.0f; - saturation_z = true; - } + /* thrust compensation for altitude only control mode */ + float att_comp; - thrust_sp(2) *= att_comp; - } + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; - /* limit max thrust */ - float thrust_abs = thrust_sp.length(); + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; - if (thrust_abs > _params.thr_max) { - if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { - /* thrust Z component is too large, limit it */ - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; - saturation_xy = true; + } else { + att_comp = 1.0f; saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } } else { - /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); - float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp(0) *= k; - thrust_sp(1) *= k; + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; saturation_xy = true; + saturation_z = true; } - } else { - /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; - thrust_sp *= k; - saturation_xy = true; - saturation_z = true; + thrust_abs = _params.thr_max; } - thrust_abs = _params.thr_max; - } + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } - /* update integrals */ - if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { - thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; - thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; - } + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { - thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; - } + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; - /* calculate attitude setpoint from thrust vector */ - if (_control_mode.flag_control_velocity_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - math::Vector<3> body_x; - math::Vector<3> body_y; - math::Vector<3> body_z; + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; - if (thrust_abs > SIGMA) { - body_z = -thrust_sp / thrust_abs; + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } - } else { - /* no thrust, set Z axis to safe value */ - body_z.zero(); - body_z(2) = 1.0f; - } + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; - if (fabsf(body_z(2)) > SIGMA) { - /* desired body_x axis, orthogonal to body_z */ - body_x = y_C % body_z; + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); - /* keep nose to front while inverted upside down */ - if (body_z(2) < 0.0f) { - body_x = -body_x; + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; } - body_x.normalize(); + /* desired body_y axis */ + body_y = body_z % body_x; - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x.zero(); - body_x(2) = 1.0f; - } + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } - /* desired body_y axis */ - body_y = body_z % body_x; + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - R(i, 0) = body_x(i); - R(i, 1) = body_y(i); - R(i, 2) = body_z(i); + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - - /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = R.to_euler(); - _att_sp.roll_body = euler(0); - _att_sp.pitch_body = euler(1); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - } + _att_sp.thrust = thrust_abs; - _att_sp.thrust = thrust_abs; + _att_sp.timestamp = hrt_absolute_time(); - _att_sp.timestamp = hrt_absolute_time(); + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); - /* publish attitude setpoint */ - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + reset_int_z = true; } - - } else { - reset_int_z = true; } } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_alt_sp = true; + _reset_lat_lon_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -957,7 +1036,7 @@ MulticopterPositionControl::task_main() } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); + mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e6b7ef93d..5139ae6cd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include <sys/types.h> #include <sys/stat.h> +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -177,6 +178,7 @@ private: class Mission _mission; + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -188,6 +190,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + bool _pos_sp_triplet_updated; + char *nav_states_str[NAV_STATE_MAX]; struct { @@ -233,8 +237,7 @@ private: RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LAND + RTL_STATE_DESCEND }; enum RTLState _rtl_state; @@ -274,6 +277,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -296,6 +303,7 @@ private: void start_mission(); void start_rtl(); void start_land(); + void start_land_home(); /** * Guards offboard mission @@ -341,11 +349,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +376,7 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), @@ -381,7 +385,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -391,6 +394,7 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), @@ -399,6 +403,7 @@ Navigator::Navigator() : _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _pos_sp_triplet_updated(false), _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); @@ -541,9 +546,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +604,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _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)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +622,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +643,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +670,118 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + /* switch to RTL if not already landed after RTL and home position set */ + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + } - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + stick_mode = true; - stick_mode = true; + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - stick_mode = true; - } - - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } + dispatch(EVENT_LOITER_REQUESTED); } + + stick_mode = true; } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + break; - break; + case NAV_STATE_RTL: + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + break; - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); } - } - } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); - } + break; - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - dispatch(EVENT_RTL_REQUESTED); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -813,10 +822,22 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (check_mission_item_reached()) { - on_mission_item_reached(); + /* publish position setpoint triplet on each position update if navigator active */ + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + _pos_sp_triplet_updated = true; + + if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + /* got global position when landing, update setpoint */ + start_land(); + } + + _global_pos_valid = _global_pos.global_valid; + + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } } } @@ -836,16 +857,19 @@ Navigator::task_main() } } + /* publish position setpoint triplet if updated */ + if (_pos_sp_triplet_updated) { + _pos_sp_triplet_updated = false; + publish_position_setpoint_triplet(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); + /* reset time counter on state changes */ + _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -881,9 +905,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - if (_global_pos.valid) { + if (_global_pos.global_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); @@ -977,7 +1001,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, @@ -1006,26 +1030,29 @@ Navigator::start_none() _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void Navigator::start_ready() { _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state != RTL_STATE_LAND) { - /* allow RTL if landed not at home */ + if (_rtl_state != RTL_STATE_DESCEND) { + /* reset RTL state if landed not at home */ _rtl_state = RTL_STATE_NONE; } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1053,12 +1080,7 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - - if (_rtl_state == RTL_STATE_LAND) { - /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ - _rtl_state = RTL_STATE_DESCEND; - } + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1068,7 +1090,7 @@ Navigator::start_loiter() _pos_sp_triplet.next.valid = false; _mission_item_valid = false; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1095,6 +1117,9 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + /* reset time counter for new item */ + _time_first_inside_orbit = 0; + _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1181,7 +1206,7 @@ Navigator::set_mission_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1189,20 +1214,22 @@ Navigator::start_rtl() { _do_takeoff = false; + /* decide if we need climb */ if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; - - if (_reset_loiter_pos) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } } } + /* if switching directly to return state, reset altitude setpoint */ + if (_rtl_state == RTL_STATE_RETURN) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + _reset_loiter_pos = true; set_rtl_item(); } @@ -1210,25 +1237,70 @@ Navigator::start_rtl() void Navigator::start_land() { + /* this state can be requested by commander even if no global position available, + * in his case controller must perform landing without position control */ _do_takeoff = false; _reset_loiter_pos = true; - _pos_sp_triplet.previous.valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + _pos_sp_triplet.next.valid = false; +} - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = _global_pos.alt; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.yaw = NAN; +void +Navigator::start_land_home() +{ + /* land to home position, should be called when hovering above home, from RTL state */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; } void Navigator::set_rtl_item() { + /*reset time counter for new RTL item */ + _time_first_inside_orbit = 0; + switch (_rtl_state) { case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); @@ -1285,7 +1357,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1316,33 +1388,6 @@ Navigator::set_rtl_item() break; } - case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } - default: { mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); @@ -1350,7 +1395,7 @@ Navigator::set_rtl_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1400,14 +1445,7 @@ Navigator::check_mission_item_reached() } if (_mission_item.nav_cmd == NAV_CMD_LAND) { - if (_vstatus.is_rotary_wing) { - return _vstatus.condition_landed; - - } else { - /* For fw there is currently no landing detector: - * make sure control is not stopped when overshooting the landing waypoint */ - return false; - } + return _vstatus.condition_landed; } /* XXX TODO count turns */ @@ -1527,27 +1565,42 @@ Navigator::on_mission_item_reached() } } - } else { - /* RTL finished */ - if (_rtl_state == RTL_STATE_LAND) { - /* landed at home position */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); - dispatch(EVENT_READY_REQUESTED); + } else if (myState == NAV_STATE_RTL) { + /* RTL completed */ + if (_rtl_state == RTL_STATE_DESCEND) { + /* hovering above home position, land if needed or loiter */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + + if (_mission_item.autocontinue) { + dispatch(EVENT_LAND_REQUESTED); + + } else { + _reset_loiter_pos = false; + dispatch(EVENT_LOITER_REQUESTED); + } } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); set_rtl_item(); } + + } else if (myState == NAV_STATE_LAND) { + /* landing completed */ + mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); + dispatch(EVENT_READY_REQUESTED); } } void Navigator::publish_position_setpoint_triplet() { - /* lazily publish the mission triplet only once available */ + /* update navigation state */ + _pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState); + + /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the mission triplet */ + /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { @@ -1556,140 +1609,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast<nav_state_t>(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 6576aae70..e72caf98e 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -52,8 +52,8 @@ static const int ERROR = -1; -Mission::Mission() : - +Mission::Mission() : + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), @@ -65,7 +65,7 @@ Mission::Mission() : Mission::~Mission() { - + } void @@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool { /* try onboard mission first */ if (current_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } + _current_mission_type = MISSION_TYPE_ONBOARD; *onboard = true; *index = _current_onboard_mission_index; - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; } + _current_mission_type = MISSION_TYPE_OFFBOARD; *onboard = false; *index = _current_offboard_mission_index; @@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { /* try onboard mission first */ if (next_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; @@ -244,14 +254,16 @@ void Mission::move_to_next() { switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - case MISSION_TYPE_NONE: - default: - break; + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } }
\ No newline at end of file diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index af1d9d7d5..1ba159a8e 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,9 +55,9 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 000000000..6a1475c9b --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index af04bb0bc..bf4f7ae97 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - bool flag_armed = false; - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; + global_pos.baro_valid = true; } } } @@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - flag_armed = armed.armed; - baro_offset -= z_est[0]; - corr_baro = 0.0f; - local_pos.ref_alt -= z_est[0]; - local_pos.ref_timestamp = t; - z_est[0] = 0.0f; - alt_avg = 0.0f; - } } /* sensor combined */ @@ -539,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { gps_valid = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } @@ -601,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); - w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); + w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); } } else { @@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.02, dt), 0.005); t_prev = t; /* use GPS if it's valid and reference position initialized */ @@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; - baro_counter += offs_corr; + corr_baro += offs_corr; } /* accelerometer bias correction */ @@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.xy_global; + global_pos.global_valid = local_pos.xy_global; if (local_pos.xy_global) { double est_lat, est_lon; @@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.z_valid) { + global_pos.baro_alt = baro_offset - local_pos.z; + } + if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index e1bbd75a6..b71f9472f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5e2c92bf4..941500f0d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { + /* no channels */ + r_raw_rc_count = 0; + system_state.rc_channels_timestamp_received = 0; + system_state.rc_channels_timestamp_valid = 0; + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); @@ -97,35 +102,62 @@ controls_tick() { /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; +#ifdef ADC_RSSI + if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } + } +#endif + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - } - /* switch S.Bus output pin as needed */ - if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { - #ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); - #endif + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } perf_end(c_gather_sbus); @@ -136,13 +168,12 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { - /* XXX sample RSSI properly here */ - rssi = 255; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } perf_end(c_gather_ppm); @@ -150,6 +181,9 @@ controls_tick() { if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -161,97 +195,100 @@ controls_tick() { */ if (dsm_updated || sbus_updated || ppm_updated) { - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_received = hrt_absolute_time(); + + /* do not command anything in failsafe, kick in the RC loss counter */ + if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } } } - } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0 || rssi == 0) { - rc_input_lost = true; - } else { - /* set RC OK flag */ + /* set RC OK flag, as we got an update */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); + } } /* @@ -264,7 +301,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -272,24 +309,32 @@ controls_tick() { PX4IO_P_STATUS_FLAGS_RC_PPM | PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SBUS); + } /* * Handle losing RC input */ - if (rc_input_lost) { + /* this kicks in if the receiver is gone or the system went to failsafe */ + if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ + r_rc_valid = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + } - /* Mark the arrays as empty */ + /* this kicks in if the receiver is completely gone */ + if (rc_input_lost) { + + /* Set channel count to zero */ r_raw_rc_count = 0; - r_rc_valid = 0; } /* diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 2e79f0ac6..f39fcf7ec 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -223,14 +223,25 @@ mixer_tick(void) } } - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { + /* set arming */ + bool needs_to_arm = (should_arm || should_always_enable_pwm); + + /* check any conditions that prevent arming */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { + needs_to_arm = false; + } + if (!should_arm && !should_always_enable_pwm) { + needs_to_arm = false; + } + + if (needs_to_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; isr_debug(5, "> PWM enabled"); - } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { + } else if (!needs_to_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3..d48c6c529 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,7 +111,6 @@ #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_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #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 */ @@ -128,8 +127,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ -#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -140,7 +137,17 @@ /* array of raw RC input values, microseconds */ #define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ -#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ +#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 @@ -157,6 +164,10 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -166,6 +177,7 @@ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 393e0560e..bb224f388 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -96,8 +96,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] @@ -115,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ */ struct sys_state_s { - volatile uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp_received; + volatile uint64_t rc_channels_timestamp_valid; /** * Last FMU receive time, in microseconds since system boot @@ -215,7 +217,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2c437d2c0..1335f52e1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,8 +90,6 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_NRSSI] = 0, - [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; @@ -144,7 +148,12 @@ uint16_t r_page_scratch[32]; */ volatile uint16_t r_page_setup[] = { +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + /* default to RSSI ADC functionality */ + [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, +#else [PX4IO_P_SETUP_FEATURES] = 0, +#endif [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, @@ -162,14 +171,22 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; -#define PX4IO_P_SETUP_FEATURES_VALID (0) +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#else +#define PX4IO_P_SETUP_FEATURES_VALID 0 +#endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -438,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_FEATURES: value &= PX4IO_P_SETUP_FEATURES_VALID; - r_setup_features = value; - /* no implemented feature selection at this point */ + /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ + + /* switch S.Bus output pin as needed */ + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); + + /* disable the conflicting options */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } + #endif + + /* disable the conflicting options with ADC RSSI */ + if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ + if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* apply changes */ + r_setup_features = value; break; @@ -456,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * lockup of the IO arming state. */ - // XXX do not reset IO's safety state by FMU for now - // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - // } - if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 495447740..f6ec542eb 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_ * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, rssi, max_channels); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -289,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *rssi = 0; - return false; + *sbus_failsafe = true; + *sbus_frame_drop = true; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + /* set a special warning flag * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; } - *rssi = 255; - return true; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e..68e6a7469 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -62,7 +62,6 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -83,6 +82,7 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/esc_status.h> +#include <uORB/topics/telemetry_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -740,7 +740,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -760,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; + struct telemetry_status_s telemetry; } buf; memset(&buf, 0, sizeof(buf)); @@ -767,7 +767,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -786,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; + int telemetry_sub; } subs; /* log message buffer: header + body */ @@ -814,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; + struct log_TELE_s log_TELE; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -847,12 +848,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- VEHICLE CONTROL MODE --- */ - subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - fds[fdsc_count].fd = subs.control_mode_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -955,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- TELEMETRY STATUS --- */ + subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + fds[fdsc_count].fd = subs.telemetry_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1002,7 +1003,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; - /* poll all topics if logging enabled or only management (first 2) if not */ + /* poll all topics if logging enabled or only management (first 3) if not */ int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ @@ -1064,11 +1065,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { /* don't orb_copy, it's already done few lines above */ - /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ - orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; @@ -1078,7 +1076,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + /* don't orb_copy, it's already done few lines above */ log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; @@ -1094,8 +1092,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } - ifds++; // skip CONTROL MODE, already handled - /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1258,6 +1254,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); LOGBUFFER_WRITE_AND_COUNT(GPOS); } @@ -1265,6 +1263,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; @@ -1358,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- TELEMETRY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 98736dd21..16bfc355d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,7 +149,6 @@ struct log_ATTC_s { #define LOG_STAT_MSG 10 struct log_STAT_s { uint8_t main_state; - uint8_t navigation_state; uint8_t arming_state; float battery_remaining; uint8_t battery_warning; @@ -205,11 +204,14 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; + float baro_alt; + uint8_t flags; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { + uint8_t nav_state; int32_t lat; int32_t lon; float alt; @@ -262,6 +264,18 @@ struct log_DIST_s { uint8_t flags; }; +/* --- TELE - TELEMETRY STATUS --- */ +#define LOG_TELE_MSG 22 +struct log_TELE_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -297,18 +311,20 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ea864390d..b176d7417 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * 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 @@ -636,41 +635,43 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + const char *paramerr = "FAIL PARM LOAD"; + /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("Failed getting roll chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("Failed getting pitch chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("Failed getting yaw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("Failed getting throttle chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("Failed getting return sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx("Failed getting assisted sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx("Failed getting mission sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + warnx(paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -742,12 +743,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("Failed updating voltage scaling param"); + warnx(paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("Failed updating current scaling param"); + warnx(paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -1274,6 +1275,9 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + if (rc_input.rc_lost) + return; + struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1318,7 +1322,7 @@ Sensors::rc_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp_last_signal; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1367,9 +1371,9 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp; + _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp; + manual_control.timestamp = rc_input.timestamp_last_signal; /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); @@ -1647,17 +1651,17 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(0, "sensors task already running"); + errx(0, "already running"); sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) - errx(1, "sensors task alloc failed"); + errx(1, "alloc failed"); if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - err(1, "sensors task start failed"); + err(1, "start failed"); } exit(0); @@ -1665,7 +1669,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) - errx(1, "sensors task not running"); + errx(1, "not running"); delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1674,10 +1678,10 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - errx(0, "task is running"); + errx(0, "is running"); } else { - errx(1, "task is not running"); + errx(1, "not running"); } } diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index afc5b072c..ccc516f39 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -93,8 +93,7 @@ void cpuload_initialize_once() #endif /* CONFIG_SCHED_WORKQUEUE */ // perform static initialization of "system" threads - for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) - { + for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) { system_load.tasks[system_load.total_count].start_time = now; system_load.tasks[system_load.total_count].total_runtime = 0; system_load.tasks[system_load.total_count].curr_start_time = 0; diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h index c7aa18d3c..16d132fdb 100644 --- a/src/modules/systemlib/cpuload.h +++ b/src/modules/systemlib/cpuload.h @@ -40,15 +40,15 @@ __BEGIN_DECLS #include <nuttx/sched.h> struct system_load_taskinfo_s { - uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load - uint64_t curr_start_time; ///< Start time of the current scheduling slot - uint64_t start_time; ///< FIRST start time of task - FAR struct tcb_s *tcb; ///< - bool valid; ///< Task is currently active / valid + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct tcb_s *tcb; ///< + bool valid; ///< Task is currently active / valid }; struct system_load_s { - uint64_t start_time; ///< Global start time of measurements + uint64_t start_time; ///< Global start time of measurements struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; uint8_t initialized; int total_count; diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index a55ddf8a3..b05273c0d 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); if (fp == NULL) { - return 1; + warnx("file not found"); + return -1; } /* read valid lines from the file into a buffer */ @@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - return 1; + warnx("line too long"); + return -1; } /* add the line to the buffer */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4b57833b6..cf1ddfc38 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,6 +45,7 @@ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" +#include <navigator/navigator_state.h> /** * @addtogroup topics @@ -53,10 +54,11 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, - SETPOINT_TYPE_LOITER, - SETPOINT_TYPE_TAKEOFF, - SETPOINT_TYPE_LAND, + SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s @@ -82,6 +84,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + nav_state_t nav_state; /**< navigation state */ }; /** diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc..5192d4d58 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ 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 */ @@ -73,4 +73,4 @@ struct telemetry_status_s { ORB_DECLARE(telemetry_status); -#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file +#endif /* TOPIC_TELEMETRY_STATUS_H */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac898..7cbb37cd8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ae771ca00..ff9e98e1c 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -61,17 +61,21 @@ */ struct vehicle_global_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool global_valid; /**< true if position satisfies validity criteria of estimator */ + bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ + + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude AMSL in meters */ float vel_n; /**< Ground north velocity, m/s */ float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ + + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba..5d40554fd 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include <stdbool.h> #include "../uORB.h" +#include <navigator/navigator_state.h> + /** * @addtogroup topics @{ */ @@ -206,6 +208,7 @@ struct vehicle_status_s bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + bool rc_input_blocked; /**< set if RC input should be ignored */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; |