diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index e232d29471..38e1bcd7a8 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -117,7 +117,7 @@ static int32_t flash_detect(struct FlashInfo* flash) { case 0x00008000: /* 32 kBytes */ /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ case 0x00010000: /* 64 kBytes */ - case 0x00200000: /* 128 kBytes */ + case 0x00020000: /* 128 kBytes */ { flash->page_size = 0x400; break; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index ba7c3e1af4..b6963dac02 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -37,6 +37,7 @@ bool_t autopilot_motors_on; bool_t autopilot_in_flight; uint32_t autopilot_motors_on_counter; uint32_t autopilot_in_flight_counter; +uint8_t autopilot_check_motor_status; bool_t kill_throttle; bool_t autopilot_rc; @@ -51,6 +52,13 @@ uint16_t autopilot_flight_time; #define AUTOPILOT_IN_FLIGHT_TIME 40 #define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20) #define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20) +// Motors ON check state machine +#define STATUS_MOTORS_OFF 0 +#define STATUS_M_OFF_STICK_PUSHED 1 +#define STATUS_START_MOTORS 2 +#define STATUS_MOTORS_ON 3 +#define STATUS_M_ON_STICK_PUSHED 4 +#define STATUS_STOP_MOTORS 5 void autopilot_init(void) { autopilot_mode = AP_MODE_KILL; @@ -59,6 +67,7 @@ void autopilot_init(void) { kill_throttle = ! autopilot_motors_on; autopilot_motors_on_counter = 0; autopilot_in_flight_counter = 0; + autopilot_check_motor_status = STATUS_MOTORS_OFF; autopilot_mode_auto2 = MODE_AUTO2; autopilot_detect_ground = FALSE; autopilot_detect_ground_once = FALSE; @@ -180,6 +189,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { (radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \ radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD) + static inline void autopilot_check_in_flight( void) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { @@ -220,35 +230,58 @@ static inline int ahrs_is_aligned(void) { } #endif +/** + * State machine to check if motors should be turned ON or OFF + * The motors start/stop when pushing the yaw stick without throttle during a given time + * An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed + * The stick must return to a neutral position before starting/stoping again + */ static inline void autopilot_check_motors_on( void ) { - if (autopilot_motors_on) { - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { - if ( autopilot_motors_on_counter > 0) { - autopilot_motors_on_counter--; - if (autopilot_motors_on_counter == 0) - autopilot_motors_on = FALSE; - } - } - else { /* sticks not in the corner */ - autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; - } - } - else { /* motors off */ - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED() && ahrs_is_aligned()) { - if ( autopilot_motors_on_counter < AUTOPILOT_MOTOR_ON_TIME) { - autopilot_motors_on_counter++; - if (autopilot_motors_on_counter == AUTOPILOT_MOTOR_ON_TIME) - autopilot_motors_on = TRUE; - } - } - else { + switch(autopilot_check_motor_status) { + case STATUS_MOTORS_OFF: + autopilot_motors_on = FALSE; autopilot_motors_on_counter = 0; - } - } + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; + break; + case STATUS_M_OFF_STICK_PUSHED: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter++; + if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME) + autopilot_check_motor_status = STATUS_START_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + case STATUS_START_MOTORS: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_MOTORS_ON: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED; + break; + case STATUS_M_ON_STICK_PUSHED: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter--; + if (autopilot_motors_on_counter == 0) + autopilot_check_motor_status = STATUS_STOP_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_STOP_MOTORS: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter = 0; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + }; } - void autopilot_on_rc_frame(void) { uint8_t new_autopilot_mode = 0; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 46b8b081f7..5b85ffac51 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -280,7 +280,7 @@ void nav_periodic_task() { /* run carrot loop */ nav_run(); - ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.); + ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.); } #include "downlink.h" diff --git a/sw/ground_segment/tmtc/gpsd2ivy.c b/sw/ground_segment/tmtc/gpsd2ivy.c index 9c2882ba58..5f31381ed2 100644 --- a/sw/ground_segment/tmtc/gpsd2ivy.c +++ b/sw/ground_segment/tmtc/gpsd2ivy.c @@ -70,8 +70,7 @@ static struct gps_data_t *gpsdata; static void update_gps(struct gps_data_t *gpsdata, char *message, - size_t len, - int level) + size_t len) { static double fix_time = 0; double fix_track = 0; @@ -117,17 +116,7 @@ static void update_gps(struct gps_data_t *gpsdata, static gboolean gps_periodic(gpointer data __attribute__ ((unused))) { - struct timeval timeout; - int ret; - fd_set rfds; - - FD_ZERO(&rfds); - FD_SET(gpsdata->gps_fd, &rfds); - - timeout.tv_sec = 0; - timeout.tv_usec = 100000; - - ret = select(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &timeout); + int ret = gps_waiting(gpsdata); if (ret == -1) { @@ -150,8 +139,7 @@ int main(int argc, char *argv[]) gps_set_raw_hook(gpsdata, update_gps); - gps_query(gpsdata, "w+x\n"); - gps_query(gpsdata, "j=1\n"); + gps_stream(gpsdata, WATCH_ENABLE, NULL); IvyInit ("GPSd2Ivy", "GPSd2Ivy READY", NULL, NULL, NULL, NULL); IvyStart("127.255.255.255");