Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge branch 'master' into dev

Browse Source
This commit is contained in:
Felix Ruess
2011-08-27 09:40:20 +02:00
parent 3c46d03602 023d2b9ae4
commit eb7b6b531b
4 changed files with 62 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/stm32/subsystems/settings_arch.c
+1 -1
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/autopilot.c
+57 -24
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/navigation.c
+1 -1
View File
@@ -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"
sw/ground_segment/tmtc/gpsd2ivy.c
+3 -15
View File
@@ -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");