mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 08:13:56 -04:00
Sync with Arduplane
This commit is contained in:
parent
f5ffa7bd7e
commit
27d3564eab
@ -73,11 +73,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||||||
}
|
}
|
||||||
|
|
||||||
uint8_t status = MAV_STATE_ACTIVE;
|
uint8_t status = MAV_STATE_ACTIVE;
|
||||||
|
|
||||||
if (!motor_armed) {
|
if (!motor_armed) {
|
||||||
status = MAV_STATE_STANDBY;
|
status = MAV_STATE_STANDBY;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||||
|
|
||||||
mavlink_msg_sys_status_send(
|
mavlink_msg_sys_status_send(
|
||||||
@ -116,12 +116,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
|||||||
chan,
|
chan,
|
||||||
nav_roll / 1.0e2,
|
nav_roll / 1.0e2,
|
||||||
nav_pitch / 1.0e2,
|
nav_pitch / 1.0e2,
|
||||||
|
nav_bearing / 1.0e2,
|
||||||
target_bearing / 1.0e2,
|
target_bearing / 1.0e2,
|
||||||
dcm.yaw_sensor / 1.0e2, // was target_bearing
|
|
||||||
wp_distance,
|
wp_distance,
|
||||||
altitude_error / 1.0e2,
|
altitude_error / 1.0e2,
|
||||||
nav_lon, // was 0
|
0,
|
||||||
nav_lat); // was 0
|
crosstrack_error); // was 0
|
||||||
}
|
}
|
||||||
|
|
||||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||||
|
Loading…
Reference in New Issue
Block a user