From 7a912a3fe47bced7c04d948cc3da094fea7542bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Aug 2012 18:47:46 +0200 Subject: Minor but important fixes across system --- apps/ardrone_control/ardrone_control.c | 44 ++++--- apps/ardrone_control/ardrone_motor_control.c | 20 +-- apps/ardrone_control/attitude_control.c | 45 ++++--- apps/commander/commander.c | 176 +++++++++++++++++---------- apps/commander/state_machine_helper.c | 73 ++++++----- apps/sensors/sensors.c | 29 +++-- apps/uORB/topics/vehicle_status.h | 3 + nuttx/configs/px4fmu/src/up_hrt.c | 5 + 8 files changed, 239 insertions(+), 156 deletions(-) diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index e49a0ae29..f80fdf971 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -166,6 +166,9 @@ int ardrone_control_main(int argc, char *argv[]) ar_init_motors(ardrone_write, &gpios); int counter = 0; + /* Led animation */ + int led_counter = 0; + /* pthread for position control */ pthread_t position_control_thread; position_control_thread_started = false; @@ -223,41 +226,46 @@ int ardrone_control_main(int argc, char *argv[]) } - //fancy led animation... - static int blubb = 0; - - if (counter % 20 == 0) { - if (blubb == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + if (counter % 30 == 0) { + if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); - if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); - if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); - if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); - if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); - if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); - if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); - if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); - if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); - if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); - if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); - if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); - blubb++; + led_counter++; - if (blubb == 12) blubb = 0; + if (led_counter == 12) led_counter = 0; } /* run at approximately 200 Hz */ usleep(5000); + + // This is a hardcore debug code piece to validate + // the motor interface + // uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; + // ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20); + // write(ardrone_write, motorSpeedBuf, 5); + // usleep(15000); + counter++; } diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c index 25847e1da..f13427eea 100644 --- a/apps/ardrone_control/ardrone_motor_control.c +++ b/apps/ardrone_control/ardrone_motor_control.c @@ -33,7 +33,8 @@ ****************************************************************************/ /* - * @file Implementation of AR.Drone 1.0 / 2.0 motor control interface + * @file ardrone_motor_control.c + * Implementation of AR.Drone 1.0 / 2.0 motor control interface */ @@ -97,23 +98,23 @@ int ar_multiplexing_init() { int fd; - fd = open("/dev/gpio", O_RDONLY | O_NONBLOCK); + fd = open(GPIO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); return fd; } + /* deactivate all outputs */ + int ret = 0; + ret += ioctl(fd, GPIO_SET, motor_gpios); + if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { printf("GPIO: output set fail\n"); close(fd); return -1; } - /* deactivate all outputs */ - int ret = 0; - ret += ioctl(fd, GPIO_SET, motor_gpios); - if (ret < 0) { printf("GPIO: clearing pins fail\n"); close(fd); @@ -167,12 +168,15 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { + /* deselect all */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); /* deselect all others */ - gpioset = motor_gpios ^ motor_gpio[motor - 1]; - ret += ioctl(fd, GPIO_SET, gpioset); + // gpioset = motor_gpios ^ motor_gpio[motor - 1]; + // ret += ioctl(fd, GPIO_SET, gpioset); } return ret; diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index 2d2596d6d..f96d22fe6 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -93,7 +93,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit static PID_t nick_controller; static PID_t roll_controller; - static const float min_gas = 1; + static const float min_gas = 10; static const float max_gas = 512; static uint16_t motor_pwm[4] = {0, 0, 0, 0}; static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -201,11 +201,11 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit // don't turn around the wrong side (only works if yaw angle is between +- 180 degree) if (yaw_e > M_PI) { - yaw_e -= 2.0f * M_PI; + yaw_e -= 2.0f * M_PI_F; } if (yaw_e < -M_PI) { - yaw_e += 2.0f * M_PI; + yaw_e += 2.0f * M_PI_F; } attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL); @@ -234,9 +234,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z; } else if (current_state == SYSTEM_STATE_MANUAL) { - attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f; - attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f; - attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265; + attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f; + attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f; + attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F; } /* add an attitude offset which needs to be estimated somewhere */ @@ -282,13 +282,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit // FLYING MODES if (current_state == SYSTEM_STATE_MANUAL) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; } else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO } else if (current_state == SYSTEM_STATE_EMCY_LANDING) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO } else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) { motor_thrust = 0; //immediately cut off thrust! @@ -297,10 +297,12 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit motor_thrust = 0; // Motor thrust must be zero in any other mode! } + if (status->rc_signal_lost) motor_thrust = 0; + // Convertion to motor-step units motor_thrust *= zcompensation; - motor_thrust *= max_gas / 20000.0f; //TODO: check this - motor_thrust += (max_gas - min_gas) / 2.f; + /* scale up from 0..1 to 10..512) */ + motor_thrust *= ((float)max_gas - min_gas); //limit control output //yawspeed @@ -418,10 +420,25 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit motor_pwm[2] = (uint16_t) motor_calc[2]; motor_pwm[3] = (uint16_t) motor_calc[3]; + // Keep motors spinning while armed + + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; + + /* Failsafe logic - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512; + motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512; + motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512; + motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; + //SEND MOTOR COMMANDS - uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; - ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - write(ardrone_write, motorSpeedBuf, 5); + if (motor_skip_counter % 5 == 0) { + uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; + ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + write(ardrone_write, motorSpeedBuf, 5); + } motor_skip_counter++; diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3236c2313..dc5647223 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -82,6 +82,7 @@ extern struct system_load_s system_load; #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define STICK_ON_OFF_LIMIT 7500 +#define STICK_THRUST_RANGE 20000 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) @@ -239,7 +240,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) struct sensor_combined_s raw; /* 30 seconds */ - const uint64_t calibration_interval_us = 10 * 1000000; + const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; const int peak_samples = 2000; @@ -266,6 +267,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status) mag_minima[1][i] = INT16_MAX; mag_minima[2][i] = INT16_MAX; } + + mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions."); uint64_t calibration_start = hrt_absolute_time(); while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) { @@ -455,35 +458,47 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta switch (cmd->command) { case MAV_CMD_DO_SET_MODE: { - update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1); + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1)) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_DENIED; + } + } + break; + + case MAV_CMD_COMPONENT_ARM_DISARM: + { + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = MAV_RESULT_ACCEPTED; + } else { + result = MAV_RESULT_DENIED; + } + } + } + break; + + /* request for an autopilot reboot */ + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + { + // if ((int)cmd->param1 == 1) { + // if (OK == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) { + // result = MAV_RESULT_ACCEPTED;//TODO: this has no effect + // } else { + // result = MAV_RESULT_DENIED; + // } + // } + } break; -// -// case MAV_CMD_COMPONENT_ARM_DISARM: -// { -// /* request to arm */ -// if (cmd->param1 == 1.0f) { -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) -// result = MAV_RESULT_ACCEPTED; -// /* request to disarm */ -// } else if (cmd->param1 == 0.0f) { -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_STANDBY)) -// result = MAV_RESULT_ACCEPTED; -// } -// } -// break; -// -// /* request for an autopilot reboot */ -// case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: -// { -// if (cmd->param1 == 1.0f) { -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) { -// result = MAV_RESULT_ACCEPTED;//TODO: this has no effect -// } -// } -// -// } -// break; // // /* request to land */ // case MAV_CMD_NAV_LAND: @@ -523,7 +538,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[commander] REJECTING gyro calibration"); + mavlink_log_critical(mavlink_fd, "[commander] REJECTING gyro calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -540,7 +555,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_state_update(status_pub, ¤t_status, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[commander] REJECTING mag calibration"); + mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration"); result = MAV_RESULT_DENIED; } handled = true; @@ -837,13 +852,15 @@ int commander_main(int argc, char *argv[]) memset(&sensors, 0, sizeof(sensors)); uint8_t vehicle_state_previous = current_status.state_machine; + float voltage_previous = 0.0f; uint64_t last_idle_time = 0; + unsigned int signal_loss_counter = 0; /* now initialized */ commander_initialized = true; - while (1) { //TODO: this while loop needs cleanup, split into sub-functions + while (1) { /* Get current values */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -859,7 +876,9 @@ int commander_main(int argc, char *argv[]) /* Slow but important 5 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO || current_status.state_machine == SYSTEM_STATE_MANUAL)) { + if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + current_status.state_machine == SYSTEM_STATE_AUTO || + current_status.state_machine == SYSTEM_STATE_MANUAL)) { /* armed */ led_toggle(LED_BLUE); @@ -888,7 +907,7 @@ int commander_main(int argc, char *argv[]) } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - //compute system load + /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) @@ -978,7 +997,6 @@ int commander_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ current_status.voltage_battery = battery_voltage; - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS @@ -1010,44 +1028,67 @@ int commander_main(int argc, char *argv[]) /* Start RC state check */ - int16_t chan3_scale = rc.chan[rc.function[YAW]].scale; - int16_t chan2_scale = rc.chan[rc.function[THROTTLE]].scale; + if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { - /* check if left stick is in lower left position --> switch to standby state */ - if (chan3_scale > STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status); - stick_on_counter = 0; + /* quadrotor specific logic - check against system type in the future */ - } else { - stick_off_counter++; - stick_on_counter = 0; + int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; + int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; + int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + + /* check if left stick is in lower left position --> switch to standby state */ + if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status); + stick_on_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } } - } - /* check if left stick is in lower right position --> arm */ - if (chan3_scale < -STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status); - stick_on_counter = 0; + /* check if left stick is in lower right position --> arm */ + if (rc_yaw_scale > STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status); + stick_on_counter = 0; - } else { - stick_on_counter++; - stick_off_counter = 0; + } else { + stick_on_counter++; + stick_off_counter = 0; + } } - } + //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + /* Check the value of the rc channel of the mode switch */ + mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { - update_state_machine_mode_manual(stat_pub, ¤t_status); + if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { + update_state_machine_mode_manual(stat_pub, ¤t_status); - } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { - update_state_machine_mode_auto(stat_pub, ¤t_status); + } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status); + + } else { + update_state_machine_mode_stabilized(stat_pub, ¤t_status); + } + + /* handle the case where RC signal was regained */ + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; } else { - update_state_machine_mode_stabilized(stat_pub, ¤t_status); + static uint64_t last_print_time = 0; + /* print error message for first RC glitch and then every 2 s / 2000 ms) */ + if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) { + mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost = true; + current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp; } /* End mode switch */ @@ -1057,12 +1098,19 @@ int commander_main(int argc, char *argv[]) current_status.counter++; current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); - - + if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + + /* If full run came back clean, transition to standby */ + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + current_status.preflight_gyro_calibration == false && + current_status.preflight_mag_calibration == false) { + /* All ok, no calibration going on, go to standby */ + do_state_update(stat_pub, ¤t_status, SYSTEM_STATE_STANDBY); + } /* Store old modes to detect and act on state transitions */ vehicle_state_previous = current_status.state_machine; + voltage_previous = current_status.voltage_battery; fflush(stdout); counter++; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a71bb797d..ebae1c61c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -417,6 +417,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status) { + // XXX CHANGE BACK if (current_status->state_machine == SYSTEM_STATE_STANDBY) { printf("[commander] arming\n"); do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); @@ -466,61 +467,55 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode) { - commander_state_machine_t current_system_state = current_status->state_machine; - printf("in update state request\n"); uint8_t ret = 1; - /* Switch on HIL if in standby */ - if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { - /* Enable HIL on request */ - current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; - ret = OK; - state_machine_publish(status_pub, current_status); - printf("[commander] Enabling HIL\n"); - } - - /* NEVER actually switch off HIL without reboot */ - if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { - fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); - ret = ERROR; - } + current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; + /* Set manual input enabled flag */ + current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - //TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c) - switch (mode) { - case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED - if (current_system_state == SYSTEM_STATE_STANDBY) { + /* vehicle is disarmed, mode requests arming */ + if (!(current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only arm in standby state */ + // XXX REMOVE + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { /* Set armed flag */ current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; /* Set manual input enabled flag */ current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); ret = OK; + printf("[commander] arming due to command request\n"); } + } - break; - - case MAV_MODE_MANUAL_DISARMED: - if (current_system_state == SYSTEM_STATE_GROUND_READY) { + /* vehicle is armed, mode requests disarming */ + //if ((current_status->mode & VEHICLE_MODE_FLAG_SAFETY_ARMED) && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only disarm in ground ready */ + //if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { /* Clear armed flag, leave manual input enabled */ - current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - } - - break; + // current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + // do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + // ret = OK; + // printf("[commander] disarming due to command request\n"); + //} + //} - default: - break; + /* Switch on HIL if in standby */ + if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { + /* Enable HIL on request */ + current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; + ret = OK; + state_machine_publish(status_pub, current_status); + printf("[commander] Enabling HIL\n"); } - #warning STATE MACHINE IS INCOMPLETE HERE - -// if(ret != 0) //Debug -// { -// strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected"); -// global_data_send_mavlink_message_out(&mavlink_statustext_msg_content); -// } + /* NEVER actually switch off HIL without reboot */ + if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { + fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + ret = ERROR; + } return ret; } diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 461b9805d..b456c4dfb 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -116,6 +116,7 @@ static void sensors_timer_loop(void *arg); #ifdef CONFIG_HRT_PPM extern uint16_t ppm_buffer[]; extern unsigned ppm_decoded_channels; +extern uint64_t ppm_last_valid_decode; #endif /* file handle that will be used for subscribing */ @@ -705,21 +706,23 @@ int sensors_main(int argc, char *argv[]) /* PPM */ if (ppmcounter == 5) { - /* Read out values from HRT */ - for (int i = 0; i < ppm_decoded_channels; i++) { - rc.chan[i].raw = ppm_buffer[i]; - /* Set the range to +-, then scale up */ - rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; - } - - rc.chan_count = ppm_decoded_channels; - - rc.timestamp = hrt_absolute_time(); - /* publish a few lines of code later if set to true */ - ppm_updated = true; + /* require at least two channels + * to consider the signal valid + */ + if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { + /* Read out values from HRT */ + for (int i = 0; i < ppm_decoded_channels; i++) { + rc.chan[i].raw = ppm_buffer[i]; + /* Set the range to +-, then scale up */ + rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; + } + rc.chan_count = ppm_decoded_channels; - //TODO: XXX check the mode switch channel and eventually send a request to the commander (see implementation in commander and mavlink) + rc.timestamp = ppm_last_valid_decode; + /* publish a few lines of code later if set to true */ + ppm_updated = true; + } ppmcounter = 0; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index d92b80046..933b58e8b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -110,6 +110,9 @@ struct vehicle_status_s bool preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool preflight_mag_calibration; /**< true if mag calibration is requested */ + bool rc_signal_lost; /**< true if no operator override channel is available */ + uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; diff --git a/nuttx/configs/px4fmu/src/up_hrt.c b/nuttx/configs/px4fmu/src/up_hrt.c index 35aecfe08..507358f8b 100644 --- a/nuttx/configs/px4fmu/src/up_hrt.c +++ b/nuttx/configs/px4fmu/src/up_hrt.c @@ -310,6 +310,7 @@ static void hrt_call_invoke(void); #define PPM_MAX_CHANNELS 12 uint16_t ppm_buffer[PPM_MAX_CHANNELS]; unsigned ppm_decoded_channels; +uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ uint16_t ppm_edge_history[32]; @@ -427,6 +428,8 @@ hrt_ppm_decode(uint32_t status) for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) ppm_buffer[i] = ppm_temp_buffer[i]; ppm_decoded_channels = i; + ppm_last_valid_decode = hrt_absolute_time(); + } /* reset for the next frame */ @@ -485,6 +488,8 @@ hrt_ppm_decode(uint32_t status) error: /* we don't like the state of the decoder, reset it and try again */ ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + } #endif /* CONFIG_HRT_PPM */ -- cgit v1.2.3