diff options
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 13 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 17 |
2 files changed, 6 insertions, 24 deletions
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 060bff2b4..15ed70dbd 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -225,28 +225,20 @@ handle_message(mavlink_message_t *msg) uint8_t ml_mode = 0; bool ml_armed = false; -// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { -// ml_armed = true; -// } - switch (quad_motors_setpoint.mode) { case 0: ml_armed = false; - break; case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; ml_armed = true; - break; + break; case 2: - - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true; - break; + break; case 3: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; break; @@ -259,7 +251,6 @@ handle_message(mavlink_message_t *msg) offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; - //offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ; if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ ml_armed = false; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 39358922d..63a39e4ee 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -141,12 +141,6 @@ l_sensor_combined(struct listener *l) last_sensor_timestamp = raw.timestamp; - /* send raw imu data */ - // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0], - // raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0], - // raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0], - // raw.magnetometer_raw[1], raw.magnetometer_raw[2]); - /* mark individual fields as changed */ uint16_t fields_updated = 0; static unsigned accel_counter = 0; @@ -161,19 +155,19 @@ l_sensor_combined(struct listener *l) } if (gyro_counter != raw.gyro_counter) { - /* mark first three dimensions as changed */ + /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); gyro_counter = raw.gyro_counter; } if (mag_counter != raw.magnetometer_counter) { - /* mark first three dimensions as changed */ + /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); mag_counter = raw.magnetometer_counter; } if (baro_counter != raw.baro_counter) { - /* mark first three dimensions as changed */ + /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); baro_counter = raw.baro_counter; } @@ -187,8 +181,6 @@ l_sensor_combined(struct listener *l) raw.baro_pres_mbar, 0 /* no diff pressure yet */, raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); - /* send pressure */ - //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100); sensors_raw_counter++; } @@ -631,8 +623,7 @@ uorb_receive_start(void) pthread_attr_init(&uorb_attr); /* Set stack size, needs more than 8000 bytes */ - /* XXX verify, should not need anything like this much unless MAVLink really sucks */ - pthread_attr_setstacksize(&uorb_attr, 8192); + pthread_attr_setstacksize(&uorb_attr, 4096); pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); |