Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: add IMU temperature reporting

Browse Source
This commit is contained in:
Jochen Anglett 2018-09-21 08:22:04 +02:00 committed by Francisco Ferreira
parent fc5ce7b273
commit 1c60417d03
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -19,7 +19,10 @@
/* /*
FRSKY Telemetry library FRSKY Telemetry library
*/ */
#include "AP_Frsky_Telem.h" #include "AP_Frsky_Telem.h"
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <stdio.h> #include <stdio.h>
@ -683,6 +686,9 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
{ {
uint32_t ap_status; uint32_t ap_status;
// IMU temperature: offset -19, 0 means temp =< 19°, 63 means temp => 82°
uint8_t imu_temp = (uint8_t) roundf(constrain_float(AP::ins().get_temperature(0), AP_IMU_TEMP_MIN, AP_IMU_TEMP_MAX) - AP_IMU_TEMP_MIN);
// control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits) // control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits)
ap_status = (uint8_t)((_ap.control_mode+1) & AP_CONTROL_MODE_LIMIT); ap_status = (uint8_t)((_ap.control_mode+1) & AP_CONTROL_MODE_LIMIT);
// simple/super simple modes flags // simple/super simple modes flags
@ -695,6 +701,8 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery)<<AP_BATT_FS_OFFSET; ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery)<<AP_BATT_FS_OFFSET;
// bad ekf flag // bad ekf flag
ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET; ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
// IMU temperature
ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
return ap_status; return ap_status;
} }

3
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -95,6 +95,9 @@ for FrSky SPort Passthrough
#define AP_ARMED_OFFSET 8 #define AP_ARMED_OFFSET 8
#define AP_BATT_FS_OFFSET 9 #define AP_BATT_FS_OFFSET 9
#define AP_EKF_FS_OFFSET 10 #define AP_EKF_FS_OFFSET 10
#define AP_IMU_TEMP_MIN 19.0f
#define AP_IMU_TEMP_MAX 82.0f
#define AP_IMU_TEMP_OFFSET 26
// for home position related data // for home position related data
#define HOME_ALT_OFFSET 12 #define HOME_ALT_OFFSET 12
#define HOME_BEARING_LIMIT 0x7F #define HOME_BEARING_LIMIT 0x7F