Skip to content

Commit

Permalink
aerovironment-updates
Browse files Browse the repository at this point in the history
  • Loading branch information
bsutherland333 committed Nov 15, 2023
1 parent 1d20caa commit 1321f56
Show file tree
Hide file tree
Showing 12 changed files with 263 additions and 636 deletions.
44 changes: 24 additions & 20 deletions include/board.h
Original file line number Diff line number Diff line change
Expand Up @@ -70,45 +70,49 @@ class Board
virtual void sensors_init() = 0;
virtual uint16_t num_sensor_errors() = 0;

virtual bool new_imu_data() = 0;
// IMU
//virtual bool new_imu_data() = 0;
virtual uint8_t imu_has_new_data() = 0;
virtual bool imu_read(float accel[3], float *temperature, float gyro[3], uint64_t *time) = 0;
virtual void imu_not_responding_error() = 0;

// Mag
virtual bool mag_present() = 0;
virtual void mag_update() = 0;
virtual void mag_read(float mag[3]) = 0;
virtual uint8_t mag_has_new_data() = 0;
virtual bool mag_read(float mag[3]) = 0;

// Baro
virtual bool baro_present() = 0;
virtual void baro_update() = 0;
virtual void baro_read(float *pressure, float *temperature) = 0;
virtual uint8_t baro_has_new_data() = 0;
virtual bool baro_read(float *pressure, float *temperature) = 0;

// Pitot
virtual bool diff_pressure_present() = 0;
virtual void diff_pressure_update() = 0;
virtual void diff_pressure_read(float *diff_pressure, float *temperature) = 0;
virtual uint8_t diff_pressure_has_new_data() = 0;
virtual bool diff_pressure_read(float *diff_pressure, float *temperature) = 0;

// Sonar
virtual bool sonar_present() = 0;
virtual void sonar_update() = 0;
virtual float sonar_read() = 0;
virtual uint8_t sonar_has_new_data() = 0;
virtual bool sonar_read(float *range) =0;

// GPS
virtual bool gnss_present() = 0;
virtual void gnss_update() = 0;
virtual uint8_t gnss_has_new_data() = 0;
virtual bool gnss_read( GNSSData *gnss, GNSSFull *gnss_full) = 0;

virtual GNSSData gnss_read() = 0;
virtual bool gnss_has_new_data() = 0;
virtual GNSSFull gnss_full_read() = 0;

virtual bool battery_voltage_present() const = 0;
virtual float battery_voltage_read() const = 0;
// Battery
virtual bool battery_present() = 0;
virtual uint8_t battery_has_new_data() = 0;
virtual bool battery_read(float *voltage, float* current) = 0;
virtual void battery_voltage_set_multiplier(double multiplier) = 0;

virtual bool battery_current_present() const = 0;
virtual float battery_current_read() const = 0;
virtual void battery_current_set_multiplier(double multiplier) = 0;

// RC
virtual void rc_init(rc_type_t rc_type) = 0;
virtual bool rc_lost() = 0;
virtual float rc_read(uint8_t channel) = 0;
virtual uint8_t rc_has_new_data() = 0;
virtual float rc_read(uint8_t chan) = 0;

// PWM
virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0;
Expand Down
11 changes: 7 additions & 4 deletions include/comm_manager.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,11 @@
#ifndef ROSFLIGHT_FIRMWARE_COMM_MANAGER_H
#define ROSFLIGHT_FIRMWARE_COMM_MANAGER_H

#define OLD_STREAMING 0 // PTT don't include old streaming junk

#include "interface/comm_link.h"
#include "interface/param_listener.h"

#include "nanoprintf.h"

#include <cstdint>
#include <functional>

Expand Down Expand Up @@ -162,6 +162,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis

void send_next_param(void);

#if OLD_STREAMING //PTT not used anymore
Stream streams_[STREAM_COUNT] = {
Stream(0, [this] { this->send_heartbeat(); }), Stream(0, [this] { this->send_status(); }),
Stream(0, [this] { this->send_attitude(); }), Stream(0, [this] { this->send_imu(); }),
Expand All @@ -170,7 +171,7 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
Stream(0, [this] { this->send_battery_status(); }), Stream(0, [this] { this->send_output_raw(); }),
Stream(0, [this] { this->send_gnss(); }), Stream(0, [this] { this->send_gnss_full(); }),
Stream(0, [this] { this->send_rc_raw(); }), Stream(20000, [this] { this->send_low_priority(); })};

#endif
// the time of week stamp for the last sent GNSS message, to prevent re-sending
uint32_t last_sent_gnss_tow_ = 0;
uint32_t last_sent_gnss_full_tow_ = 0;
Expand All @@ -181,9 +182,11 @@ class CommManager : public CommLinkInterface::ListenerInterface, public ParamLis
void init();
void param_change_callback(uint16_t param_id) override;
void receive(void);
void stream();
void stream(got_flags got);
void send_param_value(uint16_t param_id);
#if OLD_STREAMING // PTT streaming rates are read only
void set_streaming_rate(uint8_t stream_id, int16_t param_id);
#endif
void update_status();
void log(CommLinkInterface::LogSeverity severity, const char* fmt, ...);

Expand Down
127 changes: 0 additions & 127 deletions include/nanoprintf.h

This file was deleted.

25 changes: 10 additions & 15 deletions include/param.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,21 +51,6 @@ enum : uint16_t
/*** MAVLINK CONFIGURATION ***/
/*****************************/
PARAM_SYSTEM_ID,
PARAM_STREAM_HEARTBEAT_RATE,
PARAM_STREAM_STATUS_RATE,

PARAM_STREAM_ATTITUDE_RATE,
PARAM_STREAM_IMU_RATE,
PARAM_STREAM_MAG_RATE,
PARAM_STREAM_BARO_RATE,
PARAM_STREAM_AIRSPEED_RATE,
PARAM_STREAM_SONAR_RATE,
PARAM_STREAM_GNSS_RATE,
PARAM_STREAM_GNSS_FULL_RATE,
PARAM_STREAM_BATTERY_STATUS_RATE,

PARAM_STREAM_OUTPUT_RAW_RATE,
PARAM_STREAM_RC_RAW_RATE,

/********************************/
/*** CONTROLLER CONFIGURATION ***/
Expand Down Expand Up @@ -378,4 +363,14 @@ class Params

} // namespace rosflight_firmware

#ifndef GIT_VERSION_HASH
#define GIT_VERSION_HASH 0x00
//#pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!"
#endif
#ifndef GIT_VERSION_STRING
#define GIT_VERSION_STRING "empty"
//#pragma message "GIT_VERSION_STRING Undefined, setting to \"empty\"!"
#endif


#endif // ROSFLIGHT_FIRMWARE_PARAM_H
2 changes: 1 addition & 1 deletion include/rc.h
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class RC : public ParamListenerInterface
uint32_t time_of_last_stick_deviation = 0;
uint32_t time_sticks_have_been_in_arming_position_ms = 0;
uint32_t prev_time_ms = 0;
uint32_t last_rc_receive_time = 0;
// uint32_t last_rc_receive_time = 0;

rc_stick_config_t sticks[STICKS_COUNT];
rc_switch_config_t switches[SWITCHES_COUNT];
Expand Down
24 changes: 20 additions & 4 deletions include/sensors.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,20 @@

namespace rosflight_firmware
{

typedef struct
{
uint8_t imu;
uint8_t gnss;
uint8_t gnss_full;
uint8_t baro;
uint8_t mag;
uint8_t diff_pressure;
uint8_t sonar;
uint8_t battery;
uint8_t rc;
} got_flags;

enum GNSSFixType
{
GNSS_FIX_TYPE_NO_FIX,
Expand Down Expand Up @@ -145,11 +159,11 @@ class Sensors : public ParamListenerInterface
bool sonar_range_valid = false;

GNSSData gnss_data;
bool gnss_new_data = false;
GNSSFull gnss_full;
// bool gnss_new_data = false;
float gps_CNO = 0; // What is this?
bool gnss_present = false;
GNSSFull gnss_full;


turbomath::Vector mag = {0, 0, 0};

bool baro_present = false;
Expand All @@ -169,7 +183,7 @@ class Sensors : public ParamListenerInterface

// function declarations
void init();
bool run();
got_flags run();
void param_change_callback(uint16_t param_id) override;

// Calibration Functions
Expand All @@ -187,6 +201,8 @@ class Sensors : public ParamListenerInterface
imu_data_sent_ = true;
return true;
}

got_flags got;

private:
static const float BARO_MAX_CHANGE_RATE;
Expand Down
Loading

0 comments on commit 1321f56

Please sign in to comment.